Stowage/godot
2
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2025-12-08 06:09:55 +00:00
Code Issues Projects Releases Packages Wiki Activity

Rename Godot Physics classes from *SW to Godot*

Browse source 
Also moved MT physics server wrappers to the main servers folder, since
they don't have to be implementation specific.
This commit is contained in:
PouleyKetchoupp 2021-10-18 12:24:30 -07:00
parent 5bb3dbbedd
commit cc39dca9f7
86 changed files with 5351 additions and 5390 deletions
Download patch file Download diff file Expand all files Collapse all files

2
scene/register_scene_types.cpp
View file

@ -940,10 +940,8 @@ void register_scene_types() {
ClassDB::add_compatibility_class("Path", "Path3D"); ClassDB::add_compatibility_class("Path", "Path3D");
ClassDB::add_compatibility_class("PathFollow", "PathFollow3D"); ClassDB::add_compatibility_class("PathFollow", "PathFollow3D");
ClassDB::add_compatibility_class("PhysicalBone", "PhysicalBone3D"); ClassDB::add_compatibility_class("PhysicalBone", "PhysicalBone3D");
ClassDB::add_compatibility_class("Physics2DDirectBodyStateSW", "PhysicsDirectBodyState2DSW");
ClassDB::add_compatibility_class("Physics2DDirectBodyState", "PhysicsDirectBodyState2D"); ClassDB::add_compatibility_class("Physics2DDirectBodyState", "PhysicsDirectBodyState2D");
ClassDB::add_compatibility_class("Physics2DDirectSpaceState", "PhysicsDirectSpaceState2D"); ClassDB::add_compatibility_class("Physics2DDirectSpaceState", "PhysicsDirectSpaceState2D");
ClassDB::add_compatibility_class("Physics2DServerSW", "PhysicsServer2DSW");
ClassDB::add_compatibility_class("Physics2DServer", "PhysicsServer2D"); ClassDB::add_compatibility_class("Physics2DServer", "PhysicsServer2D");
ClassDB::add_compatibility_class("Physics2DShapeQueryParameters", "PhysicsShapeQueryParameters2D"); ClassDB::add_compatibility_class("Physics2DShapeQueryParameters", "PhysicsShapeQueryParameters2D");
ClassDB::add_compatibility_class("Physics2DTestMotionResult", "PhysicsTestMotionResult2D"); ClassDB::add_compatibility_class("Physics2DTestMotionResult", "PhysicsTestMotionResult2D");

42
servers/physics_2d/area_2d_sw.cpp → servers/physics_2d/godot_area_2d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* area_2d_sw.cpp */ /* godot_area_2d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,31 +28,31 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "area_2d_sw.h" #include "godot_area_2d.h"
#include "body_2d_sw.h" #include "godot_body_2d.h"
#include "space_2d_sw.h" #include "godot_space_2d.h"
Area2DSW::BodyKey::BodyKey(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { GodotArea2D::BodyKey::BodyKey(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
rid = p_body->get_self(); rid = p_body->get_self();
instance_id = p_body->get_instance_id(); instance_id = p_body->get_instance_id();
body_shape = p_body_shape; body_shape = p_body_shape;
area_shape = p_area_shape; area_shape = p_area_shape;
} }
Area2DSW::BodyKey::BodyKey(Area2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { GodotArea2D::BodyKey::BodyKey(GodotArea2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
rid = p_body->get_self(); rid = p_body->get_self();
instance_id = p_body->get_instance_id(); instance_id = p_body->get_instance_id();
body_shape = p_body_shape; body_shape = p_body_shape;
area_shape = p_area_shape; area_shape = p_area_shape;
} }
void Area2DSW::_shapes_changed() { void GodotArea2D::_shapes_changed() {
if (!moved_list.in_list() && get_space()) { if (!moved_list.in_list() && get_space()) {
get_space()->area_add_to_moved_list(&moved_list); get_space()->area_add_to_moved_list(&moved_list);
} }
} }
void Area2DSW::set_transform(const Transform2D &p_transform) { void GodotArea2D::set_transform(const Transform2D &p_transform) {
if (!moved_list.in_list() && get_space()) { if (!moved_list.in_list() && get_space()) {
get_space()->area_add_to_moved_list(&moved_list); get_space()->area_add_to_moved_list(&moved_list);
} }
@ -61,7 +61,7 @@ void Area2DSW::set_transform(const Transform2D &p_transform) {
_set_inv_transform(p_transform.affine_inverse()); _set_inv_transform(p_transform.affine_inverse());
} }
void Area2DSW::set_space(Space2DSW *p_space) { void GodotArea2D::set_space(GodotSpace2D *p_space) {
if (get_space()) { if (get_space()) {
if (monitor_query_list.in_list()) { if (monitor_query_list.in_list()) {
get_space()->area_remove_from_monitor_query_list(&monitor_query_list); get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
@ -77,7 +77,7 @@ void Area2DSW::set_space(Space2DSW *p_space) {
_set_space(p_space); _set_space(p_space);
} }
void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) { void GodotArea2D::set_monitor_callback(ObjectID p_id, const StringName &p_method) {
if (p_id == monitor_callback_id) { if (p_id == monitor_callback_id) {
monitor_callback_method = p_method; monitor_callback_method = p_method;
return; return;
@ -98,7 +98,7 @@ void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) {
} }
} }
void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { void GodotArea2D::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) {
if (p_id == area_monitor_callback_id) { if (p_id == area_monitor_callback_id) {
area_monitor_callback_method = p_method; area_monitor_callback_method = p_method;
return; return;
@ -119,7 +119,7 @@ void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_meth
} }
} }
void Area2DSW::set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode) { void GodotArea2D::set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_mode) {
bool do_override = p_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; bool do_override = p_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED;
if (do_override == (space_override_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) { if (do_override == (space_override_mode != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED)) {
return; return;
@ -129,7 +129,7 @@ void Area2DSW::set_space_override_mode(PhysicsServer2D::AreaSpaceOverrideMode p_
_shape_changed(); _shape_changed();
} }
void Area2DSW::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value) { void GodotArea2D::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value) {
switch (p_param) { switch (p_param) {
case PhysicsServer2D::AREA_PARAM_GRAVITY: case PhysicsServer2D::AREA_PARAM_GRAVITY:
gravity = p_value; gravity = p_value;
@ -158,7 +158,7 @@ void Area2DSW::set_param(PhysicsServer2D::AreaParameter p_param, const Variant &
} }
} }
Variant Area2DSW::get_param(PhysicsServer2D::AreaParameter p_param) const { Variant GodotArea2D::get_param(PhysicsServer2D::AreaParameter p_param) const {
switch (p_param) { switch (p_param) {
case PhysicsServer2D::AREA_PARAM_GRAVITY: case PhysicsServer2D::AREA_PARAM_GRAVITY:
return gravity; return gravity;
@ -181,7 +181,7 @@ Variant Area2DSW::get_param(PhysicsServer2D::AreaParameter p_param) const {
return Variant(); return Variant();
} }
void Area2DSW::_queue_monitor_update() { void GodotArea2D::_queue_monitor_update() {
ERR_FAIL_COND(!get_space()); ERR_FAIL_COND(!get_space());
if (!monitor_query_list.in_list()) { if (!monitor_query_list.in_list()) {
@ -189,7 +189,7 @@ void Area2DSW::_queue_monitor_update() {
} }
} }
void Area2DSW::set_monitorable(bool p_monitorable) { void GodotArea2D::set_monitorable(bool p_monitorable) {
if (monitorable == p_monitorable) { if (monitorable == p_monitorable) {
return; return;
} }
@ -198,7 +198,7 @@ void Area2DSW::set_monitorable(bool p_monitorable) {
_set_static(!monitorable); _set_static(!monitorable);
} }
void Area2DSW::call_queries() { void GodotArea2D::call_queries() {
if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) {
Variant res[5]; Variant res[5];
Variant *resptr[5]; Variant *resptr[5];
@ -274,7 +274,7 @@ void Area2DSW::call_queries() {
} }
} }
void Area2DSW::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const { void GodotArea2D::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const {
if (is_gravity_point()) { if (is_gravity_point()) {
const real_t gravity_distance_scale = get_gravity_distance_scale(); const real_t gravity_distance_scale = get_gravity_distance_scale();
Vector2 v = get_transform().xform(get_gravity_vector()) - p_position; Vector2 v = get_transform().xform(get_gravity_vector()) - p_position;
@ -294,12 +294,12 @@ void Area2DSW::compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) co
} }
} }
Area2DSW::Area2DSW() : GodotArea2D::GodotArea2D() :
CollisionObject2DSW(TYPE_AREA), GodotCollisionObject2D(TYPE_AREA),
monitor_query_list(this), monitor_query_list(this),
moved_list(this) { moved_list(this) {
_set_static(true); //areas are not active by default _set_static(true); //areas are not active by default
} }
Area2DSW::~Area2DSW() { GodotArea2D::~GodotArea2D() {
} }

57
servers/physics_2d/area_2d_sw.h → servers/physics_2d/godot_area_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* area_2d_sw.h */ /* godot_area_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,18 +28,19 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef AREA_2D_SW_H #ifndef GODOT_AREA_2D_H
#define AREA_2D_SW_H #define GODOT_AREA_2D_H
#include "godot_collision_object_2d.h"
#include "collision_object_2d_sw.h"
#include "core/templates/self_list.h" #include "core/templates/self_list.h"
#include "servers/physics_server_2d.h" #include "servers/physics_server_2d.h"
class Space2DSW; class GodotSpace2D;
class Body2DSW; class GodotBody2D;
class Constraint2DSW; class GodotConstraint2D;
class Area2DSW : public CollisionObject2DSW { class GodotArea2D : public GodotCollisionObject2D {
PhysicsServer2D::AreaSpaceOverrideMode space_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED; PhysicsServer2D::AreaSpaceOverrideMode space_override_mode = PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED;
real_t gravity = 9.80665; real_t gravity = 9.80665;
Vector2 gravity_vector = Vector2(0, -1); Vector2 gravity_vector = Vector2(0, -1);
@ -57,8 +58,8 @@ class Area2DSW : public CollisionObject2DSW {
ObjectID area_monitor_callback_id; ObjectID area_monitor_callback_id;
StringName area_monitor_callback_method; StringName area_monitor_callback_method;
SelfList<Area2DSW> monitor_query_list; SelfList<GodotArea2D> monitor_query_list;
SelfList<Area2DSW> moved_list; SelfList<GodotArea2D> moved_list;
struct BodyKey { struct BodyKey {
RID rid; RID rid;
@ -79,8 +80,8 @@ class Area2DSW : public CollisionObject2DSW {
} }
_FORCE_INLINE_ BodyKey() {} _FORCE_INLINE_ BodyKey() {}
BodyKey(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); BodyKey(GodotBody2D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
BodyKey(Area2DSW *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 { struct BodyState {
@ -92,7 +93,7 @@ class Area2DSW : public CollisionObject2DSW {
Map<BodyKey, BodyState> monitored_bodies; Map<BodyKey, BodyState> monitored_bodies;
Map<BodyKey, BodyState> monitored_areas; Map<BodyKey, BodyState> monitored_areas;
Set<Constraint2DSW *> constraints; Set<GodotConstraint2D *> constraints;
virtual void _shapes_changed(); virtual void _shapes_changed();
void _queue_monitor_update(); void _queue_monitor_update();
@ -104,11 +105,11 @@ public:
void set_area_monitor_callback(ObjectID p_id, const StringName &p_method); void set_area_monitor_callback(ObjectID p_id, const StringName &p_method);
_FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); } _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); }
_FORCE_INLINE_ void add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); _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(Body2DSW *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(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_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(Area2DSW *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); void set_param(PhysicsServer2D::AreaParameter p_param, const Variant &p_value);
Variant get_param(PhysicsServer2D::AreaParameter p_param) const; Variant get_param(PhysicsServer2D::AreaParameter p_param) const;
@ -140,9 +141,9 @@ public:
_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
_FORCE_INLINE_ int get_priority() const { return priority; } _FORCE_INLINE_ int get_priority() const { return priority; }
_FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint) { constraints.insert(p_constraint); }
_FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint) { constraints.erase(p_constraint); } _FORCE_INLINE_ void remove_constraint(GodotConstraint2D *p_constraint) { constraints.erase(p_constraint); }
_FORCE_INLINE_ const Set<Constraint2DSW *> &get_constraints() const { return constraints; } _FORCE_INLINE_ const Set<GodotConstraint2D *> &get_constraints() const { return constraints; }
_FORCE_INLINE_ void clear_constraints() { constraints.clear(); } _FORCE_INLINE_ void clear_constraints() { constraints.clear(); }
void set_monitorable(bool p_monitorable); void set_monitorable(bool p_monitorable);
@ -150,17 +151,17 @@ public:
void set_transform(const Transform2D &p_transform); void set_transform(const Transform2D &p_transform);
void set_space(Space2DSW *p_space); void set_space(GodotSpace2D *p_space);
void call_queries(); void call_queries();
void compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const; void compute_gravity(const Vector2 &p_position, Vector2 &r_gravity) const;
Area2DSW(); GodotArea2D();
~Area2DSW(); ~GodotArea2D();
}; };
void Area2DSW::add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { 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); BodyKey bk(p_body, p_body_shape, p_area_shape);
monitored_bodies[bk].inc(); monitored_bodies[bk].inc();
if (!monitor_query_list.in_list()) { if (!monitor_query_list.in_list()) {
@ -168,7 +169,7 @@ void Area2DSW::add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape, uint32
} }
} }
void Area2DSW::remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { 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); BodyKey bk(p_body, p_body_shape, p_area_shape);
monitored_bodies[bk].dec(); monitored_bodies[bk].dec();
if (!monitor_query_list.in_list()) { if (!monitor_query_list.in_list()) {
@ -176,7 +177,7 @@ void Area2DSW::remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape, u
} }
} }
void Area2DSW::add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { 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); BodyKey bk(p_area, p_area_shape, p_self_shape);
monitored_areas[bk].inc(); monitored_areas[bk].inc();
if (!monitor_query_list.in_list()) { if (!monitor_query_list.in_list()) {
@ -184,7 +185,7 @@ void Area2DSW::add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape, uint32
} }
} }
void Area2DSW::remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { 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); BodyKey bk(p_area, p_area_shape, p_self_shape);
monitored_areas[bk].dec(); monitored_areas[bk].dec();
if (!monitor_query_list.in_list()) { if (!monitor_query_list.in_list()) {
@ -192,4 +193,4 @@ void Area2DSW::remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape, u
} }
} }
#endif // AREA_2D_SW_H #endif // GODOT_AREA_2D_H

30
servers/physics_2d/area_pair_2d_sw.cpp → servers/physics_2d/godot_area_pair_2d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* area_pair_2d_sw.cpp */ /* godot_area_pair_2d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,12 +28,12 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "area_pair_2d_sw.h" #include "godot_area_pair_2d.h"
#include "collision_solver_2d_sw.h" #include "godot_collision_solver_2d.h"
bool AreaPair2DSW::setup(real_t p_step) { bool GodotAreaPair2D::setup(real_t p_step) {
bool result = false; bool result = false;
if (area->collides_with(body) && CollisionSolver2DSW::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)) { 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; result = true;
} }
@ -51,7 +51,7 @@ bool AreaPair2DSW::setup(real_t p_step) {
return process_collision; return process_collision;
} }
bool AreaPair2DSW::pre_solve(real_t p_step) { bool GodotAreaPair2D::pre_solve(real_t p_step) {
if (!process_collision) { if (!process_collision) {
return false; return false;
} }
@ -77,11 +77,11 @@ bool AreaPair2DSW::pre_solve(real_t p_step) {
return false; // Never do any post solving. return false; // Never do any post solving.
} }
void AreaPair2DSW::solve(real_t p_step) { void GodotAreaPair2D::solve(real_t p_step) {
// Nothing to do. // Nothing to do.
} }
AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape) { GodotAreaPair2D::GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotArea2D *p_area, int p_area_shape) {
body = p_body; body = p_body;
area = p_area; area = p_area;
body_shape = p_body_shape; body_shape = p_body_shape;
@ -93,7 +93,7 @@ AreaPair2DSW::AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area,
} }
} }
AreaPair2DSW::~AreaPair2DSW() { GodotAreaPair2D::~GodotAreaPair2D() {
if (colliding) { if (colliding) {
if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) { if (area->get_space_override_mode() != PhysicsServer2D::AREA_SPACE_OVERRIDE_DISABLED) {
body->remove_area(area); body->remove_area(area);
@ -108,10 +108,10 @@ AreaPair2DSW::~AreaPair2DSW() {
////////////////////////////////// //////////////////////////////////
bool Area2Pair2DSW::setup(real_t p_step) { bool GodotArea2Pair2D::setup(real_t p_step) {
bool result_a = area_a->collides_with(area_b); bool result_a = area_a->collides_with(area_b);
bool result_b = area_b->collides_with(area_a); bool result_b = area_b->collides_with(area_a);
if ((result_a || result_b) && !CollisionSolver2DSW::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)) { 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_a = false;
result_b = false; result_b = false;
} }
@ -139,7 +139,7 @@ bool Area2Pair2DSW::setup(real_t p_step) {
return process_collision; return process_collision;
} }
bool Area2Pair2DSW::pre_solve(real_t p_step) { bool GodotArea2Pair2D::pre_solve(real_t p_step) {
if (process_collision_a) { if (process_collision_a) {
if (colliding_a) { if (colliding_a) {
area_a->add_area_to_query(area_b, shape_b, shape_a); area_a->add_area_to_query(area_b, shape_b, shape_a);
@ -159,11 +159,11 @@ bool Area2Pair2DSW::pre_solve(real_t p_step) {
return false; // Never do any post solving. return false; // Never do any post solving.
} }
void Area2Pair2DSW::solve(real_t p_step) { void GodotArea2Pair2D::solve(real_t p_step) {
// Nothing to do. // Nothing to do.
} }
Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b) { GodotArea2Pair2D::GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotArea2D *p_area_b, int p_shape_b) {
area_a = p_area_a; area_a = p_area_a;
area_b = p_area_b; area_b = p_area_b;
shape_a = p_shape_a; shape_a = p_shape_a;
@ -172,7 +172,7 @@ Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area
area_b->add_constraint(this); area_b->add_constraint(this);
} }
Area2Pair2DSW::~Area2Pair2DSW() { GodotArea2Pair2D::~GodotArea2Pair2D() {
if (colliding_a) { if (colliding_a) {
if (area_a->has_area_monitor_callback()) { if (area_a->has_area_monitor_callback()) {
area_a->remove_area_from_query(area_b, shape_b, shape_a); area_a->remove_area_from_query(area_b, shape_b, shape_a);

34
servers/physics_2d/area_pair_2d_sw.h → servers/physics_2d/godot_area_pair_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* area_pair_2d_sw.h */ /* godot_area_pair_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,16 +28,16 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef AREA_PAIR_2D_SW_H #ifndef GODOT_AREA_PAIR_2D_H
#define AREA_PAIR_2D_SW_H #define GODOT_AREA_PAIR_2D_H
#include "area_2d_sw.h" #include "godot_area_2d.h"
#include "body_2d_sw.h" #include "godot_body_2d.h"
#include "constraint_2d_sw.h" #include "godot_constraint_2d.h"
class AreaPair2DSW : public Constraint2DSW { class GodotAreaPair2D : public GodotConstraint2D {
Body2DSW *body = nullptr; GodotBody2D *body = nullptr;
Area2DSW *area = nullptr; GodotArea2D *area = nullptr;
int body_shape = 0; int body_shape = 0;
int area_shape = 0; int area_shape = 0;
bool colliding = false; bool colliding = false;
@ -48,13 +48,13 @@ public:
virtual bool pre_solve(real_t p_step) override; virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override; virtual void solve(real_t p_step) override;
AreaPair2DSW(Body2DSW *p_body, int p_body_shape, Area2DSW *p_area, int p_area_shape); GodotAreaPair2D(GodotBody2D *p_body, int p_body_shape, GodotArea2D *p_area, int p_area_shape);
~AreaPair2DSW(); ~GodotAreaPair2D();
}; };
class Area2Pair2DSW : public Constraint2DSW { class GodotArea2Pair2D : public GodotConstraint2D {
Area2DSW *area_a = nullptr; GodotArea2D *area_a = nullptr;
Area2DSW *area_b = nullptr; GodotArea2D *area_b = nullptr;
int shape_a = 0; int shape_a = 0;
int shape_b = 0; int shape_b = 0;
bool colliding_a = false; bool colliding_a = false;
@ -67,8 +67,8 @@ public:
virtual bool pre_solve(real_t p_step) override; virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override; virtual void solve(real_t p_step) override;
Area2Pair2DSW(Area2DSW *p_area_a, int p_shape_a, Area2DSW *p_area_b, int p_shape_b); GodotArea2Pair2D(GodotArea2D *p_area_a, int p_shape_a, GodotArea2D *p_area_b, int p_shape_b);
~Area2Pair2DSW(); ~GodotArea2Pair2D();
}; };
#endif // AREA_PAIR_2D_SW_H #endif // GODOT_AREA_PAIR_2D_H

74
servers/physics_2d/body_2d_sw.cpp → servers/physics_2d/godot_body_2d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* body_2d_sw.cpp */ /* godot_body_2d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,19 +28,19 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "body_2d_sw.h" #include "godot_body_2d.h"
#include "area_2d_sw.h" #include "godot_area_2d.h"
#include "body_direct_state_2d_sw.h" #include "godot_body_direct_state_2d.h"
#include "space_2d_sw.h" #include "godot_space_2d.h"
void Body2DSW::_mass_properties_changed() { void GodotBody2D::_mass_properties_changed() {
if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) { if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) {
get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list); get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list);
} }
} }
void Body2DSW::update_mass_properties() { void GodotBody2D::update_mass_properties() {
//update shapes and motions //update shapes and motions
switch (mode) { switch (mode) {
@ -83,7 +83,7 @@ void Body2DSW::update_mass_properties() {
continue; continue;
} }
const Shape2DSW *shape = get_shape(i); const GodotShape2D *shape = get_shape(i);
real_t area = get_shape_aabb(i).get_area(); real_t area = get_shape_aabb(i).get_area();
if (area == 0.0) { if (area == 0.0) {
@ -121,13 +121,13 @@ void Body2DSW::update_mass_properties() {
} }
} }
void Body2DSW::reset_mass_properties() { void GodotBody2D::reset_mass_properties() {
calculate_inertia = true; calculate_inertia = true;
calculate_center_of_mass = true; calculate_center_of_mass = true;
_mass_properties_changed(); _mass_properties_changed();
} }
void Body2DSW::set_active(bool p_active) { void GodotBody2D::set_active(bool p_active) {
if (active == p_active) { if (active == p_active) {
return; return;
} }
@ -146,7 +146,7 @@ void Body2DSW::set_active(bool p_active) {
} }
} }
void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) { void GodotBody2D::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &p_value) {
switch (p_param) { switch (p_param) {
case PhysicsServer2D::BODY_PARAM_BOUNCE: { case PhysicsServer2D::BODY_PARAM_BOUNCE: {
bounce = p_value; bounce = p_value;
@ -195,7 +195,7 @@ void Body2DSW::set_param(PhysicsServer2D::BodyParameter p_param, const Variant &
} }
} }
Variant Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const { Variant GodotBody2D::get_param(PhysicsServer2D::BodyParameter p_param) const {
switch (p_param) { switch (p_param) {
case PhysicsServer2D::BODY_PARAM_BOUNCE: { case PhysicsServer2D::BODY_PARAM_BOUNCE: {
return bounce; return bounce;
@ -228,7 +228,7 @@ Variant Body2DSW::get_param(PhysicsServer2D::BodyParameter p_param) const {
return 0; return 0;
} }
void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) { void GodotBody2D::set_mode(PhysicsServer2D::BodyMode p_mode) {
PhysicsServer2D::BodyMode prev = mode; PhysicsServer2D::BodyMode prev = mode;
mode = p_mode; mode = p_mode;
@ -267,16 +267,16 @@ void Body2DSW::set_mode(PhysicsServer2D::BodyMode p_mode) {
} }
} }
PhysicsServer2D::BodyMode Body2DSW::get_mode() const { PhysicsServer2D::BodyMode GodotBody2D::get_mode() const {
return mode; return mode;
} }
void Body2DSW::_shapes_changed() { void GodotBody2D::_shapes_changed() {
_mass_properties_changed(); _mass_properties_changed();
wakeup_neighbours(); wakeup_neighbours();
} }
void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant) { void GodotBody2D::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_variant) {
switch (p_state) { switch (p_state) {
case PhysicsServer2D::BODY_STATE_TRANSFORM: { case PhysicsServer2D::BODY_STATE_TRANSFORM: {
if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { if (mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
@ -344,7 +344,7 @@ void Body2DSW::set_state(PhysicsServer2D::BodyState p_state, const Variant &p_va
} }
} }
Variant Body2DSW::get_state(PhysicsServer2D::BodyState p_state) const { Variant GodotBody2D::get_state(PhysicsServer2D::BodyState p_state) const {
switch (p_state) { switch (p_state) {
case PhysicsServer2D::BODY_STATE_TRANSFORM: { case PhysicsServer2D::BODY_STATE_TRANSFORM: {
return get_transform(); return get_transform();
@ -366,7 +366,7 @@ Variant Body2DSW::get_state(PhysicsServer2D::BodyState p_state) const {
return Variant(); return Variant();
} }
void Body2DSW::set_space(Space2DSW *p_space) { void GodotBody2D::set_space(GodotSpace2D *p_space) {
if (get_space()) { if (get_space()) {
wakeup_neighbours(); wakeup_neighbours();
@ -391,7 +391,7 @@ void Body2DSW::set_space(Space2DSW *p_space) {
} }
} }
void Body2DSW::_compute_area_gravity_and_damping(const Area2DSW *p_area) { void GodotBody2D::_compute_area_gravity_and_damping(const GodotArea2D *p_area) {
Vector2 area_gravity; Vector2 area_gravity;
p_area->compute_gravity(get_transform().get_origin(), area_gravity); p_area->compute_gravity(get_transform().get_origin(), area_gravity);
gravity += area_gravity; gravity += area_gravity;
@ -400,13 +400,13 @@ void Body2DSW::_compute_area_gravity_and_damping(const Area2DSW *p_area) {
area_angular_damp += p_area->get_angular_damp(); area_angular_damp += p_area->get_angular_damp();
} }
void Body2DSW::integrate_forces(real_t p_step) { void GodotBody2D::integrate_forces(real_t p_step) {
if (mode == PhysicsServer2D::BODY_MODE_STATIC) { if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
return; return;
} }
Area2DSW *def_area = get_space()->get_default_area(); GodotArea2D *def_area = get_space()->get_default_area();
// Area2DSW *damp_area = def_area; // GodotArea2D *damp_area = def_area;
ERR_FAIL_COND(!def_area); ERR_FAIL_COND(!def_area);
int ac = areas.size(); int ac = areas.size();
@ -528,7 +528,7 @@ void Body2DSW::integrate_forces(real_t p_step) {
contact_count = 0; contact_count = 0;
} }
void Body2DSW::integrate_velocities(real_t p_step) { void GodotBody2D::integrate_velocities(real_t p_step) {
if (mode == PhysicsServer2D::BODY_MODE_STATIC) { if (mode == PhysicsServer2D::BODY_MODE_STATIC) {
return; return;
} }
@ -570,17 +570,17 @@ void Body2DSW::integrate_velocities(real_t p_step) {
} }
} }
void Body2DSW::wakeup_neighbours() { void GodotBody2D::wakeup_neighbours() {
for (const Pair<Constraint2DSW *, int> &E : constraint_list) { for (const Pair<GodotConstraint2D *, int> &E : constraint_list) {
const Constraint2DSW *c = E.first; const GodotConstraint2D *c = E.first;
Body2DSW **n = c->get_body_ptr(); GodotBody2D **n = c->get_body_ptr();
int bc = c->get_body_count(); int bc = c->get_body_count();
for (int i = 0; i < bc; i++) { for (int i = 0; i < bc; i++) {
if (i == E.second) { if (i == E.second) {
continue; continue;
} }
Body2DSW *b = n[i]; GodotBody2D *b = n[i];
if (b->mode < PhysicsServer2D::BODY_MODE_DYNAMIC) { if (b->mode < PhysicsServer2D::BODY_MODE_DYNAMIC) {
continue; continue;
} }
@ -592,7 +592,7 @@ void Body2DSW::wakeup_neighbours() {
} }
} }
void Body2DSW::call_queries() { void GodotBody2D::call_queries() {
if (fi_callback_data) { if (fi_callback_data) {
if (!fi_callback_data->callable.get_object()) { if (!fi_callback_data->callable.get_object()) {
set_force_integration_callback(Callable()); set_force_integration_callback(Callable());
@ -616,7 +616,7 @@ void Body2DSW::call_queries() {
} }
} }
bool Body2DSW::sleep_test(real_t p_step) { bool GodotBody2D::sleep_test(real_t p_step) {
if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) { if (mode == PhysicsServer2D::BODY_MODE_STATIC || mode == PhysicsServer2D::BODY_MODE_KINEMATIC) {
return true; return true;
} else if (!can_sleep) { } else if (!can_sleep) {
@ -633,12 +633,12 @@ bool Body2DSW::sleep_test(real_t p_step) {
} }
} }
void Body2DSW::set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback) { void GodotBody2D::set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback) {
body_state_callback_instance = p_instance; body_state_callback_instance = p_instance;
body_state_callback = p_callback; body_state_callback = p_callback;
} }
void Body2DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { void GodotBody2D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
if (p_callable.get_object()) { if (p_callable.get_object()) {
if (!fi_callback_data) { if (!fi_callback_data) {
fi_callback_data = memnew(ForceIntegrationCallbackData); fi_callback_data = memnew(ForceIntegrationCallbackData);
@ -651,23 +651,23 @@ void Body2DSW::set_force_integration_callback(const Callable &p_callable, const
} }
} }
PhysicsDirectBodyState2DSW *Body2DSW::get_direct_state() { GodotPhysicsDirectBodyState2D *GodotBody2D::get_direct_state() {
if (!direct_state) { if (!direct_state) {
direct_state = memnew(PhysicsDirectBodyState2DSW); direct_state = memnew(GodotPhysicsDirectBodyState2D);
direct_state->body = this; direct_state->body = this;
} }
return direct_state; return direct_state;
} }
Body2DSW::Body2DSW() : GodotBody2D::GodotBody2D() :
CollisionObject2DSW(TYPE_BODY), GodotCollisionObject2D(TYPE_BODY),
active_list(this), active_list(this),
mass_properties_update_list(this), mass_properties_update_list(this),
direct_state_query_list(this) { direct_state_query_list(this) {
_set_static(false); _set_static(false);
} }
Body2DSW::~Body2DSW() { GodotBody2D::~GodotBody2D() {
if (fi_callback_data) { if (fi_callback_data) {
memdelete(fi_callback_data); memdelete(fi_callback_data);
} }

57
servers/physics_2d/body_2d_sw.h → servers/physics_2d/godot_body_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* body_2d_sw.h */ /* godot_body_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,19 +28,20 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef BODY_2D_SW_H #ifndef GODOT_BODY_2D_H
#define BODY_2D_SW_H #define GODOT_BODY_2D_H
#include "godot_area_2d.h"
#include "godot_collision_object_2d.h"
#include "area_2d_sw.h"
#include "collision_object_2d_sw.h"
#include "core/templates/list.h" #include "core/templates/list.h"
#include "core/templates/pair.h" #include "core/templates/pair.h"
#include "core/templates/vset.h" #include "core/templates/vset.h"
class Constraint2DSW; class GodotConstraint2D;
class PhysicsDirectBodyState2DSW; class GodotPhysicsDirectBodyState2D;
class Body2DSW : public CollisionObject2DSW { class GodotBody2D : public GodotCollisionObject2D {
PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_DYNAMIC; PhysicsServer2D::BodyMode mode = PhysicsServer2D::BODY_MODE_DYNAMIC;
Vector2 biased_linear_velocity; Vector2 biased_linear_velocity;
@ -79,9 +80,9 @@ class Body2DSW : public CollisionObject2DSW {
Vector2 applied_force; Vector2 applied_force;
real_t applied_torque = 0.0; real_t applied_torque = 0.0;
SelfList<Body2DSW> active_list; SelfList<GodotBody2D> active_list;
SelfList<Body2DSW> mass_properties_update_list; SelfList<GodotBody2D> mass_properties_update_list;
SelfList<Body2DSW> direct_state_query_list; SelfList<GodotBody2D> direct_state_query_list;
VSet<RID> exceptions; VSet<RID> exceptions;
PhysicsServer2D::CCDMode continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED; PhysicsServer2D::CCDMode continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED;
@ -93,15 +94,15 @@ class Body2DSW : public CollisionObject2DSW {
virtual void _shapes_changed(); virtual void _shapes_changed();
Transform2D new_transform; Transform2D new_transform;
List<Pair<Constraint2DSW *, int>> constraint_list; List<Pair<GodotConstraint2D *, int>> constraint_list;
struct AreaCMP { struct AreaCMP {
Area2DSW *area = nullptr; GodotArea2D *area = nullptr;
int refCount = 0; 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_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_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); }
_FORCE_INLINE_ AreaCMP() {} _FORCE_INLINE_ AreaCMP() {}
_FORCE_INLINE_ AreaCMP(Area2DSW *p_area) { _FORCE_INLINE_ AreaCMP(GodotArea2D *p_area) {
area = p_area; area = p_area;
refCount = 1; refCount = 1;
} }
@ -134,21 +135,21 @@ class Body2DSW : public CollisionObject2DSW {
ForceIntegrationCallbackData *fi_callback_data = nullptr; ForceIntegrationCallbackData *fi_callback_data = nullptr;
PhysicsDirectBodyState2DSW *direct_state = nullptr; GodotPhysicsDirectBodyState2D *direct_state = nullptr;
uint64_t island_step = 0; uint64_t island_step = 0;
_FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area2DSW *p_area); _FORCE_INLINE_ void _compute_area_gravity_and_damping(const GodotArea2D *p_area);
friend class PhysicsDirectBodyState2DSW; // i give up, too many functions to expose friend class GodotPhysicsDirectBodyState2D; // i give up, too many functions to expose
public: public:
void set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback); void set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback);
void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant());
PhysicsDirectBodyState2DSW *get_direct_state(); GodotPhysicsDirectBodyState2D *get_direct_state();
_FORCE_INLINE_ void add_area(Area2DSW *p_area) { _FORCE_INLINE_ void add_area(GodotArea2D *p_area) {
int index = areas.find(AreaCMP(p_area)); int index = areas.find(AreaCMP(p_area));
if (index > -1) { if (index > -1) {
areas.write[index].refCount += 1; areas.write[index].refCount += 1;
@ -157,7 +158,7 @@ public:
} }
} }
_FORCE_INLINE_ void remove_area(Area2DSW *p_area) { _FORCE_INLINE_ void remove_area(GodotArea2D *p_area) {
int index = areas.find(AreaCMP(p_area)); int index = areas.find(AreaCMP(p_area));
if (index > -1) { if (index > -1) {
areas.write[index].refCount -= 1; areas.write[index].refCount -= 1;
@ -188,9 +189,9 @@ public:
_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _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 set_island_step(uint64_t p_step) { island_step = p_step; }
_FORCE_INLINE_ void add_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_list.push_back({ p_constraint, p_pos }); } _FORCE_INLINE_ void add_constraint(GodotConstraint2D *p_constraint, int p_pos) { constraint_list.push_back({ p_constraint, p_pos }); }
_FORCE_INLINE_ void remove_constraint(Constraint2DSW *p_constraint, int p_pos) { constraint_list.erase({ 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<Constraint2DSW *, int>> &get_constraint_list() const { return constraint_list; } const List<Pair<GodotConstraint2D *, int>> &get_constraint_list() const { return constraint_list; }
_FORCE_INLINE_ void clear_constraint_list() { constraint_list.clear(); } _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_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; }
@ -267,7 +268,7 @@ public:
_FORCE_INLINE_ void set_continuous_collision_detection_mode(PhysicsServer2D::CCDMode p_mode) { continuous_cd_mode = p_mode; } _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; } _FORCE_INLINE_ PhysicsServer2D::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; }
void set_space(Space2DSW *p_space); void set_space(GodotSpace2D *p_space);
void update_mass_properties(); void update_mass_properties();
void reset_mass_properties(); void reset_mass_properties();
@ -302,13 +303,13 @@ public:
bool sleep_test(real_t p_step); bool sleep_test(real_t p_step);
Body2DSW(); GodotBody2D();
~Body2DSW(); ~GodotBody2D();
}; };
//add contact inline //add contact inline
void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos) { 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_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos) {
int c_max = contacts.size(); int c_max = contacts.size();
if (c_max == 0) { if (c_max == 0) {
@ -350,4 +351,4 @@ void Body2DSW::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_no
c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
} }
#endif // BODY_2D_SW_H #endif // GODOT_BODY_2D_H

74
servers/physics_2d/body_direct_state_2d_sw.cpp → servers/physics_2d/godot_body_direct_state_2d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* body_direct_state_2d_sw.cpp */ /* godot_body_direct_state_2d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,151 +28,151 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "body_direct_state_2d_sw.h" #include "godot_body_direct_state_2d.h"
#include "body_2d_sw.h" #include "godot_body_2d.h"
#include "physics_server_2d_sw.h" #include "godot_physics_server_2d.h"
#include "space_2d_sw.h" #include "godot_space_2d.h"
Vector2 PhysicsDirectBodyState2DSW::get_total_gravity() const { Vector2 GodotPhysicsDirectBodyState2D::get_total_gravity() const {
return body->gravity; return body->gravity;
} }
real_t PhysicsDirectBodyState2DSW::get_total_angular_damp() const { real_t GodotPhysicsDirectBodyState2D::get_total_angular_damp() const {
return body->area_angular_damp; return body->area_angular_damp;
} }
real_t PhysicsDirectBodyState2DSW::get_total_linear_damp() const { real_t GodotPhysicsDirectBodyState2D::get_total_linear_damp() const {
return body->area_linear_damp; return body->area_linear_damp;
} }
Vector2 PhysicsDirectBodyState2DSW::get_center_of_mass() const { Vector2 GodotPhysicsDirectBodyState2D::get_center_of_mass() const {
return body->get_center_of_mass(); return body->get_center_of_mass();
} }
real_t PhysicsDirectBodyState2DSW::get_inverse_mass() const { real_t GodotPhysicsDirectBodyState2D::get_inverse_mass() const {
return body->get_inv_mass(); return body->get_inv_mass();
} }
real_t PhysicsDirectBodyState2DSW::get_inverse_inertia() const { real_t GodotPhysicsDirectBodyState2D::get_inverse_inertia() const {
return body->get_inv_inertia(); return body->get_inv_inertia();
} }
void PhysicsDirectBodyState2DSW::set_linear_velocity(const Vector2 &p_velocity) { void GodotPhysicsDirectBodyState2D::set_linear_velocity(const Vector2 &p_velocity) {
body->wakeup(); body->wakeup();
body->set_linear_velocity(p_velocity); body->set_linear_velocity(p_velocity);
} }
Vector2 PhysicsDirectBodyState2DSW::get_linear_velocity() const { Vector2 GodotPhysicsDirectBodyState2D::get_linear_velocity() const {
return body->get_linear_velocity(); return body->get_linear_velocity();
} }
void PhysicsDirectBodyState2DSW::set_angular_velocity(real_t p_velocity) { void GodotPhysicsDirectBodyState2D::set_angular_velocity(real_t p_velocity) {
body->wakeup(); body->wakeup();
body->set_angular_velocity(p_velocity); body->set_angular_velocity(p_velocity);
} }
real_t PhysicsDirectBodyState2DSW::get_angular_velocity() const { real_t GodotPhysicsDirectBodyState2D::get_angular_velocity() const {
return body->get_angular_velocity(); return body->get_angular_velocity();
} }
void PhysicsDirectBodyState2DSW::set_transform(const Transform2D &p_transform) { void GodotPhysicsDirectBodyState2D::set_transform(const Transform2D &p_transform) {
body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform);
} }
Transform2D PhysicsDirectBodyState2DSW::get_transform() const { Transform2D GodotPhysicsDirectBodyState2D::get_transform() const {
return body->get_transform(); return body->get_transform();
} }
Vector2 PhysicsDirectBodyState2DSW::get_velocity_at_local_position(const Vector2 &p_position) const { Vector2 GodotPhysicsDirectBodyState2D::get_velocity_at_local_position(const Vector2 &p_position) const {
return body->get_velocity_in_local_point(p_position); return body->get_velocity_in_local_point(p_position);
} }
void PhysicsDirectBodyState2DSW::add_central_force(const Vector2 &p_force) { void GodotPhysicsDirectBodyState2D::add_central_force(const Vector2 &p_force) {
body->wakeup(); body->wakeup();
body->add_central_force(p_force); body->add_central_force(p_force);
} }
void PhysicsDirectBodyState2DSW::add_force(const Vector2 &p_force, const Vector2 &p_position) { void GodotPhysicsDirectBodyState2D::add_force(const Vector2 &p_force, const Vector2 &p_position) {
body->wakeup(); body->wakeup();
body->add_force(p_force, p_position); body->add_force(p_force, p_position);
} }
void PhysicsDirectBodyState2DSW::add_torque(real_t p_torque) { void GodotPhysicsDirectBodyState2D::add_torque(real_t p_torque) {
body->wakeup(); body->wakeup();
body->add_torque(p_torque); body->add_torque(p_torque);
} }
void PhysicsDirectBodyState2DSW::apply_central_impulse(const Vector2 &p_impulse) { void GodotPhysicsDirectBodyState2D::apply_central_impulse(const Vector2 &p_impulse) {
body->wakeup(); body->wakeup();
body->apply_central_impulse(p_impulse); body->apply_central_impulse(p_impulse);
} }
void PhysicsDirectBodyState2DSW::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) { void GodotPhysicsDirectBodyState2D::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) {
body->wakeup(); body->wakeup();
body->apply_impulse(p_impulse, p_position); body->apply_impulse(p_impulse, p_position);
} }
void PhysicsDirectBodyState2DSW::apply_torque_impulse(real_t p_torque) { void GodotPhysicsDirectBodyState2D::apply_torque_impulse(real_t p_torque) {
body->wakeup(); body->wakeup();
body->apply_torque_impulse(p_torque); body->apply_torque_impulse(p_torque);
} }
void PhysicsDirectBodyState2DSW::set_sleep_state(bool p_enable) { void GodotPhysicsDirectBodyState2D::set_sleep_state(bool p_enable) {
body->set_active(!p_enable); body->set_active(!p_enable);
} }
bool PhysicsDirectBodyState2DSW::is_sleeping() const { bool GodotPhysicsDirectBodyState2D::is_sleeping() const {
return !body->is_active(); return !body->is_active();
} }
int PhysicsDirectBodyState2DSW::get_contact_count() const { int GodotPhysicsDirectBodyState2D::get_contact_count() const {
return body->contact_count; return body->contact_count;
} }
Vector2 PhysicsDirectBodyState2DSW::get_contact_local_position(int p_contact_idx) const { Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].local_pos; return body->contacts[p_contact_idx].local_pos;
} }
Vector2 PhysicsDirectBodyState2DSW::get_contact_local_normal(int p_contact_idx) const { Vector2 GodotPhysicsDirectBodyState2D::get_contact_local_normal(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].local_normal; return body->contacts[p_contact_idx].local_normal;
} }
int PhysicsDirectBodyState2DSW::get_contact_local_shape(int p_contact_idx) const { int GodotPhysicsDirectBodyState2D::get_contact_local_shape(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
return body->contacts[p_contact_idx].local_shape; return body->contacts[p_contact_idx].local_shape;
} }
RID PhysicsDirectBodyState2DSW::get_contact_collider(int p_contact_idx) const { RID GodotPhysicsDirectBodyState2D::get_contact_collider(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
return body->contacts[p_contact_idx].collider; return body->contacts[p_contact_idx].collider;
} }
Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_position(int p_contact_idx) const { Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].collider_pos; return body->contacts[p_contact_idx].collider_pos;
} }
ObjectID PhysicsDirectBodyState2DSW::get_contact_collider_id(int p_contact_idx) const { ObjectID GodotPhysicsDirectBodyState2D::get_contact_collider_id(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID());
return body->contacts[p_contact_idx].collider_instance_id; return body->contacts[p_contact_idx].collider_instance_id;
} }
int PhysicsDirectBodyState2DSW::get_contact_collider_shape(int p_contact_idx) const { int GodotPhysicsDirectBodyState2D::get_contact_collider_shape(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
return body->contacts[p_contact_idx].collider_shape; return body->contacts[p_contact_idx].collider_shape;
} }
Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const { Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_velocity_at_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2()); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].collider_velocity_at_pos; return body->contacts[p_contact_idx].collider_velocity_at_pos;
} }
PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() { PhysicsDirectSpaceState2D *GodotPhysicsDirectBodyState2D::get_space_state() {
return body->get_space()->get_direct_state(); return body->get_space()->get_direct_state();
} }
real_t PhysicsDirectBodyState2DSW::get_step() const { real_t GodotPhysicsDirectBodyState2D::get_step() const {
return body->get_space()->get_last_step(); return body->get_space()->get_last_step();
} }

16
servers/physics_2d/body_direct_state_2d_sw.h → servers/physics_2d/godot_body_direct_state_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* body_direct_state_2d_sw.h */ /* godot_body_direct_state_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,18 +28,18 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef BODY_DIRECT_STATE_2D_SW_H #ifndef GODOT_BODY_DIRECT_STATE_2D_H
#define BODY_DIRECT_STATE_2D_SW_H #define GODOT_BODY_DIRECT_STATE_2D_H
#include "servers/physics_server_2d.h" #include "servers/physics_server_2d.h"
class Body2DSW; class GodotBody2D;
class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D { class GodotPhysicsDirectBodyState2D : public PhysicsDirectBodyState2D {
GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D); GDCLASS(GodotPhysicsDirectBodyState2D, PhysicsDirectBodyState2D);
public: public:
Body2DSW *body = nullptr; GodotBody2D *body = nullptr;
virtual Vector2 get_total_gravity() const override; virtual Vector2 get_total_gravity() const override;
virtual real_t get_total_angular_damp() const override; virtual real_t get_total_angular_damp() const override;
@ -88,4 +88,4 @@ public:
virtual real_t get_step() const override; virtual real_t get_step() const override;
}; };
#endif // BODY_2D_SW_H #endif // GODOT_BODY_DIRECT_STATE_2D_H

44
servers/physics_2d/body_pair_2d_sw.cpp → servers/physics_2d/godot_body_pair_2d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* body_pair_2d_sw.cpp */ /* godot_body_pair_2d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,20 +28,20 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "body_pair_2d_sw.h" #include "godot_body_pair_2d.h"
#include "collision_solver_2d_sw.h" #include "godot_collision_solver_2d.h"
#include "space_2d_sw.h" #include "godot_space_2d.h"
#define POSITION_CORRECTION #define POSITION_CORRECTION
#define ACCUMULATE_IMPULSES #define ACCUMULATE_IMPULSES
void BodyPair2DSW::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) { void GodotBodyPair2D::_add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self) {
BodyPair2DSW *self = (BodyPair2DSW *)p_self; GodotBodyPair2D *self = (GodotBodyPair2D *)p_self;
self->_contact_added_callback(p_point_A, p_point_B); self->_contact_added_callback(p_point_A, p_point_B);
} }
void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) { void GodotBodyPair2D::_contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B) {
// check if we already have the contact // check if we already have the contact
Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A); Vector2 local_A = A->get_inv_transform().basis_xform(p_point_A);
@ -121,7 +121,7 @@ void BodyPair2DSW::_contact_added_callback(const Vector2 &p_point_A, const Vecto
} }
} }
void BodyPair2DSW::_validate_contacts() { void GodotBodyPair2D::_validate_contacts() {
//make sure to erase contacts that are no longer valid //make sure to erase contacts that are no longer valid
real_t max_separation = space->get_contact_max_separation(); real_t max_separation = space->get_contact_max_separation();
@ -164,7 +164,7 @@ void BodyPair2DSW::_validate_contacts() {
} }
} }
bool BodyPair2DSW::_test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result) { 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, bool p_swap_result) {
Vector2 motion = p_A->get_linear_velocity() * p_step; Vector2 motion = p_A->get_linear_velocity() * p_step;
real_t mlen = motion.length(); real_t mlen = motion.length();
if (mlen < CMP_EPSILON) { if (mlen < CMP_EPSILON) {
@ -217,15 +217,15 @@ bool BodyPair2DSW::_test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const
return true; return true;
} }
real_t combine_bounce(Body2DSW *A, Body2DSW *B) { real_t combine_bounce(GodotBody2D *A, GodotBody2D *B) {
return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1);
} }
real_t combine_friction(Body2DSW *A, Body2DSW *B) { real_t combine_friction(GodotBody2D *A, GodotBody2D *B) {
return ABS(MIN(A->get_friction(), B->get_friction())); return ABS(MIN(A->get_friction(), B->get_friction()));
} }
bool BodyPair2DSW::setup(real_t p_step) { bool GodotBodyPair2D::setup(real_t p_step) {
if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
collided = false; collided = false;
return false; return false;
@ -257,8 +257,8 @@ bool BodyPair2DSW::setup(real_t p_step) {
xform_Bu.elements[2] -= offset_A; xform_Bu.elements[2] -= offset_A;
Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
Shape2DSW *shape_A_ptr = A->get_shape(shape_A); GodotShape2D *shape_A_ptr = A->get_shape(shape_A);
Shape2DSW *shape_B_ptr = B->get_shape(shape_B); GodotShape2D *shape_B_ptr = B->get_shape(shape_B);
Vector2 motion_A, motion_B; Vector2 motion_A, motion_B;
@ -271,7 +271,7 @@ bool BodyPair2DSW::setup(real_t p_step) {
bool prev_collided = collided; bool prev_collided = collided;
collided = CollisionSolver2DSW::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis); collided = GodotCollisionSolver2D::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis);
if (!collided) { if (!collided) {
//test ccd (currently just a raycast) //test ccd (currently just a raycast)
@ -344,7 +344,7 @@ bool BodyPair2DSW::setup(real_t p_step) {
return true; return true;
} }
bool BodyPair2DSW::pre_solve(real_t p_step) { bool GodotBodyPair2D::pre_solve(real_t p_step) {
if (!collided || oneway_disabled) { if (!collided || oneway_disabled) {
return false; return false;
} }
@ -353,8 +353,8 @@ bool BodyPair2DSW::pre_solve(real_t p_step) {
real_t bias = 0.3; real_t bias = 0.3;
Shape2DSW *shape_A_ptr = A->get_shape(shape_A); GodotShape2D *shape_A_ptr = A->get_shape(shape_A);
Shape2DSW *shape_B_ptr = B->get_shape(shape_B); 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() || shape_B_ptr->get_custom_bias()) {
if (shape_A_ptr->get_custom_bias() == 0) { if (shape_A_ptr->get_custom_bias() == 0) {
@ -466,7 +466,7 @@ bool BodyPair2DSW::pre_solve(real_t p_step) {
return do_process; return do_process;
} }
void BodyPair2DSW::solve(real_t p_step) { void GodotBodyPair2D::solve(real_t p_step) {
if (!collided || oneway_disabled) { if (!collided || oneway_disabled) {
return; return;
} }
@ -528,8 +528,8 @@ void BodyPair2DSW::solve(real_t p_step) {
} }
} }
BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B) : GodotBodyPair2D::GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B) :
Constraint2DSW(_arr, 2) { GodotConstraint2D(_arr, 2) {
A = p_A; A = p_A;
B = p_B; B = p_B;
shape_A = p_shape_A; shape_A = p_shape_A;
@ -539,7 +539,7 @@ BodyPair2DSW::BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_sh
B->add_constraint(this, 1); B->add_constraint(this, 1);
} }
BodyPair2DSW::~BodyPair2DSW() { GodotBodyPair2D::~GodotBodyPair2D() {
A->remove_constraint(this, 0); A->remove_constraint(this, 0);
B->remove_constraint(this, 1); B->remove_constraint(this, 1);
} }

28
servers/physics_2d/body_pair_2d_sw.h → servers/physics_2d/godot_body_pair_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* body_pair_2d_sw.h */ /* godot_body_pair_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,23 +28,23 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef BODY_PAIR_2D_SW_H #ifndef GODOT_BODY_PAIR_2D_H
#define BODY_PAIR_2D_SW_H #define GODOT_BODY_PAIR_2D_H
#include "body_2d_sw.h" #include "godot_body_2d.h"
#include "constraint_2d_sw.h" #include "godot_constraint_2d.h"
class BodyPair2DSW : public Constraint2DSW { class GodotBodyPair2D : public GodotConstraint2D {
enum { enum {
MAX_CONTACTS = 2 MAX_CONTACTS = 2
}; };
union { union {
struct { struct {
Body2DSW *A; GodotBody2D *A;
Body2DSW *B; GodotBody2D *B;
}; };
Body2DSW *_arr[2] = { nullptr, nullptr }; GodotBody2D *_arr[2] = { nullptr, nullptr };
}; };
int shape_A = 0; int shape_A = 0;
@ -53,7 +53,7 @@ class BodyPair2DSW : public Constraint2DSW {
bool collide_A = false; bool collide_A = false;
bool collide_B = false; bool collide_B = false;
Space2DSW *space = nullptr; GodotSpace2D *space = nullptr;
struct Contact { struct Contact {
Vector2 position; Vector2 position;
@ -81,7 +81,7 @@ class BodyPair2DSW : public Constraint2DSW {
bool oneway_disabled = false; bool oneway_disabled = false;
bool report_contacts_only = false; bool report_contacts_only = false;
bool _test_ccd(real_t p_step, Body2DSW *p_A, int p_shape_A, const Transform2D &p_xform_A, Body2DSW *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = 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, bool p_swap_result = false);
void _validate_contacts(); void _validate_contacts();
static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self); 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); _FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B);
@ -91,8 +91,8 @@ public:
virtual bool pre_solve(real_t p_step) override; virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override; virtual void solve(real_t p_step) override;
BodyPair2DSW(Body2DSW *p_A, int p_shape_A, Body2DSW *p_B, int p_shape_B); GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B);
~BodyPair2DSW(); ~GodotBodyPair2D();
}; };
#endif // BODY_PAIR_2D_SW_H #endif // GODOT_BODY_PAIR_2D_H

8
servers/physics_2d/broad_phase_2d_sw.cpp → servers/physics_2d/godot_broad_phase_2d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* broad_phase_2d_sw.cpp */ /* godot_broad_phase_2d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,9 +28,9 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "broad_phase_2d_sw.h" #include "godot_broad_phase_2d.h"
BroadPhase2DSW::CreateFunction BroadPhase2DSW::create_func = nullptr; GodotBroadPhase2D::CreateFunction GodotBroadPhase2D::create_func = nullptr;
BroadPhase2DSW::~BroadPhase2DSW() { GodotBroadPhase2D::~GodotBroadPhase2D() {
} }

28
servers/physics_2d/broad_phase_2d_sw.h → servers/physics_2d/godot_broad_phase_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* broad_phase_2d_sw.h */ /* godot_broad_phase_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,44 +28,44 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef BROAD_PHASE_2D_SW_H #ifndef GODOT_BROAD_PHASE_2D_H
#define BROAD_PHASE_2D_SW_H #define GODOT_BROAD_PHASE_2D_H
#include "core/math/math_funcs.h" #include "core/math/math_funcs.h"
#include "core/math/rect2.h" #include "core/math/rect2.h"
class CollisionObject2DSW; class GodotCollisionObject2D;
class BroadPhase2DSW { class GodotBroadPhase2D {
public: public:
typedef BroadPhase2DSW *(*CreateFunction)(); typedef GodotBroadPhase2D *(*CreateFunction)();
static CreateFunction create_func; static CreateFunction create_func;
typedef uint32_t ID; typedef uint32_t ID;
typedef void *(*PairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_userdata); typedef void *(*PairCallback)(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_userdata);
typedef void (*UnpairCallback)(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, 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 // 0 is an invalid ID
virtual ID create(CollisionObject2DSW *p_object_, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false) = 0; 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 move(ID p_id, const Rect2 &p_aabb) = 0;
virtual void set_static(ID p_id, bool p_static) = 0; virtual void set_static(ID p_id, bool p_static) = 0;
virtual void remove(ID p_id) = 0; virtual void remove(ID p_id) = 0;
virtual CollisionObject2DSW *get_object(ID p_id) const = 0; virtual GodotCollisionObject2D *get_object(ID p_id) const = 0;
virtual bool is_static(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 get_subindex(ID p_id) const = 0;
virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 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, CollisionObject2DSW **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_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 set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0;
virtual void update() = 0; virtual void update() = 0;
virtual ~BroadPhase2DSW(); virtual ~GodotBroadPhase2D();
}; };
#endif // BROAD_PHASE_2D_SW_H #endif // GODOT_BROAD_PHASE_2D_H

48
servers/physics_2d/broad_phase_2d_bvh.cpp → servers/physics_2d/godot_broad_phase_2d_bvh.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* broad_phase_2d_bvh.cpp */ /* godot_broad_phase_2d_bvh.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,51 +28,51 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "broad_phase_2d_bvh.h" #include "godot_broad_phase_2d_bvh.h"
#include "collision_object_2d_sw.h" #include "godot_collision_object_2d.h"
BroadPhase2DSW::ID BroadPhase2DBVH::create(CollisionObject2DSW *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) { GodotBroadPhase2D::ID GodotBroadPhase2DBVH::create(GodotCollisionObject2D *p_object, int p_subindex, const Rect2 &p_aabb, bool p_static) {
ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care?
return oid + 1; return oid + 1;
} }
void BroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) { void GodotBroadPhase2DBVH::move(ID p_id, const Rect2 &p_aabb) {
bvh.move(p_id - 1, p_aabb); bvh.move(p_id - 1, p_aabb);
} }
void BroadPhase2DBVH::set_static(ID p_id, bool p_static) { void GodotBroadPhase2DBVH::set_static(ID p_id, bool p_static) {
CollisionObject2DSW *it = bvh.get(p_id - 1); GodotCollisionObject2D *it = bvh.get(p_id - 1);
bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care? bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care?
} }
void BroadPhase2DBVH::remove(ID p_id) { void GodotBroadPhase2DBVH::remove(ID p_id) {
bvh.erase(p_id - 1); bvh.erase(p_id - 1);
} }
CollisionObject2DSW *BroadPhase2DBVH::get_object(ID p_id) const { GodotCollisionObject2D *GodotBroadPhase2DBVH::get_object(ID p_id) const {
CollisionObject2DSW *it = bvh.get(p_id - 1); GodotCollisionObject2D *it = bvh.get(p_id - 1);
ERR_FAIL_COND_V(!it, nullptr); ERR_FAIL_COND_V(!it, nullptr);
return it; return it;
} }
bool BroadPhase2DBVH::is_static(ID p_id) const { bool GodotBroadPhase2DBVH::is_static(ID p_id) const {
return !bvh.is_pairable(p_id - 1); return !bvh.is_pairable(p_id - 1);
} }
int BroadPhase2DBVH::get_subindex(ID p_id) const { int GodotBroadPhase2DBVH::get_subindex(ID p_id) const {
return bvh.get_subindex(p_id - 1); return bvh.get_subindex(p_id - 1);
} }
int BroadPhase2DBVH::cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices) { 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, p_result_indices); return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);
} }
int BroadPhase2DBVH::cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *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, p_result_indices); return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
} }
void *BroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B) { 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) {
BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self); GodotBroadPhase2DBVH *bpo = (GodotBroadPhase2DBVH *)(self);
if (!bpo->pair_callback) { if (!bpo->pair_callback) {
return nullptr; return nullptr;
} }
@ -80,8 +80,8 @@ void *BroadPhase2DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject2
return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
} }
void BroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject2DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject2DSW *p_object_B, int subindex_B, void *pairdata) { 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) {
BroadPhase2DBVH *bpo = (BroadPhase2DBVH *)(self); GodotBroadPhase2DBVH *bpo = (GodotBroadPhase2DBVH *)(self);
if (!bpo->unpair_callback) { if (!bpo->unpair_callback) {
return; return;
} }
@ -89,25 +89,25 @@ void BroadPhase2DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject
bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
} }
void BroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { void GodotBroadPhase2DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
pair_callback = p_pair_callback; pair_callback = p_pair_callback;
pair_userdata = p_userdata; pair_userdata = p_userdata;
} }
void BroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { void GodotBroadPhase2DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
unpair_callback = p_unpair_callback; unpair_callback = p_unpair_callback;
unpair_userdata = p_userdata; unpair_userdata = p_userdata;
} }
void BroadPhase2DBVH::update() { void GodotBroadPhase2DBVH::update() {
bvh.update(); bvh.update();
} }
BroadPhase2DSW *BroadPhase2DBVH::_create() { GodotBroadPhase2D *GodotBroadPhase2DBVH::_create() {
return memnew(BroadPhase2DBVH); return memnew(GodotBroadPhase2DBVH);
} }
BroadPhase2DBVH::BroadPhase2DBVH() { GodotBroadPhase2DBVH::GodotBroadPhase2DBVH() {
bvh.set_pair_callback(_pair_callback, this); bvh.set_pair_callback(_pair_callback, this);
bvh.set_unpair_callback(_unpair_callback, this); bvh.set_unpair_callback(_unpair_callback, this);
} }

31
servers/physics_2d/broad_phase_2d_bvh.h → servers/physics_2d/godot_broad_phase_2d_bvh.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* broad_phase_2d_bvh.h */ /* godot_broad_phase_2d_bvh.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,19 +28,20 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef BROAD_PHASE_2D_BVH_H #ifndef GODOT_BROAD_PHASE_2D_BVH_H
#define BROAD_PHASE_2D_BVH_H #define GODOT_BROAD_PHASE_2D_BVH_H
#include "godot_broad_phase_2d.h"
#include "broad_phase_2d_sw.h"
#include "core/math/bvh.h" #include "core/math/bvh.h"
#include "core/math/rect2.h" #include "core/math/rect2.h"
#include "core/math/vector2.h" #include "core/math/vector2.h"
class BroadPhase2DBVH : public BroadPhase2DSW { class GodotBroadPhase2DBVH : public GodotBroadPhase2D {
BVH_Manager<CollisionObject2DSW, true, 128, Rect2, Vector2> bvh; BVH_Manager<GodotCollisionObject2D, true, 128, Rect2, Vector2> bvh;
static void *_pair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int); static void *_pair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int);
static void _unpair_callback(void *, uint32_t, CollisionObject2DSW *, int, uint32_t, CollisionObject2DSW *, int, void *); static void _unpair_callback(void *, uint32_t, GodotCollisionObject2D *, int, uint32_t, GodotCollisionObject2D *, int, void *);
PairCallback pair_callback = nullptr; PairCallback pair_callback = nullptr;
void *pair_userdata = nullptr; void *pair_userdata = nullptr;
@ -49,25 +50,25 @@ class BroadPhase2DBVH : public BroadPhase2DSW {
public: public:
// 0 is an invalid ID // 0 is an invalid ID
virtual ID create(CollisionObject2DSW *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false); virtual ID create(GodotCollisionObject2D *p_object, int p_subindex = 0, const Rect2 &p_aabb = Rect2(), bool p_static = false);
virtual void move(ID p_id, const Rect2 &p_aabb); virtual void move(ID p_id, const Rect2 &p_aabb);
virtual void set_static(ID p_id, bool p_static); virtual void set_static(ID p_id, bool p_static);
virtual void remove(ID p_id); virtual void remove(ID p_id);
virtual CollisionObject2DSW *get_object(ID p_id) const; virtual GodotCollisionObject2D *get_object(ID p_id) const;
virtual bool is_static(ID p_id) const; virtual bool is_static(ID p_id) const;
virtual int get_subindex(ID p_id) const; virtual int get_subindex(ID p_id) const;
virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); virtual int cull_segment(const Vector2 &p_from, const Vector2 &p_to, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr);
virtual int cull_aabb(const Rect2 &p_aabb, CollisionObject2DSW **p_results, int p_max_results, int *p_result_indices = nullptr); virtual int cull_aabb(const Rect2 &p_aabb, GodotCollisionObject2D **p_results, int p_max_results, int *p_result_indices = nullptr);
virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata);
virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata);
virtual void update(); virtual void update();
static BroadPhase2DSW *_create(); static GodotBroadPhase2D *_create();
BroadPhase2DBVH(); GodotBroadPhase2DBVH();
}; };
#endif // BROAD_PHASE_2D_BVH_H #endif // GODOT_BROAD_PHASE_2D_BVH_H

48
servers/physics_2d/collision_object_2d_sw.cpp → servers/physics_2d/godot_collision_object_2d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* collision_object_2d_sw.cpp */ /* godot_collision_object_2d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,11 +28,11 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "collision_object_2d_sw.h" #include "godot_collision_object_2d.h"
#include "servers/physics_2d/physics_server_2d_sw.h" #include "godot_physics_server_2d.h"
#include "space_2d_sw.h" #include "godot_space_2d.h"
void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_transform, bool p_disabled) { void GodotCollisionObject2D::add_shape(GodotShape2D *p_shape, const Transform2D &p_transform, bool p_disabled) {
Shape s; Shape s;
s.shape = p_shape; s.shape = p_shape;
s.xform = p_transform; s.xform = p_transform;
@ -45,11 +45,11 @@ void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_tra
p_shape->add_owner(this); p_shape->add_owner(this);
if (!pending_shape_update_list.in_list()) { if (!pending_shape_update_list.in_list()) {
PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
} }
} }
void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) { void GodotCollisionObject2D::set_shape(int p_index, GodotShape2D *p_shape) {
ERR_FAIL_INDEX(p_index, shapes.size()); ERR_FAIL_INDEX(p_index, shapes.size());
shapes[p_index].shape->remove_owner(this); shapes[p_index].shape->remove_owner(this);
shapes.write[p_index].shape = p_shape; shapes.write[p_index].shape = p_shape;
@ -57,25 +57,25 @@ void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) {
p_shape->add_owner(this); p_shape->add_owner(this);
if (!pending_shape_update_list.in_list()) { if (!pending_shape_update_list.in_list()) {
PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
} }
} }
void CollisionObject2DSW::set_shape_transform(int p_index, const Transform2D &p_transform) { void GodotCollisionObject2D::set_shape_transform(int p_index, const Transform2D &p_transform) {
ERR_FAIL_INDEX(p_index, shapes.size()); ERR_FAIL_INDEX(p_index, shapes.size());
shapes.write[p_index].xform = p_transform; shapes.write[p_index].xform = p_transform;
shapes.write[p_index].xform_inv = p_transform.affine_inverse(); shapes.write[p_index].xform_inv = p_transform.affine_inverse();
if (!pending_shape_update_list.in_list()) { if (!pending_shape_update_list.in_list()) {
PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
} }
} }
void CollisionObject2DSW::set_shape_disabled(int p_idx, bool p_disabled) { void GodotCollisionObject2D::set_shape_disabled(int p_idx, bool p_disabled) {
ERR_FAIL_INDEX(p_idx, shapes.size()); ERR_FAIL_INDEX(p_idx, shapes.size());
CollisionObject2DSW::Shape &shape = shapes.write[p_idx]; GodotCollisionObject2D::Shape &shape = shapes.write[p_idx];
if (shape.disabled == p_disabled) { if (shape.disabled == p_disabled) {
return; return;
} }
@ -90,16 +90,16 @@ void CollisionObject2DSW::set_shape_disabled(int p_idx, bool p_disabled) {
space->get_broadphase()->remove(shape.bpid); space->get_broadphase()->remove(shape.bpid);
shape.bpid = 0; shape.bpid = 0;
if (!pending_shape_update_list.in_list()) { if (!pending_shape_update_list.in_list()) {
PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
} }
} else if (!p_disabled && shape.bpid == 0) { } else if (!p_disabled && shape.bpid == 0) {
if (!pending_shape_update_list.in_list()) { if (!pending_shape_update_list.in_list()) {
PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
} }
} }
} }
void CollisionObject2DSW::remove_shape(Shape2DSW *p_shape) { void GodotCollisionObject2D::remove_shape(GodotShape2D *p_shape) {
//remove a shape, all the times it appears //remove a shape, all the times it appears
for (int i = 0; i < shapes.size(); i++) { for (int i = 0; i < shapes.size(); i++) {
if (shapes[i].shape == p_shape) { if (shapes[i].shape == p_shape) {
@ -109,7 +109,7 @@ void CollisionObject2DSW::remove_shape(Shape2DSW *p_shape) {
} }
} }
void CollisionObject2DSW::remove_shape(int p_index) { void GodotCollisionObject2D::remove_shape(int p_index) {
//remove anything from shape to be erased to end, so subindices don't change //remove anything from shape to be erased to end, so subindices don't change
ERR_FAIL_INDEX(p_index, shapes.size()); ERR_FAIL_INDEX(p_index, shapes.size());
for (int i = p_index; i < shapes.size(); i++) { for (int i = p_index; i < shapes.size(); i++) {
@ -124,13 +124,13 @@ void CollisionObject2DSW::remove_shape(int p_index) {
shapes.remove(p_index); shapes.remove(p_index);
if (!pending_shape_update_list.in_list()) { if (!pending_shape_update_list.in_list()) {
PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); GodotPhysicsServer2D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
} }
// _update_shapes(); // _update_shapes();
// _shapes_changed(); // _shapes_changed();
} }
void CollisionObject2DSW::_set_static(bool p_static) { void GodotCollisionObject2D::_set_static(bool p_static) {
if (_static == p_static) { if (_static == p_static) {
return; return;
} }
@ -147,7 +147,7 @@ void CollisionObject2DSW::_set_static(bool p_static) {
} }
} }
void CollisionObject2DSW::_unregister_shapes() { void GodotCollisionObject2D::_unregister_shapes() {
for (int i = 0; i < shapes.size(); i++) { for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i]; Shape &s = shapes.write[i];
if (s.bpid > 0) { if (s.bpid > 0) {
@ -157,7 +157,7 @@ void CollisionObject2DSW::_unregister_shapes() {
} }
} }
void CollisionObject2DSW::_update_shapes() { void GodotCollisionObject2D::_update_shapes() {
if (!space) { if (!space) {
return; return;
} }
@ -184,7 +184,7 @@ void CollisionObject2DSW::_update_shapes() {
} }
} }
void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) { void GodotCollisionObject2D::_update_shapes_with_motion(const Vector2 &p_motion) {
if (!space) { if (!space) {
return; return;
} }
@ -211,7 +211,7 @@ void CollisionObject2DSW::_update_shapes_with_motion(const Vector2 &p_motion) {
} }
} }
void CollisionObject2DSW::_set_space(Space2DSW *p_space) { void GodotCollisionObject2D::_set_space(GodotSpace2D *p_space) {
if (space) { if (space) {
space->remove_object(this); space->remove_object(this);
@ -232,12 +232,12 @@ void CollisionObject2DSW::_set_space(Space2DSW *p_space) {
} }
} }
void CollisionObject2DSW::_shape_changed() { void GodotCollisionObject2D::_shape_changed() {
_update_shapes(); _update_shapes();
_shapes_changed(); _shapes_changed();
} }
CollisionObject2DSW::CollisionObject2DSW(Type p_type) : GodotCollisionObject2D::GodotCollisionObject2D(Type p_type) :
pending_shape_update_list(this) { pending_shape_update_list(this) {
type = p_type; type = p_type;
} }

47
servers/physics_2d/collision_object_2d_sw.h → servers/physics_2d/godot_collision_object_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* collision_object_2d_sw.h */ /* godot_collision_object_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,17 +28,18 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef COLLISION_OBJECT_2D_SW_H #ifndef GODOT_COLLISION_OBJECT_2D_H
#define COLLISION_OBJECT_2D_SW_H #define GODOT_COLLISION_OBJECT_2D_H
#include "godot_broad_phase_2d.h"
#include "godot_shape_2d.h"
#include "broad_phase_2d_sw.h"
#include "core/templates/self_list.h" #include "core/templates/self_list.h"
#include "servers/physics_server_2d.h" #include "servers/physics_server_2d.h"
#include "shape_2d_sw.h"
class Space2DSW; class GodotSpace2D;
class CollisionObject2DSW : public ShapeOwner2DSW { class GodotCollisionObject2D : public GodotShapeOwner2D {
public: public:
enum Type { enum Type {
TYPE_AREA, TYPE_AREA,
@ -55,23 +56,23 @@ private:
struct Shape { struct Shape {
Transform2D xform; Transform2D xform;
Transform2D xform_inv; Transform2D xform_inv;
BroadPhase2DSW::ID bpid = 0; GodotBroadPhase2D::ID bpid = 0;
Rect2 aabb_cache; //for rayqueries Rect2 aabb_cache; //for rayqueries
Shape2DSW *shape = nullptr; GodotShape2D *shape = nullptr;
bool disabled = false; bool disabled = false;
bool one_way_collision = false; bool one_way_collision = false;
real_t one_way_collision_margin = 0.0; real_t one_way_collision_margin = 0.0;
}; };
Vector<Shape> shapes; Vector<Shape> shapes;
Space2DSW *space = nullptr; GodotSpace2D *space = nullptr;
Transform2D transform; Transform2D transform;
Transform2D inv_transform; Transform2D inv_transform;
uint32_t collision_mask = 1; uint32_t collision_mask = 1;
uint32_t collision_layer = 1; uint32_t collision_layer = 1;
bool _static = true; bool _static = true;
SelfList<CollisionObject2DSW> pending_shape_update_list; SelfList<GodotCollisionObject2D> pending_shape_update_list;
void _update_shapes(); void _update_shapes();
@ -89,9 +90,9 @@ protected:
void _set_static(bool p_static); void _set_static(bool p_static);
virtual void _shapes_changed() = 0; virtual void _shapes_changed() = 0;
void _set_space(Space2DSW *p_space); void _set_space(GodotSpace2D *p_space);
CollisionObject2DSW(Type p_type); GodotCollisionObject2D(Type p_type);
public: public:
_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
@ -106,12 +107,12 @@ public:
void _shape_changed(); void _shape_changed();
_FORCE_INLINE_ Type get_type() const { return type; } _FORCE_INLINE_ Type get_type() const { return type; }
void add_shape(Shape2DSW *p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false); void add_shape(GodotShape2D *p_shape, const Transform2D &p_transform = Transform2D(), bool p_disabled = false);
void set_shape(int p_index, Shape2DSW *p_shape); void set_shape(int p_index, GodotShape2D *p_shape);
void set_shape_transform(int p_index, const Transform2D &p_transform); void set_shape_transform(int p_index, const Transform2D &p_transform);
_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
_FORCE_INLINE_ Shape2DSW *get_shape(int p_index) const { _FORCE_INLINE_ GodotShape2D *get_shape(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size()); CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].shape; return shapes[p_index].shape;
} }
@ -130,7 +131,7 @@ public:
_FORCE_INLINE_ const Transform2D &get_transform() const { return transform; } _FORCE_INLINE_ const Transform2D &get_transform() const { return transform; }
_FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; } _FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; }
_FORCE_INLINE_ Space2DSW *get_space() const { return space; } _FORCE_INLINE_ GodotSpace2D *get_space() const { return space; }
void set_shape_disabled(int p_idx, bool p_disabled); void set_shape_disabled(int p_idx, bool p_disabled);
_FORCE_INLINE_ bool is_shape_disabled(int p_idx) const { _FORCE_INLINE_ bool is_shape_disabled(int p_idx) const {
@ -165,25 +166,25 @@ public:
} }
_FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; } _FORCE_INLINE_ uint32_t get_collision_layer() const { return collision_layer; }
void remove_shape(Shape2DSW *p_shape); void remove_shape(GodotShape2D *p_shape);
void remove_shape(int p_index); void remove_shape(int p_index);
virtual void set_space(Space2DSW *p_space) = 0; virtual void set_space(GodotSpace2D *p_space) = 0;
_FORCE_INLINE_ bool is_static() const { return _static; } _FORCE_INLINE_ bool is_static() const { return _static; }
void set_pickable(bool p_pickable) { pickable = p_pickable; } void set_pickable(bool p_pickable) { pickable = p_pickable; }
_FORCE_INLINE_ bool is_pickable() const { return pickable; } _FORCE_INLINE_ bool is_pickable() const { return pickable; }
_FORCE_INLINE_ bool collides_with(CollisionObject2DSW *p_other) const { _FORCE_INLINE_ bool collides_with(GodotCollisionObject2D *p_other) const {
return p_other->collision_layer & collision_mask; return p_other->collision_layer & collision_mask;
} }
_FORCE_INLINE_ bool interacts_with(CollisionObject2DSW *p_other) const { _FORCE_INLINE_ bool interacts_with(GodotCollisionObject2D *p_other) const {
return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask;
} }
virtual ~CollisionObject2DSW() {} virtual ~GodotCollisionObject2D() {}
}; };
#endif // COLLISION_OBJECT_2D_SW_H #endif // GODOT_COLLISION_OBJECT_2D_H

26
servers/physics_2d/collision_solver_2d_sw.cpp → servers/physics_2d/godot_collision_solver_2d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* collision_solver_2d_sw.cpp */ /* godot_collision_solver_2d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,14 +28,14 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "collision_solver_2d_sw.h" #include "godot_collision_solver_2d.h"
#include "collision_solver_2d_sat.h" #include "godot_collision_solver_2d_sat.h"
#define collision_solver sat_2d_calculate_penetration #define collision_solver sat_2d_calculate_penetration
//#define collision_solver gjk_epa_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration
bool CollisionSolver2DSW::solve_static_world_boundary(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { 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, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
const WorldBoundaryShape2DSW *world_boundary = static_cast<const WorldBoundaryShape2DSW *>(p_shape_A); const GodotWorldBoundaryShape2D *world_boundary = static_cast<const GodotWorldBoundaryShape2D *>(p_shape_A);
if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) { if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_WORLD_BOUNDARY) {
return false; return false;
} }
@ -73,8 +73,8 @@ bool CollisionSolver2DSW::solve_static_world_boundary(const Shape2DSW *p_shape_A
return found; return found;
} }
bool CollisionSolver2DSW::solve_separation_ray(const Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *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) { 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 SeparationRayShape2DSW *ray = static_cast<const SeparationRayShape2DSW *>(p_shape_A); const GodotSeparationRayShape2D *ray = static_cast<const GodotSeparationRayShape2D *>(p_shape_A);
if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY) { if (p_shape_B->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY) {
return false; return false;
} }
@ -134,13 +134,13 @@ bool CollisionSolver2DSW::solve_separation_ray(const Shape2DSW *p_shape_A, const
struct _ConcaveCollisionInfo2D { struct _ConcaveCollisionInfo2D {
const Transform2D *transform_A = nullptr; const Transform2D *transform_A = nullptr;
const Shape2DSW *shape_A = nullptr; const GodotShape2D *shape_A = nullptr;
const Transform2D *transform_B = nullptr; const Transform2D *transform_B = nullptr;
Vector2 motion_A; Vector2 motion_A;
Vector2 motion_B; Vector2 motion_B;
real_t margin_A = 0.0; real_t margin_A = 0.0;
real_t margin_B = 0.0; real_t margin_B = 0.0;
CollisionSolver2DSW::CallbackResult result_callback; GodotCollisionSolver2D::CallbackResult result_callback;
void *userdata = nullptr; void *userdata = nullptr;
bool swap_result = false; bool swap_result = false;
bool collided = false; bool collided = false;
@ -149,7 +149,7 @@ struct _ConcaveCollisionInfo2D {
Vector2 *sep_axis = nullptr; Vector2 *sep_axis = nullptr;
}; };
bool CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex) { bool GodotCollisionSolver2D::concave_callback(void *p_userdata, GodotShape2D *p_convex) {
_ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata); _ConcaveCollisionInfo2D &cinfo = *(_ConcaveCollisionInfo2D *)(p_userdata);
cinfo.aabb_tests++; cinfo.aabb_tests++;
@ -165,8 +165,8 @@ bool CollisionSolver2DSW::concave_callback(void *p_userdata, Shape2DSW *p_convex
return !cinfo.result_callback; return !cinfo.result_callback;
} }
bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *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) { 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 ConcaveShape2DSW *concave_B = static_cast<const ConcaveShape2DSW *>(p_shape_B); const GodotConcaveShape2D *concave_B = static_cast<const GodotConcaveShape2D *>(p_shape_B);
_ConcaveCollisionInfo2D cinfo; _ConcaveCollisionInfo2D cinfo;
cinfo.transform_A = &p_transform_A; cinfo.transform_A = &p_transform_A;
@ -209,7 +209,7 @@ bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transf
return cinfo.collided; return cinfo.collided;
} }
bool CollisionSolver2DSW::solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *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) { 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_A = p_shape_A->get_type();
PhysicsServer2D::ShapeType type_B = p_shape_B->get_type(); PhysicsServer2D::ShapeType type_B = p_shape_B->get_type();
bool concave_A = p_shape_A->is_concave(); bool concave_A = p_shape_A->is_concave();

22
servers/physics_2d/collision_solver_2d_sw.h → servers/physics_2d/godot_collision_solver_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* collision_solver_2d_sw.h */ /* godot_collision_solver_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,23 +28,23 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef COLLISION_SOLVER_2D_SW_H #ifndef GODOT_COLLISION_SOLVER_2D_H
#define COLLISION_SOLVER_2D_SW_H #define GODOT_COLLISION_SOLVER_2D_H
#include "shape_2d_sw.h" #include "godot_shape_2d.h"
class CollisionSolver2DSW { class GodotCollisionSolver2D {
public: public:
typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata); typedef void (*CallbackResult)(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata);
private: private:
static bool solve_static_world_boundary(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); 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, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool concave_callback(void *p_userdata, Shape2DSW *p_convex); static bool concave_callback(void *p_userdata, GodotShape2D *p_convex);
static bool solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *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_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 Shape2DSW *p_shape_A, const Vector2 &p_motion_A, const Transform2D &p_transform_A, const Shape2DSW *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); 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: public:
static bool solve(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *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); 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 // COLLISION_SOLVER_2D_SW_H #endif // GODOT_COLLISION_SOLVER_2D_H

134
servers/physics_2d/collision_solver_2d_sat.cpp → servers/physics_2d/godot_collision_solver_2d_sat.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* collision_solver_2d_sat.cpp */ /* godot_collision_solver_2d_sat.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,12 +28,12 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "collision_solver_2d_sat.h" #include "godot_collision_solver_2d_sat.h"
#include "core/math/geometry_2d.h" #include "core/math/geometry_2d.h"
struct _CollectorCallback2D { struct _CollectorCallback2D {
CollisionSolver2DSW::CallbackResult callback; GodotCollisionSolver2D::CallbackResult callback;
void *userdata = nullptr; void *userdata = nullptr;
bool swap = false; bool swap = false;
bool collided = false; bool collided = false;
@ -384,14 +384,14 @@ public:
(castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \ (castB && !separator.test_axis(((m_a) - ((m_b) + p_motion_b)).normalized())) || \
(castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized()))) (castA && castB && !separator.test_axis(((m_a) + p_motion_a - ((m_b) + p_motion_b)).normalized())))
typedef void (*CollisionFunc)(const Shape2DSW *, const Transform2D &, const Shape2DSW *, const Transform2D &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, real_t, real_t); typedef void (*CollisionFunc)(const GodotShape2D *, const Transform2D &, const GodotShape2D *, const Transform2D &, _CollectorCallback2D *p_collector, const Vector2 &, const Vector2 &, real_t, real_t);
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_segment_segment(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_segment_segment(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
const SegmentShape2DSW *segment_B = static_cast<const SegmentShape2DSW *>(p_b); const GodotSegmentShape2D *segment_B = static_cast<const GodotSegmentShape2D *>(p_b);
SeparatorAxisTest2D<SegmentShape2DSW, SegmentShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotSegmentShape2D, GodotSegmentShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, segment_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -430,11 +430,11 @@ static void _collision_segment_segment(const Shape2DSW *p_a, const Transform2D &
} }
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_segment_circle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_segment_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW *>(p_b); const GodotCircleShape2D *circle_B = static_cast<const GodotCircleShape2D *>(p_b);
SeparatorAxisTest2D<SegmentShape2DSW, CircleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotSegmentShape2D, GodotCircleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -463,11 +463,11 @@ static void _collision_segment_circle(const Shape2DSW *p_a, const Transform2D &p
} }
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_segment_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_segment_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b); const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b);
SeparatorAxisTest2D<SegmentShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotSegmentShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -534,11 +534,11 @@ static void _collision_segment_rectangle(const Shape2DSW *p_a, const Transform2D
} }
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_segment_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_segment_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
SeparatorAxisTest2D<SegmentShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotSegmentShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -575,11 +575,11 @@ static void _collision_segment_capsule(const Shape2DSW *p_a, const Transform2D &
} }
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_segment_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_segment_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const SegmentShape2DSW *segment_A = static_cast<const SegmentShape2DSW *>(p_a); const GodotSegmentShape2D *segment_A = static_cast<const GodotSegmentShape2D *>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
SeparatorAxisTest2D<SegmentShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotSegmentShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(segment_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -614,11 +614,11 @@ static void _collision_segment_convex_polygon(const Shape2DSW *p_a, const Transf
///////// /////////
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_circle_circle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_circle_circle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
const CircleShape2DSW *circle_B = static_cast<const CircleShape2DSW *>(p_b); const GodotCircleShape2D *circle_B = static_cast<const GodotCircleShape2D *>(p_b);
SeparatorAxisTest2D<CircleShape2DSW, CircleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotCircleShape2D, GodotCircleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, circle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -636,11 +636,11 @@ static void _collision_circle_circle(const Shape2DSW *p_a, const Transform2D &p_
} }
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_circle_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_circle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b); const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b);
SeparatorAxisTest2D<CircleShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotCircleShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -694,11 +694,11 @@ static void _collision_circle_rectangle(const Shape2DSW *p_a, const Transform2D
} }
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_circle_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_circle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
SeparatorAxisTest2D<CircleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotCircleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -727,11 +727,11 @@ static void _collision_circle_capsule(const Shape2DSW *p_a, const Transform2D &p
} }
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_circle_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_circle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const CircleShape2DSW *circle_A = static_cast<const CircleShape2DSW *>(p_a); const GodotCircleShape2D *circle_A = static_cast<const GodotCircleShape2D *>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
SeparatorAxisTest2D<CircleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotCircleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(circle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -758,11 +758,11 @@ static void _collision_circle_convex_polygon(const Shape2DSW *p_a, const Transfo
///////// /////////
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_rectangle_rectangle(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_rectangle_rectangle(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a); const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a);
const RectangleShape2DSW *rectangle_B = static_cast<const RectangleShape2DSW *>(p_b); const GodotRectangleShape2D *rectangle_B = static_cast<const GodotRectangleShape2D *>(p_b);
SeparatorAxisTest2D<RectangleShape2DSW, RectangleShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotRectangleShape2D, GodotRectangleShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, rectangle_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -832,11 +832,11 @@ static void _collision_rectangle_rectangle(const Shape2DSW *p_a, const Transform
} }
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_rectangle_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a); const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
SeparatorAxisTest2D<RectangleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotRectangleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -910,11 +910,11 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D
} }
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_rectangle_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_rectangle_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const RectangleShape2DSW *rectangle_A = static_cast<const RectangleShape2DSW *>(p_a); const GodotRectangleShape2D *rectangle_A = static_cast<const GodotRectangleShape2D *>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
SeparatorAxisTest2D<RectangleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotRectangleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(rectangle_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -972,11 +972,11 @@ static void _collision_rectangle_convex_polygon(const Shape2DSW *p_a, const Tran
///////// /////////
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_capsule_capsule(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_capsule_capsule(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW *>(p_a); const GodotCapsuleShape2D *capsule_A = static_cast<const GodotCapsuleShape2D *>(p_a);
const CapsuleShape2DSW *capsule_B = static_cast<const CapsuleShape2DSW *>(p_b); const GodotCapsuleShape2D *capsule_B = static_cast<const GodotCapsuleShape2D *>(p_b);
SeparatorAxisTest2D<CapsuleShape2DSW, CapsuleShape2DSW, castA, castB, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotCapsuleShape2D, GodotCapsuleShape2D, castA, castB, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -1020,11 +1020,11 @@ static void _collision_capsule_capsule(const Shape2DSW *p_a, const Transform2D &
} }
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_capsule_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const CapsuleShape2DSW *capsule_A = static_cast<const CapsuleShape2DSW *>(p_a); const GodotCapsuleShape2D *capsule_A = static_cast<const GodotCapsuleShape2D *>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
SeparatorAxisTest2D<CapsuleShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotCapsuleShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(capsule_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -1066,11 +1066,11 @@ static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Transf
///////// /////////
template <bool castA, bool castB, bool withMargin> template <bool castA, bool castB, bool withMargin>
static void _collision_convex_polygon_convex_polygon(const Shape2DSW *p_a, const Transform2D &p_transform_a, const Shape2DSW *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) { static void _collision_convex_polygon_convex_polygon(const GodotShape2D *p_a, const Transform2D &p_transform_a, const GodotShape2D *p_b, const Transform2D &p_transform_b, _CollectorCallback2D *p_collector, const Vector2 &p_motion_a, const Vector2 &p_motion_b, real_t p_margin_A, real_t p_margin_B) {
const ConvexPolygonShape2DSW *convex_A = static_cast<const ConvexPolygonShape2DSW *>(p_a); const GodotConvexPolygonShape2D *convex_A = static_cast<const GodotConvexPolygonShape2D *>(p_a);
const ConvexPolygonShape2DSW *convex_B = static_cast<const ConvexPolygonShape2DSW *>(p_b); const GodotConvexPolygonShape2D *convex_B = static_cast<const GodotConvexPolygonShape2D *>(p_b);
SeparatorAxisTest2D<ConvexPolygonShape2DSW, ConvexPolygonShape2DSW, castA, castB, withMargin> separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B); SeparatorAxisTest2D<GodotConvexPolygonShape2D, GodotConvexPolygonShape2D, castA, castB, withMargin> separator(convex_A, p_transform_a, convex_B, p_transform_b, p_collector, p_motion_a, p_motion_b, p_margin_A, p_margin_B);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -1107,7 +1107,7 @@ static void _collision_convex_polygon_convex_polygon(const Shape2DSW *p_a, const
//////// ////////
bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CollisionSolver2DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) { 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, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) {
PhysicsServer2D::ShapeType type_A = p_shape_A->get_type(); PhysicsServer2D::ShapeType type_A = p_shape_A->get_type();
ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false); ERR_FAIL_COND_V(type_A == PhysicsServer2D::SHAPE_WORLD_BOUNDARY, false);
@ -1359,8 +1359,8 @@ bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D
callback.collided = false; callback.collided = false;
callback.sep_axis = sep_axis; callback.sep_axis = sep_axis;
const Shape2DSW *A = p_shape_A; const GodotShape2D *A = p_shape_A;
const Shape2DSW *B = p_shape_B; const GodotShape2D *B = p_shape_B;
const Transform2D *transform_A = &p_transform_A; const Transform2D *transform_A = &p_transform_A;
const Transform2D *transform_B = &p_transform_B; const Transform2D *transform_B = &p_transform_B;
const Vector2 *motion_A = &p_motion_A; const Vector2 *motion_A = &p_motion_A;

12
servers/physics_2d/collision_solver_2d_sat.h → servers/physics_2d/godot_collision_solver_2d_sat.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* collision_solver_2d_sat.h */ /* godot_collision_solver_2d_sat.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,11 +28,11 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef COLLISION_SOLVER_2D_SAT_H #ifndef GODOT_COLLISION_SOLVER_2D_SAT_H
#define COLLISION_SOLVER_2D_SAT_H #define GODOT_COLLISION_SOLVER_2D_SAT_H
#include "collision_solver_2d_sw.h" #include "godot_collision_solver_2d.h"
bool sat_2d_calculate_penetration(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CollisionSolver2DSW::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); 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 // COLLISION_SOLVER_2D_SAT_H #endif // GODOT_COLLISION_SOLVER_2D_SAT_H

20
servers/physics_2d/constraint_2d_sw.h → servers/physics_2d/godot_constraint_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* constraint_2d_sw.h */ /* godot_constraint_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,13 +28,13 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef CONSTRAINT_2D_SW_H #ifndef GODOT_CONSTRAINT_2D_H
#define CONSTRAINT_2D_SW_H #define GODOT_CONSTRAINT_2D_H
#include "body_2d_sw.h" #include "godot_body_2d.h"
class Constraint2DSW { class GodotConstraint2D {
Body2DSW **_body_ptr; GodotBody2D **_body_ptr;
int _body_count; int _body_count;
uint64_t island_step = 0; uint64_t island_step = 0;
bool disabled_collisions_between_bodies = true; bool disabled_collisions_between_bodies = true;
@ -42,7 +42,7 @@ class Constraint2DSW {
RID self; RID self;
protected: protected:
Constraint2DSW(Body2DSW **p_body_ptr = nullptr, int p_body_count = 0) { GodotConstraint2D(GodotBody2D **p_body_ptr = nullptr, int p_body_count = 0) {
_body_ptr = p_body_ptr; _body_ptr = p_body_ptr;
_body_count = p_body_count; _body_count = p_body_count;
} }
@ -54,7 +54,7 @@ public:
_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _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 set_island_step(uint64_t p_step) { island_step = p_step; }
_FORCE_INLINE_ Body2DSW **get_body_ptr() const { return _body_ptr; } _FORCE_INLINE_ GodotBody2D **get_body_ptr() const { return _body_ptr; }
_FORCE_INLINE_ int get_body_count() const { return _body_count; } _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_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; }
@ -64,7 +64,7 @@ public:
virtual bool pre_solve(real_t p_step) = 0; virtual bool pre_solve(real_t p_step) = 0;
virtual void solve(real_t p_step) = 0; virtual void solve(real_t p_step) = 0;
virtual ~Constraint2DSW() {} virtual ~GodotConstraint2D() {}
}; };
#endif // CONSTRAINT_2D_SW_H #endif // GODOT_CONSTRAINT_2D_H

58
servers/physics_2d/joints_2d_sw.cpp → servers/physics_2d/godot_joints_2d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* joints_2d_sw.cpp */ /* godot_joints_2d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,9 +28,9 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "joints_2d_sw.h" #include "godot_joints_2d.h"
#include "space_2d_sw.h" #include "godot_space_2d.h"
//based on chipmunk joint constraints //based on chipmunk joint constraints
@ -55,7 +55,7 @@
* SOFTWARE. * SOFTWARE.
*/ */
void Joint2DSW::copy_settings_from(Joint2DSW *p_joint) { void GodotJoint2D::copy_settings_from(GodotJoint2D *p_joint) {
set_self(p_joint->get_self()); set_self(p_joint->get_self());
set_max_force(p_joint->get_max_force()); set_max_force(p_joint->get_max_force());
set_bias(p_joint->get_bias()); set_bias(p_joint->get_bias());
@ -63,7 +63,7 @@ void Joint2DSW::copy_settings_from(Joint2DSW *p_joint) {
disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies());
} }
static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) { static inline real_t k_scalar(GodotBody2D *a, GodotBody2D *b, const Vector2 &rA, const Vector2 &rB, const Vector2 &n) {
real_t value = 0.0; real_t value = 0.0;
{ {
@ -82,7 +82,7 @@ static inline real_t k_scalar(Body2DSW *a, Body2DSW *b, const Vector2 &rA, const
} }
static inline Vector2 static inline Vector2
relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB) { 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(); Vector2 sum = a->get_linear_velocity() - (rA - a->get_center_of_mass()).orthogonal() * a->get_angular_velocity();
if (b) { if (b) {
return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum; return (b->get_linear_velocity() - (rB - b->get_center_of_mass()).orthogonal() * b->get_angular_velocity()) - sum;
@ -92,11 +92,11 @@ relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB) {
} }
static inline real_t static inline real_t
normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n) { normal_relative_velocity(GodotBody2D *a, GodotBody2D *b, Vector2 rA, Vector2 rB, Vector2 n) {
return relative_velocity(a, b, rA, rB).dot(n); return relative_velocity(a, b, rA, rB).dot(n);
} }
bool PinJoint2DSW::setup(real_t p_step) { bool GodotPinJoint2D::setup(real_t p_step) {
dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
@ -104,7 +104,7 @@ bool PinJoint2DSW::setup(real_t p_step) {
return false; return false;
} }
Space2DSW *space = A->get_space(); GodotSpace2D *space = A->get_space();
ERR_FAIL_COND_V(!space, false); ERR_FAIL_COND_V(!space, false);
rA = A->get_transform().basis_xform(anchor_A); rA = A->get_transform().basis_xform(anchor_A);
@ -158,7 +158,7 @@ inline Vector2 custom_cross(const Vector2 &p_vec, real_t p_other) {
return Vector2(p_other * p_vec.y, -p_other * p_vec.x); return Vector2(p_other * p_vec.y, -p_other * p_vec.x);
} }
bool PinJoint2DSW::pre_solve(real_t p_step) { bool GodotPinJoint2D::pre_solve(real_t p_step) {
// Apply accumulated impulse. // Apply accumulated impulse.
if (dynamic_A) { if (dynamic_A) {
A->apply_impulse(-P, rA); A->apply_impulse(-P, rA);
@ -170,7 +170,7 @@ bool PinJoint2DSW::pre_solve(real_t p_step) {
return true; return true;
} }
void PinJoint2DSW::solve(real_t p_step) { void GodotPinJoint2D::solve(real_t p_step) {
// compute relative velocity // compute relative velocity
Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity()); Vector2 vA = A->get_linear_velocity() - custom_cross(rA - A->get_center_of_mass(), A->get_angular_velocity());
@ -193,21 +193,21 @@ void PinJoint2DSW::solve(real_t p_step) {
P += impulse; P += impulse;
} }
void PinJoint2DSW::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) { void GodotPinJoint2D::set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value) {
if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) { if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) {
softness = p_value; softness = p_value;
} }
} }
real_t PinJoint2DSW::get_param(PhysicsServer2D::PinJointParam p_param) const { real_t GodotPinJoint2D::get_param(PhysicsServer2D::PinJointParam p_param) const {
if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) { if (p_param == PhysicsServer2D::PIN_JOINT_SOFTNESS) {
return softness; return softness;
} }
ERR_FAIL_V(0); ERR_FAIL_V(0);
} }
PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b) : GodotPinJoint2D::GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b) :
Joint2DSW(_arr, p_body_b ? 2 : 1) { GodotJoint2D(_arr, p_body_b ? 2 : 1) {
A = p_body_a; A = p_body_a;
B = p_body_b; B = p_body_b;
anchor_A = p_body_a->get_inv_transform().xform(p_pos); anchor_A = p_body_a->get_inv_transform().xform(p_pos);
@ -224,7 +224,7 @@ PinJoint2DSW::PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p
////////////////////////////////////////////// //////////////////////////////////////////////
static inline void static inline void
k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) { k_tensor(GodotBody2D *a, GodotBody2D *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) {
// calculate mass matrix // calculate mass matrix
// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... // 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 k11, k12, k21, k22;
@ -273,7 +273,7 @@ mult_k(const Vector2 &vr, const Vector2 &k1, const Vector2 &k2) {
return Vector2(vr.dot(k1), vr.dot(k2)); return Vector2(vr.dot(k1), vr.dot(k2));
} }
bool GrooveJoint2DSW::setup(real_t p_step) { bool GodotGrooveJoint2D::setup(real_t p_step) {
dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
@ -281,7 +281,7 @@ bool GrooveJoint2DSW::setup(real_t p_step) {
return false; return false;
} }
Space2DSW *space = A->get_space(); GodotSpace2D *space = A->get_space();
ERR_FAIL_COND_V(!space, false); ERR_FAIL_COND_V(!space, false);
// calculate endpoints in worldspace // calculate endpoints in worldspace
@ -329,7 +329,7 @@ bool GrooveJoint2DSW::setup(real_t p_step) {
return true; return true;
} }
bool GrooveJoint2DSW::pre_solve(real_t p_step) { bool GodotGrooveJoint2D::pre_solve(real_t p_step) {
// Apply accumulated impulse. // Apply accumulated impulse.
if (dynamic_A) { if (dynamic_A) {
A->apply_impulse(-jn_acc, rA); A->apply_impulse(-jn_acc, rA);
@ -341,7 +341,7 @@ bool GrooveJoint2DSW::pre_solve(real_t p_step) {
return true; return true;
} }
void GrooveJoint2DSW::solve(real_t p_step) { void GodotGrooveJoint2D::solve(real_t p_step) {
// compute impulse // compute impulse
Vector2 vr = relative_velocity(A, B, rA, rB); Vector2 vr = relative_velocity(A, B, rA, rB);
@ -361,8 +361,8 @@ void GrooveJoint2DSW::solve(real_t p_step) {
} }
} }
GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b) : 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) :
Joint2DSW(_arr, 2) { GodotJoint2D(_arr, 2) {
A = p_body_a; A = p_body_a;
B = p_body_b; B = p_body_b;
@ -379,7 +379,7 @@ GrooveJoint2DSW::GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_
////////////////////////////////////////////// //////////////////////////////////////////////
////////////////////////////////////////////// //////////////////////////////////////////////
bool DampedSpringJoint2DSW::setup(real_t p_step) { bool GodotDampedSpringJoint2D::setup(real_t p_step) {
dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); dynamic_A = (A->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC); dynamic_B = (B->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC);
@ -412,7 +412,7 @@ bool DampedSpringJoint2DSW::setup(real_t p_step) {
return true; return true;
} }
bool DampedSpringJoint2DSW::pre_solve(real_t p_step) { bool GodotDampedSpringJoint2D::pre_solve(real_t p_step) {
// Apply spring force. // Apply spring force.
if (dynamic_A) { if (dynamic_A) {
A->apply_impulse(-j, rA); A->apply_impulse(-j, rA);
@ -424,7 +424,7 @@ bool DampedSpringJoint2DSW::pre_solve(real_t p_step) {
return true; return true;
} }
void DampedSpringJoint2DSW::solve(real_t p_step) { void GodotDampedSpringJoint2D::solve(real_t p_step) {
// compute relative velocity // compute relative velocity
real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn; real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn;
@ -442,7 +442,7 @@ void DampedSpringJoint2DSW::solve(real_t p_step) {
} }
} }
void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) { void GodotDampedSpringJoint2D::set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value) {
switch (p_param) { switch (p_param) {
case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: {
rest_length = p_value; rest_length = p_value;
@ -456,7 +456,7 @@ void DampedSpringJoint2DSW::set_param(PhysicsServer2D::DampedSpringParam p_param
} }
} }
real_t DampedSpringJoint2DSW::get_param(PhysicsServer2D::DampedSpringParam p_param) const { real_t GodotDampedSpringJoint2D::get_param(PhysicsServer2D::DampedSpringParam p_param) const {
switch (p_param) { switch (p_param) {
case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: { case PhysicsServer2D::DAMPED_SPRING_REST_LENGTH: {
return rest_length; return rest_length;
@ -472,8 +472,8 @@ real_t DampedSpringJoint2DSW::get_param(PhysicsServer2D::DampedSpringParam p_par
ERR_FAIL_V(0); ERR_FAIL_V(0);
} }
DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, Body2DSW *p_body_a, Body2DSW *p_body_b) : GodotDampedSpringJoint2D::GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b) :
Joint2DSW(_arr, 2) { GodotJoint2D(_arr, 2) {
A = p_body_a; A = p_body_a;
B = p_body_b; B = p_body_b;
anchor_A = A->get_inv_transform().xform(p_anchor_a); anchor_A = A->get_inv_transform().xform(p_anchor_a);

54
servers/physics_2d/joints_2d_sw.h → servers/physics_2d/godot_joints_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* joints_2d_sw.h */ /* godot_joints_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,13 +28,13 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef JOINTS_2D_SW_H #ifndef GODOT_JOINTS_2D_H
#define JOINTS_2D_SW_H #define GODOT_JOINTS_2D_H
#include "body_2d_sw.h" #include "godot_body_2d.h"
#include "constraint_2d_sw.h" #include "godot_constraint_2d.h"
class Joint2DSW : public Constraint2DSW { class GodotJoint2D : public GodotConstraint2D {
real_t bias = 0; real_t bias = 0;
real_t max_bias = 3.40282e+38; real_t max_bias = 3.40282e+38;
real_t max_force = 3.40282e+38; real_t max_force = 3.40282e+38;
@ -57,15 +57,15 @@ public:
virtual bool pre_solve(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 {} virtual void solve(real_t p_step) override {}
void copy_settings_from(Joint2DSW *p_joint); void copy_settings_from(GodotJoint2D *p_joint);
virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; } virtual PhysicsServer2D::JointType get_type() const { return PhysicsServer2D::JOINT_TYPE_MAX; }
Joint2DSW(Body2DSW **p_body_ptr = nullptr, int p_body_count = 0) : GodotJoint2D(GodotBody2D **p_body_ptr = nullptr, int p_body_count = 0) :
Constraint2DSW(p_body_ptr, p_body_count) {} GodotConstraint2D(p_body_ptr, p_body_count) {}
virtual ~Joint2DSW() { virtual ~GodotJoint2D() {
for (int i = 0; i < get_body_count(); i++) { for (int i = 0; i < get_body_count(); i++) {
Body2DSW *body = get_body_ptr()[i]; GodotBody2D *body = get_body_ptr()[i];
if (body) { if (body) {
body->remove_constraint(this, i); body->remove_constraint(this, i);
} }
@ -73,14 +73,14 @@ public:
}; };
}; };
class PinJoint2DSW : public Joint2DSW { class GodotPinJoint2D : public GodotJoint2D {
union { union {
struct { struct {
Body2DSW *A; GodotBody2D *A;
Body2DSW *B; GodotBody2D *B;
}; };
Body2DSW *_arr[2] = { nullptr, nullptr }; GodotBody2D *_arr[2] = { nullptr, nullptr };
}; };
Transform2D M; Transform2D M;
@ -101,17 +101,17 @@ public:
void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value); void set_param(PhysicsServer2D::PinJointParam p_param, real_t p_value);
real_t get_param(PhysicsServer2D::PinJointParam p_param) const; real_t get_param(PhysicsServer2D::PinJointParam p_param) const;
PinJoint2DSW(const Vector2 &p_pos, Body2DSW *p_body_a, Body2DSW *p_body_b = nullptr); GodotPinJoint2D(const Vector2 &p_pos, GodotBody2D *p_body_a, GodotBody2D *p_body_b = nullptr);
}; };
class GrooveJoint2DSW : public Joint2DSW { class GodotGrooveJoint2D : public GodotJoint2D {
union { union {
struct { struct {
Body2DSW *A; GodotBody2D *A;
Body2DSW *B; GodotBody2D *B;
}; };
Body2DSW *_arr[2] = { nullptr, nullptr }; GodotBody2D *_arr[2] = { nullptr, nullptr };
}; };
Vector2 A_groove_1; Vector2 A_groove_1;
@ -135,17 +135,17 @@ public:
virtual bool pre_solve(real_t p_step) override; virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override; virtual void solve(real_t p_step) override;
GrooveJoint2DSW(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, Body2DSW *p_body_a, Body2DSW *p_body_b); 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 DampedSpringJoint2DSW : public Joint2DSW { class GodotDampedSpringJoint2D : public GodotJoint2D {
union { union {
struct { struct {
Body2DSW *A; GodotBody2D *A;
Body2DSW *B; GodotBody2D *B;
}; };
Body2DSW *_arr[2] = { nullptr, nullptr }; GodotBody2D *_arr[2] = { nullptr, nullptr };
}; };
Vector2 anchor_A; Vector2 anchor_A;
@ -172,7 +172,7 @@ public:
void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value); void set_param(PhysicsServer2D::DampedSpringParam p_param, real_t p_value);
real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const; real_t get_param(PhysicsServer2D::DampedSpringParam p_param) const;
DampedSpringJoint2DSW(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, Body2DSW *p_body_a, Body2DSW *p_body_b); GodotDampedSpringJoint2D(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, GodotBody2D *p_body_a, GodotBody2D *p_body_b);
}; };
#endif // JOINTS_2D_SW_H #endif // GODOT_JOINTS_2D_H

1350
servers/physics_2d/godot_physics_server_2d.cpp Normal file
View file

File diff suppressed because it is too large Load diff

50
servers/physics_2d/physics_server_2d_sw.h → servers/physics_2d/godot_physics_server_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* physics_server_2d_sw.h */ /* godot_physics_server_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,21 +28,22 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef PHYSICS_2D_SERVER_SW #ifndef GODOT_PHYSICS_SERVER_2D_H
#define PHYSICS_2D_SERVER_SW #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 "core/templates/rid_owner.h"
#include "joints_2d_sw.h"
#include "servers/physics_server_2d.h" #include "servers/physics_server_2d.h"
#include "shape_2d_sw.h"
#include "space_2d_sw.h"
#include "step_2d_sw.h"
class PhysicsServer2DSW : public PhysicsServer2D { class GodotPhysicsServer2D : public PhysicsServer2D {
GDCLASS(PhysicsServer2DSW, PhysicsServer2D); GDCLASS(GodotPhysicsServer2D, PhysicsServer2D);
friend class PhysicsDirectSpaceState2DSW; friend class GodotPhysicsDirectSpaceState2D;
friend class PhysicsDirectBodyState2DSW; friend class GodotPhysicsDirectBodyState2D;
bool active = true; bool active = true;
int iterations = 0; int iterations = 0;
bool doing_sync = false; bool doing_sync = false;
@ -55,20 +56,19 @@ class PhysicsServer2DSW : public PhysicsServer2D {
bool flushing_queries = false; bool flushing_queries = false;
Step2DSW *stepper = nullptr; GodotStep2D *stepper = nullptr;
Set<const Space2DSW *> active_spaces; Set<const GodotSpace2D *> active_spaces;
mutable RID_PtrOwner<Shape2DSW, true> shape_owner; mutable RID_PtrOwner<GodotShape2D, true> shape_owner;
mutable RID_PtrOwner<Space2DSW, true> space_owner; mutable RID_PtrOwner<GodotSpace2D, true> space_owner;
mutable RID_PtrOwner<Area2DSW, true> area_owner; mutable RID_PtrOwner<GodotArea2D, true> area_owner;
mutable RID_PtrOwner<Body2DSW, true> body_owner; mutable RID_PtrOwner<GodotBody2D, true> body_owner;
mutable RID_PtrOwner<Joint2DSW, true> joint_owner; mutable RID_PtrOwner<GodotJoint2D, true> joint_owner;
static PhysicsServer2DSW *singletonsw; static GodotPhysicsServer2D *godot_singleton;
//void _clear_query(Query2DSW *p_query); friend class GodotCollisionObject2D;
friend class CollisionObject2DSW; SelfList<GodotCollisionObject2D>::List pending_shape_update_list;
SelfList<CollisionObject2DSW>::List pending_shape_update_list;
void _update_shapes(); void _update_shapes();
RID _shape_create(ShapeType p_shape); RID _shape_create(ShapeType p_shape);
@ -292,8 +292,8 @@ public:
int get_process_info(ProcessInfo p_info) override; int get_process_info(ProcessInfo p_info) override;
PhysicsServer2DSW(bool p_using_threads = false); GodotPhysicsServer2D(bool p_using_threads = false);
~PhysicsServer2DSW() {} ~GodotPhysicsServer2D() {}
}; };
#endif #endif // GODOT_PHYSICS_SERVER_2D_H

128
servers/physics_2d/shape_2d_sw.cpp → servers/physics_2d/godot_shape_2d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* shape_2d_sw.cpp */ /* godot_shape_2d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,29 +28,29 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "shape_2d_sw.h" #include "godot_shape_2d.h"
#include "core/math/geometry_2d.h" #include "core/math/geometry_2d.h"
#include "core/templates/sort_array.h" #include "core/templates/sort_array.h"
void Shape2DSW::configure(const Rect2 &p_aabb) { void GodotShape2D::configure(const Rect2 &p_aabb) {
aabb = p_aabb; aabb = p_aabb;
configured = true; configured = true;
for (const KeyValue<ShapeOwner2DSW *, int> &E : owners) { for (const KeyValue<GodotShapeOwner2D *, int> &E : owners) {
ShapeOwner2DSW *co = (ShapeOwner2DSW *)E.key; GodotShapeOwner2D *co = (GodotShapeOwner2D *)E.key;
co->_shape_changed(); co->_shape_changed();
} }
} }
Vector2 Shape2DSW::get_support(const Vector2 &p_normal) const { Vector2 GodotShape2D::get_support(const Vector2 &p_normal) const {
Vector2 res[2]; Vector2 res[2];
int amnt; int amnt;
get_supports(p_normal, res, amnt); get_supports(p_normal, res, amnt);
return res[0]; return res[0];
} }
void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) { void GodotShape2D::add_owner(GodotShapeOwner2D *p_owner) {
Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner); Map<GodotShapeOwner2D *, int>::Element *E = owners.find(p_owner);
if (E) { if (E) {
E->get()++; E->get()++;
} else { } else {
@ -58,8 +58,8 @@ void Shape2DSW::add_owner(ShapeOwner2DSW *p_owner) {
} }
} }
void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner) { void GodotShape2D::remove_owner(GodotShapeOwner2D *p_owner) {
Map<ShapeOwner2DSW *, int>::Element *E = owners.find(p_owner); Map<GodotShapeOwner2D *, int>::Element *E = owners.find(p_owner);
ERR_FAIL_COND(!E); ERR_FAIL_COND(!E);
E->get()--; E->get()--;
if (E->get() == 0) { if (E->get() == 0) {
@ -67,15 +67,15 @@ void Shape2DSW::remove_owner(ShapeOwner2DSW *p_owner) {
} }
} }
bool Shape2DSW::is_owner(ShapeOwner2DSW *p_owner) const { bool GodotShape2D::is_owner(GodotShapeOwner2D *p_owner) const {
return owners.has(p_owner); return owners.has(p_owner);
} }
const Map<ShapeOwner2DSW *, int> &Shape2DSW::get_owners() const { const Map<GodotShapeOwner2D *, int> &GodotShape2D::get_owners() const {
return owners; return owners;
} }
Shape2DSW::~Shape2DSW() { GodotShape2D::~GodotShape2D() {
ERR_FAIL_COND(owners.size()); ERR_FAIL_COND(owners.size());
} }
@ -83,15 +83,15 @@ Shape2DSW::~Shape2DSW() {
/*********************************************************/ /*********************************************************/
/*********************************************************/ /*********************************************************/
void WorldBoundaryShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { void GodotWorldBoundaryShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
r_amount = 0; r_amount = 0;
} }
bool WorldBoundaryShape2DSW::contains_point(const Vector2 &p_point) const { bool GodotWorldBoundaryShape2D::contains_point(const Vector2 &p_point) const {
return normal.dot(p_point) < d; return normal.dot(p_point) < d;
} }
bool WorldBoundaryShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { 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; Vector2 segment = p_begin - p_end;
real_t den = normal.dot(segment); real_t den = normal.dot(segment);
@ -113,11 +113,11 @@ bool WorldBoundaryShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec
return true; return true;
} }
real_t WorldBoundaryShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { real_t GodotWorldBoundaryShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
return 0; return 0;
} }
void WorldBoundaryShape2DSW::set_data(const Variant &p_data) { void GodotWorldBoundaryShape2D::set_data(const Variant &p_data) {
ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY); ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY);
Array arr = p_data; Array arr = p_data;
ERR_FAIL_COND(arr.size() != 2); ERR_FAIL_COND(arr.size() != 2);
@ -126,7 +126,7 @@ void WorldBoundaryShape2DSW::set_data(const Variant &p_data) {
configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2))); configure(Rect2(Vector2(-1e4, -1e4), Vector2(1e4 * 2, 1e4 * 2)));
} }
Variant WorldBoundaryShape2DSW::get_data() const { Variant GodotWorldBoundaryShape2D::get_data() const {
Array arr; Array arr;
arr.resize(2); arr.resize(2);
arr[0] = normal; arr[0] = normal;
@ -138,7 +138,7 @@ Variant WorldBoundaryShape2DSW::get_data() const {
/*********************************************************/ /*********************************************************/
/*********************************************************/ /*********************************************************/
void SeparationRayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { void GodotSeparationRayShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
r_amount = 1; r_amount = 1;
if (p_normal.y > 0) { if (p_normal.y > 0) {
@ -148,26 +148,26 @@ void SeparationRayShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_su
} }
} }
bool SeparationRayShape2DSW::contains_point(const Vector2 &p_point) const { bool GodotSeparationRayShape2D::contains_point(const Vector2 &p_point) const {
return false; return false;
} }
bool SeparationRayShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { 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 return false; //rays can't be intersected
} }
real_t SeparationRayShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { real_t GodotSeparationRayShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
return 0; //rays are mass-less return 0; //rays are mass-less
} }
void SeparationRayShape2DSW::set_data(const Variant &p_data) { void GodotSeparationRayShape2D::set_data(const Variant &p_data) {
Dictionary d = p_data; Dictionary d = p_data;
length = d["length"]; length = d["length"];
slide_on_slope = d["slide_on_slope"]; slide_on_slope = d["slide_on_slope"];
configure(Rect2(0, 0, 0.001, length)); configure(Rect2(0, 0, 0.001, length));
} }
Variant SeparationRayShape2DSW::get_data() const { Variant GodotSeparationRayShape2D::get_data() const {
Dictionary d; Dictionary d;
d["length"] = length; d["length"] = length;
d["slide_on_slope"] = slide_on_slope; d["slide_on_slope"] = slide_on_slope;
@ -178,7 +178,7 @@ Variant SeparationRayShape2DSW::get_data() const {
/*********************************************************/ /*********************************************************/
/*********************************************************/ /*********************************************************/
void SegmentShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { 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) { if (Math::abs(p_normal.dot(n)) > _SEGMENT_IS_VALID_SUPPORT_THRESHOLD) {
r_supports[0] = a; r_supports[0] = a;
r_supports[1] = b; r_supports[1] = b;
@ -195,11 +195,11 @@ void SegmentShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports
r_amount = 1; r_amount = 1;
} }
bool SegmentShape2DSW::contains_point(const Vector2 &p_point) const { bool GodotSegmentShape2D::contains_point(const Vector2 &p_point) const {
return false; return false;
} }
bool SegmentShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { 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)) { if (!Geometry2D::segment_intersects_segment(p_begin, p_end, a, b, &r_point)) {
return false; return false;
} }
@ -213,11 +213,11 @@ bool SegmentShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &
return true; return true;
} }
real_t SegmentShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { 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; return p_mass * ((a * p_scale).distance_squared_to(b * p_scale)) / 12;
} }
void SegmentShape2DSW::set_data(const Variant &p_data) { void GodotSegmentShape2D::set_data(const Variant &p_data) {
ERR_FAIL_COND(p_data.get_type() != Variant::RECT2); ERR_FAIL_COND(p_data.get_type() != Variant::RECT2);
Rect2 r = p_data; Rect2 r = p_data;
@ -237,7 +237,7 @@ void SegmentShape2DSW::set_data(const Variant &p_data) {
configure(aabb); configure(aabb);
} }
Variant SegmentShape2DSW::get_data() const { Variant GodotSegmentShape2D::get_data() const {
Rect2 r; Rect2 r;
r.position = a; r.position = a;
r.size = b; r.size = b;
@ -248,16 +248,16 @@ Variant SegmentShape2DSW::get_data() const {
/*********************************************************/ /*********************************************************/
/*********************************************************/ /*********************************************************/
void CircleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { void GodotCircleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
r_amount = 1; r_amount = 1;
*r_supports = p_normal * radius; *r_supports = p_normal * radius;
} }
bool CircleShape2DSW::contains_point(const Vector2 &p_point) const { bool GodotCircleShape2D::contains_point(const Vector2 &p_point) const {
return p_point.length_squared() < radius * radius; return p_point.length_squared() < radius * radius;
} }
bool CircleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { 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; Vector2 line_vec = p_end - p_begin;
real_t a, b, c; real_t a, b, c;
@ -283,19 +283,19 @@ bool CircleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p
return true; return true;
} }
real_t CircleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { 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 a = radius * p_scale.x;
real_t b = radius * p_scale.y; real_t b = radius * p_scale.y;
return p_mass * (a * a + b * b) / 4; return p_mass * (a * a + b * b) / 4;
} }
void CircleShape2DSW::set_data(const Variant &p_data) { void GodotCircleShape2D::set_data(const Variant &p_data) {
ERR_FAIL_COND(!p_data.is_num()); ERR_FAIL_COND(!p_data.is_num());
radius = p_data; radius = p_data;
configure(Rect2(-radius, -radius, radius * 2, radius * 2)); configure(Rect2(-radius, -radius, radius * 2, radius * 2));
} }
Variant CircleShape2DSW::get_data() const { Variant GodotCircleShape2D::get_data() const {
return radius; return radius;
} }
@ -303,7 +303,7 @@ Variant CircleShape2DSW::get_data() const {
/*********************************************************/ /*********************************************************/
/*********************************************************/ /*********************************************************/
void RectangleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { void GodotRectangleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
Vector2 ag; Vector2 ag;
ag[i] = 1.0; ag[i] = 1.0;
@ -333,7 +333,7 @@ void RectangleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_suppor
(p_normal.y < 0) ? -half_extents.y : half_extents.y); (p_normal.y < 0) ? -half_extents.y : half_extents.y);
} }
bool RectangleShape2DSW::contains_point(const Vector2 &p_point) const { bool GodotRectangleShape2D::contains_point(const Vector2 &p_point) const {
real_t x = p_point.x; real_t x = p_point.x;
real_t y = p_point.y; real_t y = p_point.y;
real_t edge_x = half_extents.x; real_t edge_x = half_extents.x;
@ -341,23 +341,23 @@ bool RectangleShape2DSW::contains_point(const Vector2 &p_point) const {
return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y); return (x >= -edge_x) && (x < edge_x) && (y >= -edge_y) && (y < edge_y);
} }
bool RectangleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { 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); return get_aabb().intersects_segment(p_begin, p_end, &r_point, &r_normal);
} }
real_t RectangleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { real_t GodotRectangleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
Vector2 he2 = half_extents * 2 * p_scale; Vector2 he2 = half_extents * 2 * p_scale;
return p_mass * he2.dot(he2) / 12.0; return p_mass * he2.dot(he2) / 12.0;
} }
void RectangleShape2DSW::set_data(const Variant &p_data) { void GodotRectangleShape2D::set_data(const Variant &p_data) {
ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2); ERR_FAIL_COND(p_data.get_type() != Variant::VECTOR2);
half_extents = p_data; half_extents = p_data;
configure(Rect2(-half_extents, half_extents * 2.0)); configure(Rect2(-half_extents, half_extents * 2.0));
} }
Variant RectangleShape2DSW::get_data() const { Variant GodotRectangleShape2D::get_data() const {
return half_extents; return half_extents;
} }
@ -365,7 +365,7 @@ Variant RectangleShape2DSW::get_data() const {
/*********************************************************/ /*********************************************************/
/*********************************************************/ /*********************************************************/
void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { void GodotCapsuleShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
Vector2 n = p_normal; Vector2 n = p_normal;
real_t d = n.y; real_t d = n.y;
@ -392,7 +392,7 @@ void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports
} }
} }
bool CapsuleShape2DSW::contains_point(const Vector2 &p_point) const { bool GodotCapsuleShape2D::contains_point(const Vector2 &p_point) const {
Vector2 p = p_point; Vector2 p = p_point;
p.y = Math::abs(p.y); p.y = Math::abs(p.y);
p.y -= height * 0.5 - radius; p.y -= height * 0.5 - radius;
@ -403,7 +403,7 @@ bool CapsuleShape2DSW::contains_point(const Vector2 &p_point) const {
return p.length_squared() < radius * radius; return p.length_squared() < radius * radius;
} }
bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { bool GodotCapsuleShape2D::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const {
real_t d = 1e10; real_t d = 1e10;
Vector2 n = (p_end - p_begin).normalized(); Vector2 n = (p_end - p_begin).normalized();
bool collided = false; bool collided = false;
@ -463,12 +463,12 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &
return collided; //todo return collided; //todo
} }
real_t CapsuleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { real_t GodotCapsuleShape2D::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
Vector2 he2 = Vector2(radius * 2, height) * p_scale; Vector2 he2 = Vector2(radius * 2, height) * p_scale;
return p_mass * he2.dot(he2) / 12.0; return p_mass * he2.dot(he2) / 12.0;
} }
void CapsuleShape2DSW::set_data(const Variant &p_data) { void GodotCapsuleShape2D::set_data(const Variant &p_data) {
ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2); ERR_FAIL_COND(p_data.get_type() != Variant::ARRAY && p_data.get_type() != Variant::VECTOR2);
if (p_data.get_type() == Variant::ARRAY) { if (p_data.get_type() == Variant::ARRAY) {
@ -486,7 +486,7 @@ void CapsuleShape2DSW::set_data(const Variant &p_data) {
configure(Rect2(-he, he * 2)); configure(Rect2(-he, he * 2));
} }
Variant CapsuleShape2DSW::get_data() const { Variant GodotCapsuleShape2D::get_data() const {
return Point2(height, radius); return Point2(height, radius);
} }
@ -494,7 +494,7 @@ Variant CapsuleShape2DSW::get_data() const {
/*********************************************************/ /*********************************************************/
/*********************************************************/ /*********************************************************/
void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { void GodotConvexPolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
int support_idx = -1; int support_idx = -1;
real_t d = -1e10; real_t d = -1e10;
r_amount = 0; r_amount = 0;
@ -522,7 +522,7 @@ void ConvexPolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_su
r_supports[0] = points[support_idx].pos; r_supports[0] = points[support_idx].pos;
} }
bool ConvexPolygonShape2DSW::contains_point(const Vector2 &p_point) const { bool GodotConvexPolygonShape2D::contains_point(const Vector2 &p_point) const {
bool out = false; bool out = false;
bool in = false; bool in = false;
@ -538,7 +538,7 @@ bool ConvexPolygonShape2DSW::contains_point(const Vector2 &p_point) const {
return in != out; return in != out;
} }
bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { 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(); Vector2 n = (p_end - p_begin).normalized();
real_t d = 1e10; real_t d = 1e10;
bool inters = false; bool inters = false;
@ -568,7 +568,7 @@ bool ConvexPolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vec
return inters; return inters;
} }
real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const { 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."); ERR_FAIL_COND_V_MSG(point_count == 0, 0, "Convex polygon shape has no points.");
Rect2 aabb; Rect2 aabb;
aabb.position = points[0].pos * p_scale; aabb.position = points[0].pos * p_scale;
@ -579,7 +579,7 @@ real_t ConvexPolygonShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2
return p_mass * aabb.size.dot(aabb.size) / 12.0; return p_mass * aabb.size.dot(aabb.size) / 12.0;
} }
void ConvexPolygonShape2DSW::set_data(const Variant &p_data) { void GodotConvexPolygonShape2D::set_data(const Variant &p_data) {
#ifdef REAL_T_IS_DOUBLE #ifdef REAL_T_IS_DOUBLE
ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY);
#else #else
@ -635,7 +635,7 @@ void ConvexPolygonShape2DSW::set_data(const Variant &p_data) {
configure(aabb); configure(aabb);
} }
Variant ConvexPolygonShape2DSW::get_data() const { Variant GodotConvexPolygonShape2D::get_data() const {
Vector<Vector2> dvr; Vector<Vector2> dvr;
dvr.resize(point_count); dvr.resize(point_count);
@ -647,7 +647,7 @@ Variant ConvexPolygonShape2DSW::get_data() const {
return dvr; return dvr;
} }
ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW() { GodotConvexPolygonShape2D::~GodotConvexPolygonShape2D() {
if (points) { if (points) {
memdelete_arr(points); memdelete_arr(points);
} }
@ -655,7 +655,7 @@ ConvexPolygonShape2DSW::~ConvexPolygonShape2DSW() {
////////////////////////////////////////////////// //////////////////////////////////////////////////
void ConcavePolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const { void GodotConcavePolygonShape2D::get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const {
real_t d = -1e10; real_t d = -1e10;
int idx = -1; int idx = -1;
for (int i = 0; i < points.size(); i++) { for (int i = 0; i < points.size(); i++) {
@ -671,11 +671,11 @@ void ConcavePolygonShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_s
*r_supports = points[idx]; *r_supports = points[idx];
} }
bool ConcavePolygonShape2DSW::contains_point(const Vector2 &p_point) const { bool GodotConcavePolygonShape2D::contains_point(const Vector2 &p_point) const {
return false; //sorry return false; //sorry
} }
bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &p_end, Vector2 &r_point, Vector2 &r_normal) const { 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) { if (segments.size() == 0 || points.size() == 0) {
return false; return false;
} }
@ -783,7 +783,7 @@ bool ConcavePolygonShape2DSW::intersect_segment(const Vector2 &p_begin, const Ve
return inters; return inters;
} }
int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) { int GodotConcavePolygonShape2D::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) {
if (p_len == 1) { if (p_len == 1) {
bvh_depth = MAX(p_depth, bvh_depth); bvh_depth = MAX(p_depth, bvh_depth);
bvh.push_back(*p_bvh); bvh.push_back(*p_bvh);
@ -821,7 +821,7 @@ int ConcavePolygonShape2DSW::_generate_bvh(BVH *p_bvh, int p_len, int p_depth) {
return node_idx; return node_idx;
} }
void ConcavePolygonShape2DSW::set_data(const Variant &p_data) { void GodotConcavePolygonShape2D::set_data(const Variant &p_data) {
#ifdef REAL_T_IS_DOUBLE #ifdef REAL_T_IS_DOUBLE
ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY); ERR_FAIL_COND(p_data.get_type() != Variant::PACKED_VECTOR2_ARRAY && p_data.get_type() != Variant::PACKED_FLOAT64_ARRAY);
#else #else
@ -898,7 +898,7 @@ void ConcavePolygonShape2DSW::set_data(const Variant &p_data) {
configure(aabb); configure(aabb);
} }
Variant ConcavePolygonShape2DSW::get_data() const { Variant GodotConcavePolygonShape2D::get_data() const {
Vector<Vector2> rsegments; Vector<Vector2> rsegments;
int len = segments.size(); int len = segments.size();
rsegments.resize(len * 2); rsegments.resize(len * 2);
@ -911,7 +911,7 @@ Variant ConcavePolygonShape2DSW::get_data() const {
return rsegments; return rsegments;
} }
void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { 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); uint32_t *stack = (uint32_t *)alloca(sizeof(int) * bvh_depth);
enum { enum {
@ -957,7 +957,7 @@ void ConcavePolygonShape2DSW::cull(const Rect2 &p_local_aabb, QueryCallback p_ca
Vector2 a = pointptr[s.points[0]]; Vector2 a = pointptr[s.points[0]];
Vector2 b = pointptr[s.points[1]]; Vector2 b = pointptr[s.points[1]];
SegmentShape2DSW ss(a, b, (b - a).orthogonal().normalized()); GodotSegmentShape2D ss(a, b, (b - a).orthogonal().normalized());
if (p_callback(p_userdata, &ss)) { if (p_callback(p_userdata, &ss)) {
return; return;

70
servers/physics_2d/shape_2d_sw.h → servers/physics_2d/godot_shape_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* shape_2d_sw.h */ /* godot_shape_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,29 +28,29 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef SHAPE_2D_2DSW_H #ifndef GODOT_SHAPE_2D_H
#define SHAPE_2D_2DSW_H #define GODOT_SHAPE_2D_H
#include "servers/physics_server_2d.h" #include "servers/physics_server_2d.h"
#define _SEGMENT_IS_VALID_SUPPORT_THRESHOLD 0.99998 #define _SEGMENT_IS_VALID_SUPPORT_THRESHOLD 0.99998
class Shape2DSW; class GodotShape2D;
class ShapeOwner2DSW { class GodotShapeOwner2D {
public: public:
virtual void _shape_changed() = 0; virtual void _shape_changed() = 0;
virtual void remove_shape(Shape2DSW *p_shape) = 0; virtual void remove_shape(GodotShape2D *p_shape) = 0;
virtual ~ShapeOwner2DSW() {} virtual ~GodotShapeOwner2D() {}
}; };
class Shape2DSW { class GodotShape2D {
RID self; RID self;
Rect2 aabb; Rect2 aabb;
bool configured = false; bool configured = false;
real_t custom_bias = 0.0; real_t custom_bias = 0.0;
Map<ShapeOwner2DSW *, int> owners; Map<GodotShapeOwner2D *, int> owners;
protected: protected:
void configure(const Rect2 &p_aabb); void configure(const Rect2 &p_aabb);
@ -83,10 +83,10 @@ public:
_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; } _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; } _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
void add_owner(ShapeOwner2DSW *p_owner); void add_owner(GodotShapeOwner2D *p_owner);
void remove_owner(ShapeOwner2DSW *p_owner); void remove_owner(GodotShapeOwner2D *p_owner);
bool is_owner(ShapeOwner2DSW *p_owner) const; bool is_owner(GodotShapeOwner2D *p_owner) const;
const Map<ShapeOwner2DSW *, int> &get_owners() const; const Map<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 { _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); get_supports(p_xform.basis_xform_inv(p_normal).normalized(), r_supports, r_amount);
@ -121,8 +121,8 @@ public:
} }
} }
} }
Shape2DSW() {} GodotShape2D() {}
virtual ~Shape2DSW(); virtual ~GodotShape2D();
}; };
//let the optimizer do the magic //let the optimizer do the magic
@ -141,7 +141,7 @@ public:
r_max = MAX(maxa, maxb); \ r_max = MAX(maxa, maxb); \
} }
class WorldBoundaryShape2DSW : public Shape2DSW { class GodotWorldBoundaryShape2D : public GodotShape2D {
Vector2 normal; Vector2 normal;
real_t d = 0.0; real_t d = 0.0;
@ -178,7 +178,7 @@ public:
} }
}; };
class SeparationRayShape2DSW : public Shape2DSW { class GodotSeparationRayShape2D : public GodotShape2D {
real_t length = 0.0; real_t length = 0.0;
bool slide_on_slope = false; bool slide_on_slope = false;
@ -211,11 +211,11 @@ public:
DEFAULT_PROJECT_RANGE_CAST DEFAULT_PROJECT_RANGE_CAST
_FORCE_INLINE_ SeparationRayShape2DSW() {} _FORCE_INLINE_ GodotSeparationRayShape2D() {}
_FORCE_INLINE_ SeparationRayShape2DSW(real_t p_length) { length = p_length; } _FORCE_INLINE_ GodotSeparationRayShape2D(real_t p_length) { length = p_length; }
}; };
class SegmentShape2DSW : public Shape2DSW { class GodotSegmentShape2D : public GodotShape2D {
Vector2 a; Vector2 a;
Vector2 b; Vector2 b;
Vector2 n; Vector2 n;
@ -251,15 +251,15 @@ public:
DEFAULT_PROJECT_RANGE_CAST DEFAULT_PROJECT_RANGE_CAST
_FORCE_INLINE_ SegmentShape2DSW() {} _FORCE_INLINE_ GodotSegmentShape2D() {}
_FORCE_INLINE_ SegmentShape2DSW(const Vector2 &p_a, const Vector2 &p_b, const Vector2 &p_n) { _FORCE_INLINE_ GodotSegmentShape2D(const Vector2 &p_a, const Vector2 &p_b, const Vector2 &p_n) {
a = p_a; a = p_a;
b = p_b; b = p_b;
n = p_n; n = p_n;
} }
}; };
class CircleShape2DSW : public Shape2DSW { class GodotCircleShape2D : public GodotShape2D {
real_t radius; real_t radius;
public: public:
@ -292,7 +292,7 @@ public:
DEFAULT_PROJECT_RANGE_CAST DEFAULT_PROJECT_RANGE_CAST
}; };
class RectangleShape2DSW : public Shape2DSW { class GodotRectangleShape2D : public GodotShape2D {
Vector2 half_extents; Vector2 half_extents;
public: public:
@ -336,7 +336,7 @@ public:
return (p_xform.xform(he) - p_circle).normalized(); return (p_xform.xform(he) - p_circle).normalized();
} }
_FORCE_INLINE_ Vector2 get_box_axis(const Transform2D &p_xform, const Transform2D &p_xform_inv, const RectangleShape2DSW *p_B, const Transform2D &p_B_xform, const Transform2D &p_B_xform_inv) const { _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 a, b;
{ {
@ -364,7 +364,7 @@ public:
DEFAULT_PROJECT_RANGE_CAST DEFAULT_PROJECT_RANGE_CAST
}; };
class CapsuleShape2DSW : public Shape2DSW { class GodotCapsuleShape2D : public GodotShape2D {
real_t radius = 0.0; real_t radius = 0.0;
real_t height = 0.0; real_t height = 0.0;
@ -405,7 +405,7 @@ public:
DEFAULT_PROJECT_RANGE_CAST DEFAULT_PROJECT_RANGE_CAST
}; };
class ConvexPolygonShape2DSW : public Shape2DSW { class GodotConvexPolygonShape2D : public GodotShape2D {
struct Point { struct Point {
Vector2 pos; Vector2 pos;
Vector2 normal; //normal to next segment Vector2 normal; //normal to next segment
@ -457,21 +457,21 @@ public:
DEFAULT_PROJECT_RANGE_CAST DEFAULT_PROJECT_RANGE_CAST
ConvexPolygonShape2DSW() {} GodotConvexPolygonShape2D() {}
~ConvexPolygonShape2DSW(); ~GodotConvexPolygonShape2D();
}; };
class ConcaveShape2DSW : public Shape2DSW { class GodotConcaveShape2D : public GodotShape2D {
public: public:
virtual bool is_concave() const override { return true; } virtual bool is_concave() const override { return true; }
// Returns true to stop the query. // Returns true to stop the query.
typedef bool (*QueryCallback)(void *p_userdata, Shape2DSW *p_convex); 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; virtual void cull(const Rect2 &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0;
}; };
class ConcavePolygonShape2DSW : public ConcaveShape2DSW { class GodotConcavePolygonShape2D : public GodotConcaveShape2D {
struct Segment { struct Segment {
int points[2] = {}; int points[2] = {};
}; };
@ -507,13 +507,13 @@ public:
virtual void project_rangev(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const override { 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_min = 0;
r_max = 0; r_max = 0;
ERR_FAIL_MSG("Unsupported call to project_rangev in ConcavePolygonShape2DSW"); 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 { void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
r_min = 0; r_min = 0;
r_max = 0; r_max = 0;
ERR_FAIL_MSG("Unsupported call to project_range in ConcavePolygonShape2DSW"); 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 void get_supports(const Vector2 &p_normal, Vector2 *r_supports, int &r_amount) const override;
@ -533,4 +533,4 @@ public:
#undef DEFAULT_PROJECT_RANGE_CAST #undef DEFAULT_PROJECT_RANGE_CAST
#endif // SHAPE_2D_2DSW_H #endif // GODOT_SHAPE_2D_H

247
servers/physics_2d/space_2d_sw.cpp → servers/physics_2d/godot_space_2d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* space_2d_sw.cpp */ /* godot_space_2d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,32 +28,33 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "space_2d_sw.h" #include "godot_space_2d.h"
#include "godot_collision_solver_2d.h"
#include "godot_physics_server_2d.h"
#include "collision_solver_2d_sw.h"
#include "core/os/os.h" #include "core/os/os.h"
#include "core/templates/pair.h" #include "core/templates/pair.h"
#include "physics_server_2d_sw.h"
#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 #define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
_FORCE_INLINE_ static bool _can_collide_with(CollisionObject2DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { _FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject2D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
if (!(p_object->get_collision_layer() & p_collision_mask)) { if (!(p_object->get_collision_layer() & p_collision_mask)) {
return false; return false;
} }
if (p_object->get_type() == CollisionObject2DSW::TYPE_AREA && !p_collide_with_areas) { if (p_object->get_type() == GodotCollisionObject2D::TYPE_AREA && !p_collide_with_areas) {
return false; return false;
} }
if (p_object->get_type() == CollisionObject2DSW::TYPE_BODY && !p_collide_with_bodies) { if (p_object->get_type() == GodotCollisionObject2D::TYPE_BODY && !p_collide_with_bodies) {
return false; return false;
} }
return true; return true;
} }
int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas, ObjectID p_canvas_instance_id) { int GodotPhysicsDirectSpaceState2D::_intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas, ObjectID p_canvas_instance_id) {
if (p_result_max <= 0) { if (p_result_max <= 0) {
return 0; return 0;
} }
@ -62,7 +63,7 @@ int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, S
aabb.position = p_point - Vector2(0.00001, 0.00001); aabb.position = p_point - Vector2(0.00001, 0.00001);
aabb.size = Vector2(0.00002, 0.00002); aabb.size = Vector2(0.00002, 0.00002);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
int cc = 0; int cc = 0;
@ -75,7 +76,7 @@ int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, S
continue; continue;
} }
const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
if (p_pick_point && !col_obj->is_pickable()) { if (p_pick_point && !col_obj->is_pickable()) {
continue; continue;
@ -87,7 +88,7 @@ int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, S
int shape_idx = space->intersection_query_subindex_results[i]; int shape_idx = space->intersection_query_subindex_results[i];
Shape2DSW *shape = col_obj->get_shape(shape_idx); GodotShape2D *shape = col_obj->get_shape(shape_idx);
Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point); Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point);
@ -112,15 +113,15 @@ int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, S
return cc; return cc;
} }
int PhysicsDirectSpaceState2DSW::intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { int GodotPhysicsDirectSpaceState2D::intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) {
return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point); return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point);
} }
int PhysicsDirectSpaceState2DSW::intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) { int GodotPhysicsDirectSpaceState2D::intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point) {
return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point, true, p_canvas_instance_id); return _intersect_point_impl(p_point, r_results, p_result_max, p_exclude, p_collision_mask, p_collide_with_bodies, p_collide_with_areas, p_pick_point, true, p_canvas_instance_id);
} }
bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { bool GodotPhysicsDirectSpaceState2D::intersect_ray(const Vector2 &p_from, const Vector2 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
ERR_FAIL_COND_V(space->locked, false); ERR_FAIL_COND_V(space->locked, false);
Vector2 begin, end; Vector2 begin, end;
@ -129,14 +130,14 @@ bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vec
end = p_to; end = p_to;
normal = (end - begin).normalized(); normal = (end - begin).normalized();
int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
//todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision //todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
bool collided = false; bool collided = false;
Vector2 res_point, res_normal; Vector2 res_point, res_normal;
int res_shape; int res_shape;
const CollisionObject2DSW *res_obj; const GodotCollisionObject2D *res_obj;
real_t min_d = 1e10; real_t min_d = 1e10;
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
@ -148,7 +149,7 @@ bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vec
continue; continue;
} }
const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i]; int shape_idx = space->intersection_query_subindex_results[i];
Transform2D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); Transform2D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
@ -162,7 +163,7 @@ bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vec
local_to = col_obj->get_inv_transform().xform(end); local_to = col_obj->get_inv_transform().xform(end);
local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/ local_to = col_obj->get_shape_inv_transform(shape_idx).xform(local_to);*/
const Shape2DSW *shape = col_obj->get_shape(shape_idx); const GodotShape2D *shape = col_obj->get_shape(shape_idx);
Vector2 shape_point, shape_normal; Vector2 shape_point, shape_normal;
@ -199,18 +200,18 @@ bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vec
return true; return true;
} }
int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { int GodotPhysicsDirectSpaceState2D::intersect_shape(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
if (p_result_max <= 0) { if (p_result_max <= 0) {
return 0; return 0;
} }
Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape); GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0); ERR_FAIL_COND_V(!shape, 0);
Rect2 aabb = p_xform.xform(shape->get_aabb()); Rect2 aabb = p_xform.xform(shape->get_aabb());
aabb = aabb.grow(p_margin); aabb = aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
int cc = 0; int cc = 0;
@ -227,10 +228,10 @@ int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Trans
continue; continue;
} }
const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i]; int shape_idx = space->intersection_query_subindex_results[i];
if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_margin)) { if (!GodotCollisionSolver2D::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_margin)) {
continue; continue;
} }
@ -247,15 +248,15 @@ int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Trans
return cc; return cc;
} }
bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { bool GodotPhysicsDirectSpaceState2D::cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape); GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, false); ERR_FAIL_COND_V(!shape, false);
Rect2 aabb = p_xform.xform(shape->get_aabb()); Rect2 aabb = p_xform.xform(shape->get_aabb());
aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion
aabb = aabb.grow(p_margin); aabb = aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
real_t best_safe = 1; real_t best_safe = 1;
real_t best_unsafe = 1; real_t best_unsafe = 1;
@ -269,17 +270,17 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor
continue; //ignore excluded continue; //ignore excluded
} }
const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i]; int shape_idx = space->intersection_query_subindex_results[i];
Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way? //test initial overlap, does it collide if going all the way?
if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { if (!GodotCollisionSolver2D::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) {
continue; continue;
} }
//test initial overlap, ignore objects it's inside of. //test initial overlap, ignore objects it's inside of.
if (CollisionSolver2DSW::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) { if (GodotCollisionSolver2D::solve(shape, p_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) {
continue; continue;
} }
@ -293,7 +294,7 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor
real_t fraction = low + (hi - low) * fraction_coeff; real_t fraction = low + (hi - low) * fraction_coeff;
Vector2 sep = mnormal; //important optimization for this to work fast enough Vector2 sep = mnormal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin); bool collided = GodotCollisionSolver2D::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin);
if (collided) { if (collided) {
hi = fraction; hi = fraction;
@ -330,38 +331,38 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor
return true; return true;
} }
bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { bool GodotPhysicsDirectSpaceState2D::collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
if (p_result_max <= 0) { if (p_result_max <= 0) {
return false; return false;
} }
Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape); GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0); ERR_FAIL_COND_V(!shape, 0);
Rect2 aabb = p_shape_xform.xform(shape->get_aabb()); Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion
aabb = aabb.grow(p_margin); aabb = aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
bool collided = false; bool collided = false;
r_result_count = 0; r_result_count = 0;
PhysicsServer2DSW::CollCbkData cbk; GodotPhysicsServer2D::CollCbkData cbk;
cbk.max = p_result_max; cbk.max = p_result_max;
cbk.amount = 0; cbk.amount = 0;
cbk.passed = 0; cbk.passed = 0;
cbk.ptr = r_results; cbk.ptr = r_results;
CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk;
PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk;
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
continue; continue;
} }
const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
if (p_exclude.has(col_obj->get_self())) { if (p_exclude.has(col_obj->get_self())) {
continue; continue;
@ -372,7 +373,7 @@ bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D &
cbk.valid_dir = Vector2(); cbk.valid_dir = Vector2();
cbk.valid_depth = 0; cbk.valid_depth = 0;
if (CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_margin)) { if (GodotCollisionSolver2D::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, nullptr, p_margin)) {
collided = cbk.amount > 0; collided = cbk.amount > 0;
} }
} }
@ -383,8 +384,8 @@ bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D &
} }
struct _RestCallbackData2D { struct _RestCallbackData2D {
const CollisionObject2DSW *object = nullptr; const GodotCollisionObject2D *object = nullptr;
const CollisionObject2DSW *best_object = nullptr; const GodotCollisionObject2D *best_object = nullptr;
int local_shape = 0; int local_shape = 0;
int best_local_shape = 0; int best_local_shape = 0;
int shape = 0; int shape = 0;
@ -431,8 +432,8 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B,
rd->best_local_shape = rd->local_shape; rd->best_local_shape = rd->local_shape;
} }
bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { bool GodotPhysicsDirectSpaceState2D::rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape); GodotShape2D *shape = GodotPhysicsServer2D::godot_singleton->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0); ERR_FAIL_COND_V(!shape, 0);
real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
@ -441,7 +442,7 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion
aabb = aabb.grow(p_margin); aabb = aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space2DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace2D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
_RestCallbackData2D rcd; _RestCallbackData2D rcd;
rcd.best_len = 0; rcd.best_len = 0;
@ -454,7 +455,7 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
continue; continue;
} }
const CollisionObject2DSW *col_obj = space->intersection_query_results[i]; const GodotCollisionObject2D *col_obj = space->intersection_query_results[i];
if (p_exclude.has(col_obj->get_self())) { if (p_exclude.has(col_obj->get_self())) {
continue; continue;
@ -466,7 +467,7 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
rcd.object = col_obj; rcd.object = col_obj;
rcd.shape = shape_idx; rcd.shape = shape_idx;
rcd.local_shape = 0; rcd.local_shape = 0;
bool sc = CollisionSolver2DSW::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, p_margin); bool sc = GodotCollisionSolver2D::solve(shape, p_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, nullptr, p_margin);
if (!sc) { if (!sc) {
continue; continue;
} }
@ -481,8 +482,8 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
r_info->normal = rcd.best_normal; r_info->normal = rcd.best_normal;
r_info->point = rcd.best_contact; r_info->point = rcd.best_contact;
r_info->rid = rcd.best_object->get_self(); r_info->rid = rcd.best_object->get_self();
if (rcd.best_object->get_type() == CollisionObject2DSW::TYPE_BODY) { if (rcd.best_object->get_type() == GodotCollisionObject2D::TYPE_BODY) {
const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object); const GodotBody2D *body = static_cast<const GodotBody2D *>(rcd.best_object);
Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass()); Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass());
r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); r_info->linear_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
@ -495,7 +496,7 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
//////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////
int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) { int GodotSpace2D::_cull_aabb_for_body(GodotBody2D *p_body, const Rect2 &p_aabb) {
int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results);
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
@ -503,11 +504,11 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) {
if (intersection_query_results[i] == p_body) { if (intersection_query_results[i] == p_body) {
keep = false; keep = false;
} else if (intersection_query_results[i]->get_type() == CollisionObject2DSW::TYPE_AREA) { } else if (intersection_query_results[i]->get_type() == GodotCollisionObject2D::TYPE_AREA) {
keep = false; keep = false;
} else if (!p_body->collides_with(static_cast<Body2DSW *>(intersection_query_results[i]))) { } else if (!p_body->collides_with(static_cast<GodotBody2D *>(intersection_query_results[i]))) {
keep = false; keep = false;
} else if (static_cast<Body2DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { } else if (static_cast<GodotBody2D *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) {
keep = false; keep = false;
} }
@ -525,7 +526,7 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) {
return amount; return amount;
} }
bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result) { bool GodotSpace2D::test_body_motion(GodotBody2D *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result) {
//give me back regular physics engine logic //give me back regular physics engine logic
//this is madness //this is madness
//and most people using this function will think //and most people using this function will think
@ -587,7 +588,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
Vector2 sr[max_results * 2]; Vector2 sr[max_results * 2];
do { do {
PhysicsServer2DSW::CollCbkData cbk; GodotPhysicsServer2D::CollCbkData cbk;
cbk.max = max_results; cbk.max = max_results;
cbk.amount = 0; cbk.amount = 0;
cbk.passed = 0; cbk.passed = 0;
@ -595,8 +596,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
cbk.invalid_by_dir = 0; cbk.invalid_by_dir = 0;
excluded_shape_pair_count = 0; //last step is the one valid excluded_shape_pair_count = 0; //last step is the one valid
PhysicsServer2DSW::CollCbkData *cbkptr = &cbk; GodotPhysicsServer2D::CollCbkData *cbkptr = &cbk;
CollisionSolver2DSW::CallbackResult cbkres = PhysicsServer2DSW::_shape_col_cbk; GodotCollisionSolver2D::CallbackResult cbkres = GodotPhysicsServer2D::_shape_col_cbk;
bool collided = false; bool collided = false;
@ -607,11 +608,11 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
continue; continue;
} }
Shape2DSW *body_shape = p_body->get_shape(j); GodotShape2D *body_shape = p_body->get_shape(j);
Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j); Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i]; const GodotCollisionObject2D *col_obj = intersection_query_results[i];
if (p_parameters.exclude_bodies.has(col_obj->get_self())) { if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
continue; continue;
} }
@ -630,8 +631,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
cbk.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work cbk.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work
cbk.invalid_by_dir = 0; cbk.invalid_by_dir = 0;
if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj);
if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) {
//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
Vector2 lv = b->get_linear_velocity(); Vector2 lv = b->get_linear_velocity();
@ -651,8 +652,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
int current_passed = cbk.passed; //save how many points passed collision int current_passed = cbk.passed; //save how many points passed collision
bool did_collide = false; bool did_collide = false;
Shape2DSW *against_shape = col_obj->get_shape(shape_idx); GodotShape2D *against_shape = col_obj->get_shape(shape_idx);
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) { if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, nullptr, p_parameters.margin)) {
did_collide = cbk.passed > current_passed; //more passed, so collision actually existed did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
} }
@ -727,13 +728,13 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
continue; continue;
} }
Shape2DSW *body_shape = p_body->get_shape(body_shape_idx); GodotShape2D *body_shape = p_body->get_shape(body_shape_idx);
// Colliding separation rays allows to properly snap to the ground, // Colliding separation rays allows to properly snap to the ground,
// otherwise it's not needed in regular motion. // otherwise it's not needed in regular motion.
if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) { if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer2D::SHAPE_SEPARATION_RAY)) {
// When slide on slope is on, separation ray shape acts like a regular shape. // When slide on slope is on, separation ray shape acts like a regular shape.
if (!static_cast<SeparationRayShape2DSW *>(body_shape)->get_slide_on_slope()) { if (!static_cast<GodotSeparationRayShape2D *>(body_shape)->get_slide_on_slope()) {
continue; continue;
} }
} }
@ -746,7 +747,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
real_t best_unsafe = 1; real_t best_unsafe = 1;
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i]; const GodotCollisionObject2D *col_obj = intersection_query_results[i];
if (p_parameters.exclude_bodies.has(col_obj->get_self())) { if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
continue; continue;
} }
@ -755,7 +756,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
} }
int col_shape_idx = intersection_query_subindex_results[i]; int col_shape_idx = intersection_query_subindex_results[i];
Shape2DSW *against_shape = col_obj->get_shape(col_shape_idx); GodotShape2D *against_shape = col_obj->get_shape(col_shape_idx);
bool excluded = false; bool excluded = false;
@ -772,12 +773,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx); Transform2D col_obj_shape_xform = col_obj->get_transform() * col_obj->get_shape_transform(col_shape_idx);
//test initial overlap, does it collide if going all the way? //test initial overlap, does it collide if going all the way?
if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_parameters.motion, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { if (!GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) {
continue; continue;
} }
//test initial overlap //test initial overlap
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) { if (GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, nullptr, 0)) {
if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
Vector2 direction = col_obj_shape_xform.get_axis(1).normalized(); Vector2 direction = col_obj_shape_xform.get_axis(1).normalized();
if (motion_normal.dot(direction) < 0) { if (motion_normal.dot(direction) < 0) {
@ -797,7 +798,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
real_t fraction = low + (hi - low) * fraction_coeff; real_t fraction = low + (hi - low) * fraction_coeff;
Vector2 sep = motion_normal; //important optimization for this to work fast enough Vector2 sep = motion_normal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_parameters.motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0); bool collided = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0);
if (collided) { if (collided) {
hi = fraction; hi = fraction;
@ -824,7 +825,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) { if (body_shape->allows_one_way_collision() && col_obj->is_shape_set_as_one_way_collision(col_shape_idx)) {
Vector2 cd[2]; Vector2 cd[2];
PhysicsServer2DSW::CollCbkData cbk; GodotPhysicsServer2D::CollCbkData cbk;
cbk.max = 1; cbk.max = 1;
cbk.amount = 0; cbk.amount = 0;
cbk.passed = 0; cbk.passed = 0;
@ -834,7 +835,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
cbk.valid_depth = 10e20; cbk.valid_depth = 10e20;
Vector2 sep = motion_normal; //important optimization for this to work fast enough Vector2 sep = motion_normal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_parameters.motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), PhysicsServer2DSW::_shape_col_cbk, &cbk, &sep, 0); bool collided = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, p_parameters.motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(col_shape_idx), col_obj_shape_xform, Vector2(), GodotPhysicsServer2D::_shape_col_cbk, &cbk, &sep, 0);
if (!collided || cbk.amount == 0) { if (!collided || cbk.amount == 0) {
continue; continue;
} }
@ -891,14 +892,14 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
} }
Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j); Transform2D body_shape_xform = ugt * p_body->get_shape_transform(j);
Shape2DSW *body_shape = p_body->get_shape(j); GodotShape2D *body_shape = p_body->get_shape(j);
body_aabb.position += p_parameters.motion * unsafe; body_aabb.position += p_parameters.motion * unsafe;
int amount = _cull_aabb_for_body(p_body, body_aabb); int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i]; const GodotCollisionObject2D *col_obj = intersection_query_results[i];
if (p_parameters.exclude_bodies.has(col_obj->get_self())) { if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
continue; continue;
} }
@ -908,7 +909,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
int shape_idx = intersection_query_subindex_results[i]; int shape_idx = intersection_query_subindex_results[i];
Shape2DSW *against_shape = col_obj->get_shape(shape_idx); GodotShape2D *against_shape = col_obj->get_shape(shape_idx);
bool excluded = false; bool excluded = false;
for (int k = 0; k < excluded_shape_pair_count; k++) { for (int k = 0; k < excluded_shape_pair_count; k++) {
@ -929,8 +930,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx); real_t owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
rcd.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work rcd.valid_depth = MAX(owc_margin, p_parameters.margin); //user specified, but never less than actual margin or it won't work
if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) { if (col_obj->get_type() == GodotCollisionObject2D::TYPE_BODY) {
const Body2DSW *b = static_cast<const Body2DSW *>(col_obj); const GodotBody2D *b = static_cast<const GodotBody2D *>(col_obj);
if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) { if (b->get_mode() == PhysicsServer2D::BODY_MODE_KINEMATIC || b->get_mode() == PhysicsServer2D::BODY_MODE_DYNAMIC) {
//fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction //fix for moving platforms (kinematic and dynamic), margin is increased by how much it moved in the given direction
Vector2 lv = b->get_linear_velocity(); Vector2 lv = b->get_linear_velocity();
@ -949,7 +950,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
rcd.object = col_obj; rcd.object = col_obj;
rcd.shape = shape_idx; rcd.shape = shape_idx;
rcd.local_shape = j; rcd.local_shape = j;
bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, p_parameters.margin); bool sc = GodotCollisionSolver2D::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), _rest_cbk_result, &rcd, nullptr, p_parameters.margin);
if (!sc) { if (!sc) {
continue; continue;
} }
@ -968,7 +969,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
r_result->collision_safe_fraction = safe; r_result->collision_safe_fraction = safe;
r_result->collision_unsafe_fraction = unsafe; r_result->collision_unsafe_fraction = unsafe;
const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object); const GodotBody2D *body = static_cast<const GodotBody2D *>(rcd.best_object);
Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass()); Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass());
r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity(); r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
@ -990,134 +991,134 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const PhysicsServer2D::Motion
return collided; return collided;
} }
void *Space2DSW::_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self) { void *GodotSpace2D::_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self) {
if (!A->interacts_with(B)) { if (!A->interacts_with(B)) {
return nullptr; return nullptr;
} }
CollisionObject2DSW::Type type_A = A->get_type(); GodotCollisionObject2D::Type type_A = A->get_type();
CollisionObject2DSW::Type type_B = B->get_type(); GodotCollisionObject2D::Type type_B = B->get_type();
if (type_A > type_B) { if (type_A > type_B) {
SWAP(A, B); SWAP(A, B);
SWAP(p_subindex_A, p_subindex_B); SWAP(p_subindex_A, p_subindex_B);
SWAP(type_A, type_B); SWAP(type_A, type_B);
} }
Space2DSW *self = (Space2DSW *)p_self; GodotSpace2D *self = (GodotSpace2D *)p_self;
self->collision_pairs++; self->collision_pairs++;
if (type_A == CollisionObject2DSW::TYPE_AREA) { if (type_A == GodotCollisionObject2D::TYPE_AREA) {
Area2DSW *area = static_cast<Area2DSW *>(A); GodotArea2D *area = static_cast<GodotArea2D *>(A);
if (type_B == CollisionObject2DSW::TYPE_AREA) { if (type_B == GodotCollisionObject2D::TYPE_AREA) {
Area2DSW *area_b = static_cast<Area2DSW *>(B); GodotArea2D *area_b = static_cast<GodotArea2D *>(B);
Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b, p_subindex_B, area, p_subindex_A)); GodotArea2Pair2D *area2_pair = memnew(GodotArea2Pair2D(area_b, p_subindex_B, area, p_subindex_A));
return area2_pair; return area2_pair;
} else { } else {
Body2DSW *body = static_cast<Body2DSW *>(B); GodotBody2D *body = static_cast<GodotBody2D *>(B);
AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body, p_subindex_B, area, p_subindex_A)); GodotAreaPair2D *area_pair = memnew(GodotAreaPair2D(body, p_subindex_B, area, p_subindex_A));
return area_pair; return area_pair;
} }
} else { } else {
BodyPair2DSW *b = memnew(BodyPair2DSW((Body2DSW *)A, p_subindex_A, (Body2DSW *)B, p_subindex_B)); GodotBodyPair2D *b = memnew(GodotBodyPair2D((GodotBody2D *)A, p_subindex_A, (GodotBody2D *)B, p_subindex_B));
return b; return b;
} }
return nullptr; return nullptr;
} }
void Space2DSW::_broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, void *p_self) { void GodotSpace2D::_broadphase_unpair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_data, void *p_self) {
if (!p_data) { if (!p_data) {
return; return;
} }
Space2DSW *self = (Space2DSW *)p_self; GodotSpace2D *self = (GodotSpace2D *)p_self;
self->collision_pairs--; self->collision_pairs--;
Constraint2DSW *c = (Constraint2DSW *)p_data; GodotConstraint2D *c = (GodotConstraint2D *)p_data;
memdelete(c); memdelete(c);
} }
const SelfList<Body2DSW>::List &Space2DSW::get_active_body_list() const { const SelfList<GodotBody2D>::List &GodotSpace2D::get_active_body_list() const {
return active_list; return active_list;
} }
void Space2DSW::body_add_to_active_list(SelfList<Body2DSW> *p_body) { void GodotSpace2D::body_add_to_active_list(SelfList<GodotBody2D> *p_body) {
active_list.add(p_body); active_list.add(p_body);
} }
void Space2DSW::body_remove_from_active_list(SelfList<Body2DSW> *p_body) { void GodotSpace2D::body_remove_from_active_list(SelfList<GodotBody2D> *p_body) {
active_list.remove(p_body); active_list.remove(p_body);
} }
void Space2DSW::body_add_to_mass_properties_update_list(SelfList<Body2DSW> *p_body) { void GodotSpace2D::body_add_to_mass_properties_update_list(SelfList<GodotBody2D> *p_body) {
mass_properties_update_list.add(p_body); mass_properties_update_list.add(p_body);
} }
void Space2DSW::body_remove_from_mass_properties_update_list(SelfList<Body2DSW> *p_body) { void GodotSpace2D::body_remove_from_mass_properties_update_list(SelfList<GodotBody2D> *p_body) {
mass_properties_update_list.remove(p_body); mass_properties_update_list.remove(p_body);
} }
BroadPhase2DSW *Space2DSW::get_broadphase() { GodotBroadPhase2D *GodotSpace2D::get_broadphase() {
return broadphase; return broadphase;
} }
void Space2DSW::add_object(CollisionObject2DSW *p_object) { void GodotSpace2D::add_object(GodotCollisionObject2D *p_object) {
ERR_FAIL_COND(objects.has(p_object)); ERR_FAIL_COND(objects.has(p_object));
objects.insert(p_object); objects.insert(p_object);
} }
void Space2DSW::remove_object(CollisionObject2DSW *p_object) { void GodotSpace2D::remove_object(GodotCollisionObject2D *p_object) {
ERR_FAIL_COND(!objects.has(p_object)); ERR_FAIL_COND(!objects.has(p_object));
objects.erase(p_object); objects.erase(p_object);
} }
const Set<CollisionObject2DSW *> &Space2DSW::get_objects() const { const Set<GodotCollisionObject2D *> &GodotSpace2D::get_objects() const {
return objects; return objects;
} }
void Space2DSW::body_add_to_state_query_list(SelfList<Body2DSW> *p_body) { void GodotSpace2D::body_add_to_state_query_list(SelfList<GodotBody2D> *p_body) {
state_query_list.add(p_body); state_query_list.add(p_body);
} }
void Space2DSW::body_remove_from_state_query_list(SelfList<Body2DSW> *p_body) { void GodotSpace2D::body_remove_from_state_query_list(SelfList<GodotBody2D> *p_body) {
state_query_list.remove(p_body); state_query_list.remove(p_body);
} }
void Space2DSW::area_add_to_monitor_query_list(SelfList<Area2DSW> *p_area) { void GodotSpace2D::area_add_to_monitor_query_list(SelfList<GodotArea2D> *p_area) {
monitor_query_list.add(p_area); monitor_query_list.add(p_area);
} }
void Space2DSW::area_remove_from_monitor_query_list(SelfList<Area2DSW> *p_area) { void GodotSpace2D::area_remove_from_monitor_query_list(SelfList<GodotArea2D> *p_area) {
monitor_query_list.remove(p_area); monitor_query_list.remove(p_area);
} }
void Space2DSW::area_add_to_moved_list(SelfList<Area2DSW> *p_area) { void GodotSpace2D::area_add_to_moved_list(SelfList<GodotArea2D> *p_area) {
area_moved_list.add(p_area); area_moved_list.add(p_area);
} }
void Space2DSW::area_remove_from_moved_list(SelfList<Area2DSW> *p_area) { void GodotSpace2D::area_remove_from_moved_list(SelfList<GodotArea2D> *p_area) {
area_moved_list.remove(p_area); area_moved_list.remove(p_area);
} }
const SelfList<Area2DSW>::List &Space2DSW::get_moved_area_list() const { const SelfList<GodotArea2D>::List &GodotSpace2D::get_moved_area_list() const {
return area_moved_list; return area_moved_list;
} }
void Space2DSW::call_queries() { void GodotSpace2D::call_queries() {
while (state_query_list.first()) { while (state_query_list.first()) {
Body2DSW *b = state_query_list.first()->self(); GodotBody2D *b = state_query_list.first()->self();
state_query_list.remove(state_query_list.first()); state_query_list.remove(state_query_list.first());
b->call_queries(); b->call_queries();
} }
while (monitor_query_list.first()) { while (monitor_query_list.first()) {
Area2DSW *a = monitor_query_list.first()->self(); GodotArea2D *a = monitor_query_list.first()->self();
monitor_query_list.remove(monitor_query_list.first()); monitor_query_list.remove(monitor_query_list.first());
a->call_queries(); a->call_queries();
} }
} }
void Space2DSW::setup() { void GodotSpace2D::setup() {
contact_debug_count = 0; contact_debug_count = 0;
while (mass_properties_update_list.first()) { while (mass_properties_update_list.first()) {
@ -1126,11 +1127,11 @@ void Space2DSW::setup() {
} }
} }
void Space2DSW::update() { void GodotSpace2D::update() {
broadphase->update(); broadphase->update();
} }
void Space2DSW::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value) { void GodotSpace2D::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value) {
switch (p_param) { switch (p_param) {
case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
contact_recycle_radius = p_value; contact_recycle_radius = p_value;
@ -1156,7 +1157,7 @@ void Space2DSW::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_valu
} }
} }
real_t Space2DSW::get_param(PhysicsServer2D::SpaceParameter p_param) const { real_t GodotSpace2D::get_param(PhysicsServer2D::SpaceParameter p_param) const {
switch (p_param) { switch (p_param) {
case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: case PhysicsServer2D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
return contact_recycle_radius; return contact_recycle_radius;
@ -1176,37 +1177,37 @@ real_t Space2DSW::get_param(PhysicsServer2D::SpaceParameter p_param) const {
return 0; return 0;
} }
void Space2DSW::lock() { void GodotSpace2D::lock() {
locked = true; locked = true;
} }
void Space2DSW::unlock() { void GodotSpace2D::unlock() {
locked = false; locked = false;
} }
bool Space2DSW::is_locked() const { bool GodotSpace2D::is_locked() const {
return locked; return locked;
} }
PhysicsDirectSpaceState2DSW *Space2DSW::get_direct_state() { GodotPhysicsDirectSpaceState2D *GodotSpace2D::get_direct_state() {
return direct_access; return direct_access;
} }
Space2DSW::Space2DSW() { GodotSpace2D::GodotSpace2D() {
body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0); body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_linear", 2.0);
body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", Math::deg2rad(8.0)); body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/2d/sleep_threshold_angular", Math::deg2rad(8.0));
body_time_to_sleep = GLOBAL_DEF("physics/2d/time_before_sleep", 0.5); body_time_to_sleep = GLOBAL_DEF("physics/2d/time_before_sleep", 0.5);
ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/2d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); ProjectSettings::get_singleton()->set_custom_property_info("physics/2d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/2d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater"));
broadphase = BroadPhase2DSW::create_func(); broadphase = GodotBroadPhase2D::create_func();
broadphase->set_pair_callback(_broadphase_pair, this); broadphase->set_pair_callback(_broadphase_pair, this);
broadphase->set_unpair_callback(_broadphase_unpair, this); broadphase->set_unpair_callback(_broadphase_unpair, this);
direct_access = memnew(PhysicsDirectSpaceState2DSW); direct_access = memnew(GodotPhysicsDirectSpaceState2D);
direct_access->space = this; direct_access->space = this;
} }
Space2DSW::~Space2DSW() { GodotSpace2D::~GodotSpace2D() {
memdelete(broadphase); memdelete(broadphase);
memdelete(direct_access); memdelete(direct_access);
} }

107
servers/physics_2d/space_2d_sw.h → servers/physics_2d/godot_space_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* space_2d_sw.h */ /* godot_space_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,26 +28,27 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef SPACE_2D_SW_H #ifndef GODOT_SPACE_2D_H
#define SPACE_2D_SW_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 "area_2d_sw.h"
#include "area_pair_2d_sw.h"
#include "body_2d_sw.h"
#include "body_pair_2d_sw.h"
#include "broad_phase_2d_sw.h"
#include "collision_object_2d_sw.h"
#include "core/config/project_settings.h" #include "core/config/project_settings.h"
#include "core/templates/hash_map.h" #include "core/templates/hash_map.h"
#include "core/typedefs.h" #include "core/typedefs.h"
class PhysicsDirectSpaceState2DSW : public PhysicsDirectSpaceState2D { class GodotPhysicsDirectSpaceState2D : public PhysicsDirectSpaceState2D {
GDCLASS(PhysicsDirectSpaceState2DSW, PhysicsDirectSpaceState2D); GDCLASS(GodotPhysicsDirectSpaceState2D, PhysicsDirectSpaceState2D);
int _intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas = false, ObjectID p_canvas_instance_id = ObjectID()); int _intersect_point_impl(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_point, bool p_filter_by_canvas = false, ObjectID p_canvas_instance_id = ObjectID());
public: public:
Space2DSW *space = nullptr; GodotSpace2D *space = nullptr;
virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; virtual int intersect_point(const Vector2 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override;
virtual int intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override; virtual int intersect_point_on_canvas(const Vector2 &p_point, ObjectID p_canvas_instance_id, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_point = false) override;
@ -57,10 +58,10 @@ public:
virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; virtual bool collide_shape(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, Vector2 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; virtual bool rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
PhysicsDirectSpaceState2DSW() {} GodotPhysicsDirectSpaceState2D() {}
}; };
class Space2DSW { class GodotSpace2D {
public: public:
enum ElapsedTime { enum ElapsedTime {
ELAPSED_TIME_INTEGRATE_FORCES, ELAPSED_TIME_INTEGRATE_FORCES,
@ -74,29 +75,29 @@ public:
private: private:
struct ExcludedShapeSW { struct ExcludedShapeSW {
Shape2DSW *local_shape = nullptr; GodotShape2D *local_shape = nullptr;
const CollisionObject2DSW *against_object = nullptr; const GodotCollisionObject2D *against_object = nullptr;
int against_shape_index = 0; int against_shape_index = 0;
}; };
uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; uint64_t elapsed_time[ELAPSED_TIME_MAX] = {};
PhysicsDirectSpaceState2DSW *direct_access = nullptr; GodotPhysicsDirectSpaceState2D *direct_access = nullptr;
RID self; RID self;
BroadPhase2DSW *broadphase; GodotBroadPhase2D *broadphase;
SelfList<Body2DSW>::List active_list; SelfList<GodotBody2D>::List active_list;
SelfList<Body2DSW>::List mass_properties_update_list; SelfList<GodotBody2D>::List mass_properties_update_list;
SelfList<Body2DSW>::List state_query_list; SelfList<GodotBody2D>::List state_query_list;
SelfList<Area2DSW>::List monitor_query_list; SelfList<GodotArea2D>::List monitor_query_list;
SelfList<Area2DSW>::List area_moved_list; SelfList<GodotArea2D>::List area_moved_list;
static void *_broadphase_pair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_self); static void *_broadphase_pair(GodotCollisionObject2D *A, int p_subindex_A, GodotCollisionObject2D *B, int p_subindex_B, void *p_self);
static void _broadphase_unpair(CollisionObject2DSW *A, int p_subindex_A, CollisionObject2DSW *B, int p_subindex_B, void *p_data, 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);
Set<CollisionObject2DSW *> objects; Set<GodotCollisionObject2D *> objects;
Area2DSW *area = nullptr; GodotArea2D *area = nullptr;
real_t contact_recycle_radius = 1.0; real_t contact_recycle_radius = 1.0;
real_t contact_max_separation = 1.5; real_t contact_max_separation = 1.5;
@ -107,7 +108,7 @@ private:
INTERSECTION_QUERY_MAX = 2048 INTERSECTION_QUERY_MAX = 2048
}; };
CollisionObject2DSW *intersection_query_results[INTERSECTION_QUERY_MAX]; GodotCollisionObject2D *intersection_query_results[INTERSECTION_QUERY_MAX];
int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];
real_t body_linear_velocity_sleep_threshold = 0.0; real_t body_linear_velocity_sleep_threshold = 0.0;
@ -122,40 +123,40 @@ private:
int active_objects = 0; int active_objects = 0;
int collision_pairs = 0; int collision_pairs = 0;
int _cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb); int _cull_aabb_for_body(GodotBody2D *p_body, const Rect2 &p_aabb);
Vector<Vector2> contact_debug; Vector<Vector2> contact_debug;
int contact_debug_count = 0; int contact_debug_count = 0;
friend class PhysicsDirectSpaceState2DSW; friend class GodotPhysicsDirectSpaceState2D;
public: public:
_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
_FORCE_INLINE_ RID get_self() const { return self; } _FORCE_INLINE_ RID get_self() const { return self; }
void set_default_area(Area2DSW *p_area) { area = p_area; } void set_default_area(GodotArea2D *p_area) { area = p_area; }
Area2DSW *get_default_area() const { return area; } GodotArea2D *get_default_area() const { return area; }
const SelfList<Body2DSW>::List &get_active_body_list() const; const SelfList<GodotBody2D>::List &get_active_body_list() const;
void body_add_to_active_list(SelfList<Body2DSW> *p_body); void body_add_to_active_list(SelfList<GodotBody2D> *p_body);
void body_remove_from_active_list(SelfList<Body2DSW> *p_body); void body_remove_from_active_list(SelfList<GodotBody2D> *p_body);
void body_add_to_mass_properties_update_list(SelfList<Body2DSW> *p_body); void body_add_to_mass_properties_update_list(SelfList<GodotBody2D> *p_body);
void body_remove_from_mass_properties_update_list(SelfList<Body2DSW> *p_body); void body_remove_from_mass_properties_update_list(SelfList<GodotBody2D> *p_body);
void area_add_to_moved_list(SelfList<Area2DSW> *p_area); void area_add_to_moved_list(SelfList<GodotArea2D> *p_area);
void area_remove_from_moved_list(SelfList<Area2DSW> *p_area); void area_remove_from_moved_list(SelfList<GodotArea2D> *p_area);
const SelfList<Area2DSW>::List &get_moved_area_list() const; const SelfList<GodotArea2D>::List &get_moved_area_list() const;
void body_add_to_state_query_list(SelfList<Body2DSW> *p_body); void body_add_to_state_query_list(SelfList<GodotBody2D> *p_body);
void body_remove_from_state_query_list(SelfList<Body2DSW> *p_body); void body_remove_from_state_query_list(SelfList<GodotBody2D> *p_body);
void area_add_to_monitor_query_list(SelfList<Area2DSW> *p_area); void area_add_to_monitor_query_list(SelfList<GodotArea2D> *p_area);
void area_remove_from_monitor_query_list(SelfList<Area2DSW> *p_area); void area_remove_from_monitor_query_list(SelfList<GodotArea2D> *p_area);
BroadPhase2DSW *get_broadphase(); GodotBroadPhase2D *get_broadphase();
void add_object(CollisionObject2DSW *p_object); void add_object(GodotCollisionObject2D *p_object);
void remove_object(CollisionObject2DSW *p_object); void remove_object(GodotCollisionObject2D *p_object);
const Set<CollisionObject2DSW *> &get_objects() const; const Set<GodotCollisionObject2D *> &get_objects() const;
_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } _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_separation() const { return contact_max_separation; }
@ -187,7 +188,7 @@ public:
int get_collision_pairs() const { return collision_pairs; } int get_collision_pairs() const { return collision_pairs; }
bool test_body_motion(Body2DSW *p_body, const PhysicsServer2D::MotionParameters &p_parameters, PhysicsServer2D::MotionResult *r_result); 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); } 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_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); }
@ -199,13 +200,13 @@ public:
_FORCE_INLINE_ Vector<Vector2> get_debug_contacts() { return contact_debug; } _FORCE_INLINE_ Vector<Vector2> get_debug_contacts() { return contact_debug; }
_FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; } _FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; }
PhysicsDirectSpaceState2DSW *get_direct_state(); GodotPhysicsDirectSpaceState2D *get_direct_state();
void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } 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]; } uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; }
Space2DSW(); GodotSpace2D();
~Space2DSW(); ~GodotSpace2D();
}; };
#endif // SPACE_2D_SW_H #endif // GODOT_SPACE_2D_H

72
servers/physics_2d/step_2d_sw.cpp → servers/physics_2d/godot_step_2d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* step_2d_sw.cpp */ /* godot_step_2d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,7 +28,7 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "step_2d_sw.h" #include "godot_step_2d.h"
#include "core/os/os.h" #include "core/os/os.h"
@ -38,7 +38,7 @@
#define ISLAND_SIZE_RESERVE 512 #define ISLAND_SIZE_RESERVE 512
#define CONSTRAINT_COUNT_RESERVE 1024 #define CONSTRAINT_COUNT_RESERVE 1024
void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island) { void GodotStep2D::_populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D *> &p_body_island, LocalVector<GodotConstraint2D *> &p_constraint_island) {
p_body->set_island_step(_step); p_body->set_island_step(_step);
if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) { if (p_body->get_mode() > PhysicsServer2D::BODY_MODE_KINEMATIC) {
@ -46,8 +46,8 @@ void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_bod
p_body_island.push_back(p_body); p_body_island.push_back(p_body);
} }
for (const Pair<Constraint2DSW *, int> &E : p_body->get_constraint_list()) { for (const Pair<GodotConstraint2D *, int> &E : p_body->get_constraint_list()) {
Constraint2DSW *constraint = (Constraint2DSW *)E.first; GodotConstraint2D *constraint = (GodotConstraint2D *)E.first;
if (constraint->get_island_step() == _step) { if (constraint->get_island_step() == _step) {
continue; // Already processed. continue; // Already processed.
} }
@ -59,7 +59,7 @@ void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_bod
if (i == E.second) { if (i == E.second) {
continue; continue;
} }
Body2DSW *other_body = constraint->get_body_ptr()[i]; GodotBody2D *other_body = constraint->get_body_ptr()[i];
if (other_body->get_island_step() == _step) { if (other_body->get_island_step() == _step) {
continue; // Already processed. continue; // Already processed.
} }
@ -71,16 +71,16 @@ void Step2DSW::_populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_bod
} }
} }
void Step2DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { void GodotStep2D::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) {
Constraint2DSW *constraint = all_constraints[p_constraint_index]; GodotConstraint2D *constraint = all_constraints[p_constraint_index];
constraint->setup(delta); constraint->setup(delta);
} }
void Step2DSW::_pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const { void GodotStep2D::_pre_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const {
uint32_t constraint_count = p_constraint_island.size(); uint32_t constraint_count = p_constraint_island.size();
uint32_t valid_constraint_count = 0; uint32_t valid_constraint_count = 0;
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
Constraint2DSW *constraint = p_constraint_island[constraint_index]; GodotConstraint2D *constraint = p_constraint_island[constraint_index];
if (p_constraint_island[constraint_index]->pre_solve(delta)) { if (p_constraint_island[constraint_index]->pre_solve(delta)) {
// Keep this constraint for solving. // Keep this constraint for solving.
p_constraint_island[valid_constraint_count++] = constraint; p_constraint_island[valid_constraint_count++] = constraint;
@ -89,8 +89,8 @@ void Step2DSW::_pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_isl
p_constraint_island.resize(valid_constraint_count); p_constraint_island.resize(valid_constraint_count);
} }
void Step2DSW::_solve_island(uint32_t p_island_index, void *p_userdata) const { void GodotStep2D::_solve_island(uint32_t p_island_index, void *p_userdata) const {
const LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[p_island_index]; const LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[p_island_index];
for (int i = 0; i < iterations; i++) { for (int i = 0; i < iterations; i++) {
uint32_t constraint_count = constraint_island.size(); uint32_t constraint_count = constraint_island.size();
@ -100,12 +100,12 @@ void Step2DSW::_solve_island(uint32_t p_island_index, void *p_userdata) const {
} }
} }
void Step2DSW::_check_suspend(LocalVector<Body2DSW *> &p_body_island) const { void GodotStep2D::_check_suspend(LocalVector<GodotBody2D *> &p_body_island) const {
bool can_sleep = true; bool can_sleep = true;
uint32_t body_count = p_body_island.size(); uint32_t body_count = p_body_island.size();
for (uint32_t body_index = 0; body_index < body_count; ++body_index) { for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
Body2DSW *body = p_body_island[body_index]; GodotBody2D *body = p_body_island[body_index];
if (!body->sleep_test(delta)) { if (!body->sleep_test(delta)) {
can_sleep = false; can_sleep = false;
@ -114,7 +114,7 @@ void Step2DSW::_check_suspend(LocalVector<Body2DSW *> &p_body_island) const {
// Put all to sleep or wake up everyone. // Put all to sleep or wake up everyone.
for (uint32_t body_index = 0; body_index < body_count; ++body_index) { for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
Body2DSW *body = p_body_island[body_index]; GodotBody2D *body = p_body_island[body_index];
bool active = body->is_active(); bool active = body->is_active();
@ -124,7 +124,7 @@ void Step2DSW::_check_suspend(LocalVector<Body2DSW *> &p_body_island) const {
} }
} }
void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) { void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta, int p_iterations) {
p_space->lock(); // can't access space during this p_space->lock(); // can't access space during this
p_space->setup(); //update inertias, etc p_space->setup(); //update inertias, etc
@ -134,7 +134,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
iterations = p_iterations; iterations = p_iterations;
delta = p_delta; delta = p_delta;
const SelfList<Body2DSW>::List *body_list = &p_space->get_active_body_list(); const SelfList<GodotBody2D>::List *body_list = &p_space->get_active_body_list();
/* INTEGRATE FORCES */ /* INTEGRATE FORCES */
@ -143,7 +143,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
int active_count = 0; int active_count = 0;
const SelfList<Body2DSW> *b = body_list->first(); const SelfList<GodotBody2D> *b = body_list->first();
while (b) { while (b) {
b->self()->integrate_forces(p_delta); b->self()->integrate_forces(p_delta);
b = b->next(); b = b->next();
@ -154,7 +154,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
@ -162,11 +162,11 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
uint32_t island_count = 0; uint32_t island_count = 0;
const SelfList<Area2DSW>::List &aml = p_space->get_moved_area_list(); const SelfList<GodotArea2D>::List &aml = p_space->get_moved_area_list();
while (aml.first()) { while (aml.first()) {
for (const Set<Constraint2DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { for (const Set<GodotConstraint2D *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
Constraint2DSW *constraint = E->get(); GodotConstraint2D *constraint = E->get();
if (constraint->get_island_step() == _step) { if (constraint->get_island_step() == _step) {
continue; continue;
} }
@ -177,13 +177,13 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
if (constraint_islands.size() < island_count) { if (constraint_islands.size() < island_count) {
constraint_islands.resize(island_count); constraint_islands.resize(island_count);
} }
LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1]; LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[island_count - 1];
constraint_island.clear(); constraint_island.clear();
all_constraints.push_back(constraint); all_constraints.push_back(constraint);
constraint_island.push_back(constraint); constraint_island.push_back(constraint);
} }
p_space->area_remove_from_moved_list((SelfList<Area2DSW> *)aml.first()); //faster to remove here p_space->area_remove_from_moved_list((SelfList<GodotArea2D> *)aml.first()); //faster to remove here
} }
/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
@ -193,14 +193,14 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
uint32_t body_island_count = 0; uint32_t body_island_count = 0;
while (b) { while (b) {
Body2DSW *body = b->self(); GodotBody2D *body = b->self();
if (body->get_island_step() != _step) { if (body->get_island_step() != _step) {
++body_island_count; ++body_island_count;
if (body_islands.size() < body_island_count) { if (body_islands.size() < body_island_count) {
body_islands.resize(body_island_count); body_islands.resize(body_island_count);
} }
LocalVector<Body2DSW *> &body_island = body_islands[body_island_count - 1]; LocalVector<GodotBody2D *> &body_island = body_islands[body_island_count - 1];
body_island.clear(); body_island.clear();
body_island.reserve(BODY_ISLAND_SIZE_RESERVE); body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
@ -208,7 +208,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
if (constraint_islands.size() < island_count) { if (constraint_islands.size() < island_count) {
constraint_islands.resize(island_count); constraint_islands.resize(island_count);
} }
LocalVector<Constraint2DSW *> &constraint_island = constraint_islands[island_count - 1]; LocalVector<GodotConstraint2D *> &constraint_island = constraint_islands[island_count - 1];
constraint_island.clear(); constraint_island.clear();
constraint_island.reserve(ISLAND_SIZE_RESERVE); constraint_island.reserve(ISLAND_SIZE_RESERVE);
@ -229,18 +229,18 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
/* SETUP CONSTRAINTS / PROCESS COLLISIONS */ /* SETUP CONSTRAINTS / PROCESS COLLISIONS */
uint32_t total_contraint_count = all_constraints.size(); uint32_t total_contraint_count = all_constraints.size();
work_pool.do_work(total_contraint_count, this, &Step2DSW::_setup_contraint, nullptr); work_pool.do_work(total_contraint_count, this, &GodotStep2D::_setup_contraint, nullptr);
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
@ -256,14 +256,14 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
// Warning: _solve_island modifies the constraint islands for optimization purpose, // Warning: _solve_island modifies the constraint islands for optimization purpose,
// their content is not reliable after these calls and shouldn't be used anymore. // their content is not reliable after these calls and shouldn't be used anymore.
if (island_count > 1) { if (island_count > 1) {
work_pool.do_work(island_count, this, &Step2DSW::_solve_island, nullptr); work_pool.do_work(island_count, this, &GodotStep2D::_solve_island, nullptr);
} else if (island_count > 0) { } else if (island_count > 0) {
_solve_island(0); _solve_island(0);
} }
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
@ -271,7 +271,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
b = body_list->first(); b = body_list->first();
while (b) { while (b) {
const SelfList<Body2DSW> *n = b->next(); const SelfList<GodotBody2D> *n = b->next();
b->self()->integrate_velocities(p_delta); b->self()->integrate_velocities(p_delta);
b = n; // in case it shuts itself down b = n; // in case it shuts itself down
} }
@ -284,7 +284,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space2DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); p_space->set_elapsed_time(GodotSpace2D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
//profile_begtime=profile_endtime; //profile_begtime=profile_endtime;
} }
@ -295,7 +295,7 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
_step++; _step++;
} }
Step2DSW::Step2DSW() { GodotStep2D::GodotStep2D() {
body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
constraint_islands.reserve(ISLAND_COUNT_RESERVE); constraint_islands.reserve(ISLAND_COUNT_RESERVE);
all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
@ -303,6 +303,6 @@ Step2DSW::Step2DSW() {
work_pool.init(); work_pool.init();
} }
Step2DSW::~Step2DSW() { GodotStep2D::~GodotStep2D() {
work_pool.finish(); work_pool.finish();
} }

30
servers/physics_2d/step_2d_sw.h → servers/physics_2d/godot_step_2d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* step_2d_sw.h */ /* godot_step_2d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,15 +28,15 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef STEP_2D_SW_H #ifndef GODOT_STEP_2D_H
#define STEP_2D_SW_H #define GODOT_STEP_2D_H
#include "space_2d_sw.h" #include "godot_space_2d.h"
#include "core/templates/local_vector.h" #include "core/templates/local_vector.h"
#include "core/templates/thread_work_pool.h" #include "core/templates/thread_work_pool.h"
class Step2DSW { class GodotStep2D {
uint64_t _step = 1; uint64_t _step = 1;
int iterations = 0; int iterations = 0;
@ -44,20 +44,20 @@ class Step2DSW {
ThreadWorkPool work_pool; ThreadWorkPool work_pool;
LocalVector<LocalVector<Body2DSW *>> body_islands; LocalVector<LocalVector<GodotBody2D *>> body_islands;
LocalVector<LocalVector<Constraint2DSW *>> constraint_islands; LocalVector<LocalVector<GodotConstraint2D *>> constraint_islands;
LocalVector<Constraint2DSW *> all_constraints; LocalVector<GodotConstraint2D *> all_constraints;
void _populate_island(Body2DSW *p_body, LocalVector<Body2DSW *> &p_body_island, LocalVector<Constraint2DSW *> &p_constraint_island); void _populate_island(GodotBody2D *p_body, LocalVector<GodotBody2D *> &p_body_island, LocalVector<GodotConstraint2D *> &p_constraint_island);
void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
void _pre_solve_island(LocalVector<Constraint2DSW *> &p_constraint_island) const; void _pre_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const;
void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const; void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const;
void _check_suspend(LocalVector<Body2DSW *> &p_body_island) const; void _check_suspend(LocalVector<GodotBody2D *> &p_body_island) const;
public: public:
void step(Space2DSW *p_space, real_t p_delta, int p_iterations); void step(GodotSpace2D *p_space, real_t p_delta, int p_iterations);
Step2DSW(); GodotStep2D();
~Step2DSW(); ~GodotStep2D();
}; };
#endif // STEP_2D_SW_H #endif // GODOT_STEP_2D_H

1349
servers/physics_2d/physics_server_2d_sw.cpp
View file

File diff suppressed because it is too large Load diff

28
servers/physics_3d/gjk_epa.cpp
View file

@ -105,7 +105,7 @@ typedef unsigned char U1;
// MinkowskiDiff // MinkowskiDiff
struct MinkowskiDiff { struct MinkowskiDiff {
const Shape3DSW* m_shapes[2]; const GodotShape3D* m_shapes[2];
Transform3D transform_A; Transform3D transform_A;
Transform3D transform_B; Transform3D transform_B;
@ -113,10 +113,10 @@ struct MinkowskiDiff {
real_t margin_A = 0.0; real_t margin_A = 0.0;
real_t margin_B = 0.0; real_t margin_B = 0.0;
Vector3 (*get_support)(const Shape3DSW*, const Vector3&, real_t); Vector3 (*get_support)(const GodotShape3D*, const Vector3&, real_t);
void Initialize(const Shape3DSW* shape0, const Transform3D& wtrs0, const real_t margin0, void Initialize(const GodotShape3D* shape0, const Transform3D& wtrs0, const real_t margin0,
const Shape3DSW* shape1, const Transform3D& wtrs1, const real_t margin1) { const GodotShape3D* shape1, const Transform3D& wtrs1, const real_t margin1) {
m_shapes[0] = shape0; m_shapes[0] = shape0;
m_shapes[1] = shape1; m_shapes[1] = shape1;
transform_A = wtrs0; transform_A = wtrs0;
@ -131,11 +131,11 @@ struct MinkowskiDiff {
} }
} }
static Vector3 get_support_without_margin(const Shape3DSW* p_shape, const Vector3& p_dir, real_t p_margin) { static Vector3 get_support_without_margin(const GodotShape3D* p_shape, const Vector3& p_dir, real_t p_margin) {
return p_shape->get_support(p_dir.normalized()); return p_shape->get_support(p_dir.normalized());
} }
static Vector3 get_support_with_margin(const Shape3DSW* p_shape, const Vector3& p_dir, real_t p_margin) { static Vector3 get_support_with_margin(const GodotShape3D* p_shape, const Vector3& p_dir, real_t p_margin) {
Vector3 local_dir_norm = p_dir; Vector3 local_dir_norm = p_dir;
if (local_dir_norm.length_squared() < CMP_EPSILON2) { if (local_dir_norm.length_squared() < CMP_EPSILON2) {
local_dir_norm = Vector3(-1.0, -1.0, -1.0); local_dir_norm = Vector3(-1.0, -1.0, -1.0);
@ -862,8 +862,8 @@ struct GJK
}; };
// //
static void Initialize( const Shape3DSW* shape0, const Transform3D& wtrs0, real_t margin0, static void Initialize( const GodotShape3D* shape0, const Transform3D& wtrs0, real_t margin0,
const Shape3DSW* shape1, const Transform3D& wtrs1, real_t margin1, const GodotShape3D* shape1, const Transform3D& wtrs1, real_t margin1,
sResults& results, sResults& results,
tShape& shape) tShape& shape)
{ {
@ -884,10 +884,10 @@ struct GJK
// //
// //
bool Distance( const Shape3DSW* shape0, bool Distance( const GodotShape3D* shape0,
const Transform3D& wtrs0, const Transform3D& wtrs0,
real_t margin0, real_t margin0,
const Shape3DSW* shape1, const GodotShape3D* shape1,
const Transform3D& wtrs1, const Transform3D& wtrs1,
real_t margin1, real_t margin1,
const Vector3& guess, const Vector3& guess,
@ -925,10 +925,10 @@ bool Distance( const Shape3DSW* shape0,
// //
bool Penetration( const Shape3DSW* shape0, bool Penetration( const GodotShape3D* shape0,
const Transform3D& wtrs0, const Transform3D& wtrs0,
real_t margin0, real_t margin0,
const Shape3DSW* shape1, const GodotShape3D* shape1,
const Transform3D& wtrs1, const Transform3D& wtrs1,
real_t margin1, real_t margin1,
const Vector3& guess, const Vector3& guess,
@ -993,7 +993,7 @@ bool Penetration( const Shape3DSW* shape0,
/* clang-format on */ /* clang-format on */
bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) { bool gjk_epa_calculate_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) {
GjkEpa2::sResults res; GjkEpa2::sResults res;
if (GjkEpa2::Distance(p_shape_A, p_transform_A, 0.0, p_shape_B, p_transform_B, 0.0, p_transform_B.origin - p_transform_A.origin, res)) { if (GjkEpa2::Distance(p_shape_A, p_transform_A, 0.0, p_shape_B, p_transform_B, 0.0, p_transform_B.origin - p_transform_A.origin, res)) {
@ -1005,7 +1005,7 @@ bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p
return false; return false;
} }
bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) { bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) {
GjkEpa2::sResults res; GjkEpa2::sResults res;
if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) { if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) {

8
servers/physics_3d/gjk_epa.h
View file

@ -31,10 +31,10 @@
#ifndef GJK_EPA_H #ifndef GJK_EPA_H
#define GJK_EPA_H #define GJK_EPA_H
#include "collision_solver_3d_sw.h" #include "godot_collision_solver_3d.h"
#include "shape_3d_sw.h" #include "godot_shape_3d.h"
bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0); bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0);
bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); bool gjk_epa_calculate_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B);
#endif #endif

47
servers/physics_3d/area_3d_sw.cpp → servers/physics_3d/godot_area_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* area_3d_sw.cpp */ /* godot_area_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,39 +28,40 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "area_3d_sw.h" #include "godot_area_3d.h"
#include "body_3d_sw.h"
#include "soft_body_3d_sw.h"
#include "space_3d_sw.h"
Area3DSW::BodyKey::BodyKey(SoftBody3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { #include "godot_body_3d.h"
#include "godot_soft_body_3d.h"
#include "godot_space_3d.h"
GodotArea3D::BodyKey::BodyKey(GodotSoftBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
rid = p_body->get_self(); rid = p_body->get_self();
instance_id = p_body->get_instance_id(); instance_id = p_body->get_instance_id();
body_shape = p_body_shape; body_shape = p_body_shape;
area_shape = p_area_shape; area_shape = p_area_shape;
} }
Area3DSW::BodyKey::BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { GodotArea3D::BodyKey::BodyKey(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
rid = p_body->get_self(); rid = p_body->get_self();
instance_id = p_body->get_instance_id(); instance_id = p_body->get_instance_id();
body_shape = p_body_shape; body_shape = p_body_shape;
area_shape = p_area_shape; area_shape = p_area_shape;
} }
Area3DSW::BodyKey::BodyKey(Area3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { GodotArea3D::BodyKey::BodyKey(GodotArea3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
rid = p_body->get_self(); rid = p_body->get_self();
instance_id = p_body->get_instance_id(); instance_id = p_body->get_instance_id();
body_shape = p_body_shape; body_shape = p_body_shape;
area_shape = p_area_shape; area_shape = p_area_shape;
} }
void Area3DSW::_shapes_changed() { void GodotArea3D::_shapes_changed() {
if (!moved_list.in_list() && get_space()) { if (!moved_list.in_list() && get_space()) {
get_space()->area_add_to_moved_list(&moved_list); get_space()->area_add_to_moved_list(&moved_list);
} }
} }
void Area3DSW::set_transform(const Transform3D &p_transform) { void GodotArea3D::set_transform(const Transform3D &p_transform) {
if (!moved_list.in_list() && get_space()) { if (!moved_list.in_list() && get_space()) {
get_space()->area_add_to_moved_list(&moved_list); get_space()->area_add_to_moved_list(&moved_list);
} }
@ -69,7 +70,7 @@ void Area3DSW::set_transform(const Transform3D &p_transform) {
_set_inv_transform(p_transform.affine_inverse()); _set_inv_transform(p_transform.affine_inverse());
} }
void Area3DSW::set_space(Space3DSW *p_space) { void GodotArea3D::set_space(GodotSpace3D *p_space) {
if (get_space()) { if (get_space()) {
if (monitor_query_list.in_list()) { if (monitor_query_list.in_list()) {
get_space()->area_remove_from_monitor_query_list(&monitor_query_list); get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
@ -85,7 +86,7 @@ void Area3DSW::set_space(Space3DSW *p_space) {
_set_space(p_space); _set_space(p_space);
} }
void Area3DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) { void GodotArea3D::set_monitor_callback(ObjectID p_id, const StringName &p_method) {
if (p_id == monitor_callback_id) { if (p_id == monitor_callback_id) {
monitor_callback_method = p_method; monitor_callback_method = p_method;
return; return;
@ -106,7 +107,7 @@ void Area3DSW::set_monitor_callback(ObjectID p_id, const StringName &p_method) {
} }
} }
void Area3DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) { void GodotArea3D::set_area_monitor_callback(ObjectID p_id, const StringName &p_method) {
if (p_id == area_monitor_callback_id) { if (p_id == area_monitor_callback_id) {
area_monitor_callback_method = p_method; area_monitor_callback_method = p_method;
return; return;
@ -127,7 +128,7 @@ void Area3DSW::set_area_monitor_callback(ObjectID p_id, const StringName &p_meth
} }
} }
void Area3DSW::set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode) { void GodotArea3D::set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_mode) {
bool do_override = p_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; bool do_override = p_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
if (do_override == (space_override_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) { if (do_override == (space_override_mode != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED)) {
return; return;
@ -137,7 +138,7 @@ void Area3DSW::set_space_override_mode(PhysicsServer3D::AreaSpaceOverrideMode p_
_shape_changed(); _shape_changed();
} }
void Area3DSW::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) { void GodotArea3D::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value) {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::AREA_PARAM_GRAVITY: case PhysicsServer3D::AREA_PARAM_GRAVITY:
gravity = p_value; gravity = p_value;
@ -180,7 +181,7 @@ void Area3DSW::set_param(PhysicsServer3D::AreaParameter p_param, const Variant &
} }
} }
Variant Area3DSW::get_param(PhysicsServer3D::AreaParameter p_param) const { Variant GodotArea3D::get_param(PhysicsServer3D::AreaParameter p_param) const {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::AREA_PARAM_GRAVITY: case PhysicsServer3D::AREA_PARAM_GRAVITY:
return gravity; return gravity;
@ -211,7 +212,7 @@ Variant Area3DSW::get_param(PhysicsServer3D::AreaParameter p_param) const {
return Variant(); return Variant();
} }
void Area3DSW::_queue_monitor_update() { void GodotArea3D::_queue_monitor_update() {
ERR_FAIL_COND(!get_space()); ERR_FAIL_COND(!get_space());
if (!monitor_query_list.in_list()) { if (!monitor_query_list.in_list()) {
@ -219,7 +220,7 @@ void Area3DSW::_queue_monitor_update() {
} }
} }
void Area3DSW::set_monitorable(bool p_monitorable) { void GodotArea3D::set_monitorable(bool p_monitorable) {
if (monitorable == p_monitorable) { if (monitorable == p_monitorable) {
return; return;
} }
@ -228,7 +229,7 @@ void Area3DSW::set_monitorable(bool p_monitorable) {
_set_static(!monitorable); _set_static(!monitorable);
} }
void Area3DSW::call_queries() { void GodotArea3D::call_queries() {
if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) { if (monitor_callback_id.is_valid() && !monitored_bodies.is_empty()) {
Variant res[5]; Variant res[5];
Variant *resptr[5]; Variant *resptr[5];
@ -304,7 +305,7 @@ void Area3DSW::call_queries() {
} }
} }
void Area3DSW::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const { void GodotArea3D::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const {
if (is_gravity_point()) { if (is_gravity_point()) {
const real_t gravity_distance_scale = get_gravity_distance_scale(); const real_t gravity_distance_scale = get_gravity_distance_scale();
Vector3 v = get_transform().xform(get_gravity_vector()) - p_position; Vector3 v = get_transform().xform(get_gravity_vector()) - p_position;
@ -324,13 +325,13 @@ void Area3DSW::compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) co
} }
} }
Area3DSW::Area3DSW() : GodotArea3D::GodotArea3D() :
CollisionObject3DSW(TYPE_AREA), GodotCollisionObject3D(TYPE_AREA),
monitor_query_list(this), monitor_query_list(this),
moved_list(this) { moved_list(this) {
_set_static(true); //areas are never active _set_static(true); //areas are never active
set_ray_pickable(false); set_ray_pickable(false);
} }
Area3DSW::~Area3DSW() { GodotArea3D::~GodotArea3D() {
} }

73
servers/physics_3d/area_3d_sw.h → servers/physics_3d/godot_area_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* area_3d_sw.h */ /* godot_area_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,19 +28,20 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef AREA_SW_H #ifndef GODOT_AREA_3D_H
#define AREA_SW_H #define GODOT_AREA_3D_H
#include "godot_collision_object_3d.h"
#include "collision_object_3d_sw.h"
#include "core/templates/self_list.h" #include "core/templates/self_list.h"
#include "servers/physics_server_3d.h" #include "servers/physics_server_3d.h"
class Space3DSW; class GodotSpace3D;
class Body3DSW; class GodotBody3D;
class SoftBody3DSW; class GodotSoftBody3D;
class Constraint3DSW; class GodotConstraint3D;
class Area3DSW : public CollisionObject3DSW { class GodotArea3D : public GodotCollisionObject3D {
PhysicsServer3D::AreaSpaceOverrideMode space_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED; PhysicsServer3D::AreaSpaceOverrideMode space_override_mode = PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED;
real_t gravity = 9.80665; real_t gravity = 9.80665;
Vector3 gravity_vector = Vector3(0, -1, 0); Vector3 gravity_vector = Vector3(0, -1, 0);
@ -62,8 +63,8 @@ class Area3DSW : public CollisionObject3DSW {
ObjectID area_monitor_callback_id; ObjectID area_monitor_callback_id;
StringName area_monitor_callback_method; StringName area_monitor_callback_method;
SelfList<Area3DSW> monitor_query_list; SelfList<GodotArea3D> monitor_query_list;
SelfList<Area3DSW> moved_list; SelfList<GodotArea3D> moved_list;
struct BodyKey { struct BodyKey {
RID rid; RID rid;
@ -84,9 +85,9 @@ class Area3DSW : public CollisionObject3DSW {
} }
_FORCE_INLINE_ BodyKey() {} _FORCE_INLINE_ BodyKey() {}
BodyKey(SoftBody3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); BodyKey(GodotSoftBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
BodyKey(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); BodyKey(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
BodyKey(Area3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); BodyKey(GodotArea3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
}; };
struct BodyState { struct BodyState {
@ -99,7 +100,7 @@ class Area3DSW : public CollisionObject3DSW {
Map<BodyKey, BodyState> monitored_bodies; Map<BodyKey, BodyState> monitored_bodies;
Map<BodyKey, BodyState> monitored_areas; Map<BodyKey, BodyState> monitored_areas;
Set<Constraint3DSW *> constraints; Set<GodotConstraint3D *> constraints;
virtual void _shapes_changed(); virtual void _shapes_changed();
void _queue_monitor_update(); void _queue_monitor_update();
@ -111,14 +112,14 @@ public:
void set_area_monitor_callback(ObjectID p_id, const StringName &p_method); void set_area_monitor_callback(ObjectID p_id, const StringName &p_method);
_FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); } _FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id.is_valid(); }
_FORCE_INLINE_ void add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); _FORCE_INLINE_ void add_body_to_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
_FORCE_INLINE_ void remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape); _FORCE_INLINE_ void remove_body_from_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape);
_FORCE_INLINE_ void add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape); _FORCE_INLINE_ void add_soft_body_to_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape);
_FORCE_INLINE_ void remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape); _FORCE_INLINE_ void remove_soft_body_from_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape);
_FORCE_INLINE_ void add_area_to_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); _FORCE_INLINE_ void add_area_to_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
_FORCE_INLINE_ void remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape); _FORCE_INLINE_ void remove_area_from_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape);
void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value); void set_param(PhysicsServer3D::AreaParameter p_param, const Variant &p_value);
Variant get_param(PhysicsServer3D::AreaParameter p_param) const; Variant get_param(PhysicsServer3D::AreaParameter p_param) const;
@ -162,9 +163,9 @@ public:
_FORCE_INLINE_ void set_wind_direction(const Vector3 &p_wind_direction) { wind_direction = p_wind_direction; } _FORCE_INLINE_ void set_wind_direction(const Vector3 &p_wind_direction) { wind_direction = p_wind_direction; }
_FORCE_INLINE_ const Vector3 &get_wind_direction() const { return wind_direction; } _FORCE_INLINE_ const Vector3 &get_wind_direction() const { return wind_direction; }
_FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); }
_FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); } _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); }
_FORCE_INLINE_ const Set<Constraint3DSW *> &get_constraints() const { return constraints; } _FORCE_INLINE_ const Set<GodotConstraint3D *> &get_constraints() const { return constraints; }
_FORCE_INLINE_ void clear_constraints() { constraints.clear(); } _FORCE_INLINE_ void clear_constraints() { constraints.clear(); }
void set_monitorable(bool p_monitorable); void set_monitorable(bool p_monitorable);
@ -172,17 +173,17 @@ public:
void set_transform(const Transform3D &p_transform); void set_transform(const Transform3D &p_transform);
void set_space(Space3DSW *p_space); void set_space(GodotSpace3D *p_space);
void call_queries(); void call_queries();
void compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const; void compute_gravity(const Vector3 &p_position, Vector3 &r_gravity) const;
Area3DSW(); GodotArea3D();
~Area3DSW(); ~GodotArea3D();
}; };
void Area3DSW::add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) { void GodotArea3D::add_soft_body_to_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) {
BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape); BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape);
monitored_soft_bodies[bk].inc(); monitored_soft_bodies[bk].inc();
if (!monitor_query_list.in_list()) { if (!monitor_query_list.in_list()) {
@ -190,7 +191,7 @@ void Area3DSW::add_soft_body_to_query(SoftBody3DSW *p_soft_body, uint32_t p_soft
} }
} }
void Area3DSW::remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) { void GodotArea3D::remove_soft_body_from_query(GodotSoftBody3D *p_soft_body, uint32_t p_soft_body_shape, uint32_t p_area_shape) {
BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape); BodyKey bk(p_soft_body, p_soft_body_shape, p_area_shape);
monitored_soft_bodies[bk].dec(); monitored_soft_bodies[bk].dec();
if (!monitor_query_list.in_list()) { if (!monitor_query_list.in_list()) {
@ -198,7 +199,7 @@ void Area3DSW::remove_soft_body_from_query(SoftBody3DSW *p_soft_body, uint32_t p
} }
} }
void Area3DSW::add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { void GodotArea3D::add_body_to_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
BodyKey bk(p_body, p_body_shape, p_area_shape); BodyKey bk(p_body, p_body_shape, p_area_shape);
monitored_bodies[bk].inc(); monitored_bodies[bk].inc();
if (!monitor_query_list.in_list()) { if (!monitor_query_list.in_list()) {
@ -206,7 +207,7 @@ void Area3DSW::add_body_to_query(Body3DSW *p_body, uint32_t p_body_shape, uint32
} }
} }
void Area3DSW::remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, uint32_t p_area_shape) { void GodotArea3D::remove_body_from_query(GodotBody3D *p_body, uint32_t p_body_shape, uint32_t p_area_shape) {
BodyKey bk(p_body, p_body_shape, p_area_shape); BodyKey bk(p_body, p_body_shape, p_area_shape);
monitored_bodies[bk].dec(); monitored_bodies[bk].dec();
if (!monitor_query_list.in_list()) { if (!monitor_query_list.in_list()) {
@ -214,7 +215,7 @@ void Area3DSW::remove_body_from_query(Body3DSW *p_body, uint32_t p_body_shape, u
} }
} }
void Area3DSW::add_area_to_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { void GodotArea3D::add_area_to_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
BodyKey bk(p_area, p_area_shape, p_self_shape); BodyKey bk(p_area, p_area_shape, p_self_shape);
monitored_areas[bk].inc(); monitored_areas[bk].inc();
if (!monitor_query_list.in_list()) { if (!monitor_query_list.in_list()) {
@ -222,7 +223,7 @@ void Area3DSW::add_area_to_query(Area3DSW *p_area, uint32_t p_area_shape, uint32
} }
} }
void Area3DSW::remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, uint32_t p_self_shape) { void GodotArea3D::remove_area_from_query(GodotArea3D *p_area, uint32_t p_area_shape, uint32_t p_self_shape) {
BodyKey bk(p_area, p_area_shape, p_self_shape); BodyKey bk(p_area, p_area_shape, p_self_shape);
monitored_areas[bk].dec(); monitored_areas[bk].dec();
if (!monitor_query_list.in_list()) { if (!monitor_query_list.in_list()) {
@ -231,15 +232,15 @@ void Area3DSW::remove_area_from_query(Area3DSW *p_area, uint32_t p_area_shape, u
} }
struct AreaCMP { struct AreaCMP {
Area3DSW *area = nullptr; GodotArea3D *area = nullptr;
int refCount = 0; 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_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_ bool operator<(const AreaCMP &p_cmp) const { return area->get_priority() < p_cmp.area->get_priority(); }
_FORCE_INLINE_ AreaCMP() {} _FORCE_INLINE_ AreaCMP() {}
_FORCE_INLINE_ AreaCMP(Area3DSW *p_area) { _FORCE_INLINE_ AreaCMP(GodotArea3D *p_area) {
area = p_area; area = p_area;
refCount = 1; refCount = 1;
} }
}; };
#endif // AREA__SW_H #endif // GODOT_AREA_3D_H

43
servers/physics_3d/area_pair_3d_sw.cpp → servers/physics_3d/godot_area_pair_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* area_pair_3d_sw.cpp */ /* godot_area_pair_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,12 +28,13 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "area_pair_3d_sw.h" #include "godot_area_pair_3d.h"
#include "collision_solver_3d_sw.h"
bool AreaPair3DSW::setup(real_t p_step) { #include "godot_collision_solver_3d.h"
bool GodotAreaPair3D::setup(real_t p_step) {
bool result = false; bool result = false;
if (area->collides_with(body) && CollisionSolver3DSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) { if (area->collides_with(body) && GodotCollisionSolver3D::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) {
result = true; result = true;
} }
@ -51,7 +52,7 @@ bool AreaPair3DSW::setup(real_t p_step) {
return process_collision; return process_collision;
} }
bool AreaPair3DSW::pre_solve(real_t p_step) { bool GodotAreaPair3D::pre_solve(real_t p_step) {
if (!process_collision) { if (!process_collision) {
return false; return false;
} }
@ -77,11 +78,11 @@ bool AreaPair3DSW::pre_solve(real_t p_step) {
return false; // Never do any post solving. return false; // Never do any post solving.
} }
void AreaPair3DSW::solve(real_t p_step) { void GodotAreaPair3D::solve(real_t p_step) {
// Nothing to do. // Nothing to do.
} }
AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape) { GodotAreaPair3D::GodotAreaPair3D(GodotBody3D *p_body, int p_body_shape, GodotArea3D *p_area, int p_area_shape) {
body = p_body; body = p_body;
area = p_area; area = p_area;
body_shape = p_body_shape; body_shape = p_body_shape;
@ -93,7 +94,7 @@ AreaPair3DSW::AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area,
} }
} }
AreaPair3DSW::~AreaPair3DSW() { GodotAreaPair3D::~GodotAreaPair3D() {
if (colliding) { if (colliding) {
if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
body->remove_area(area); body->remove_area(area);
@ -108,10 +109,10 @@ AreaPair3DSW::~AreaPair3DSW() {
//////////////////////////////////////////////////// ////////////////////////////////////////////////////
bool Area2Pair3DSW::setup(real_t p_step) { bool GodotArea2Pair3D::setup(real_t p_step) {
bool result_a = area_a->collides_with(area_b); bool result_a = area_a->collides_with(area_b);
bool result_b = area_b->collides_with(area_a); bool result_b = area_b->collides_with(area_a);
if ((result_a || result_b) && !CollisionSolver3DSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) { if ((result_a || result_b) && !GodotCollisionSolver3D::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) {
result_a = false; result_a = false;
result_b = false; result_b = false;
} }
@ -139,7 +140,7 @@ bool Area2Pair3DSW::setup(real_t p_step) {
return process_collision; return process_collision;
} }
bool Area2Pair3DSW::pre_solve(real_t p_step) { bool GodotArea2Pair3D::pre_solve(real_t p_step) {
if (process_collision_a) { if (process_collision_a) {
if (colliding_a) { if (colliding_a) {
area_a->add_area_to_query(area_b, shape_b, shape_a); area_a->add_area_to_query(area_b, shape_b, shape_a);
@ -159,11 +160,11 @@ bool Area2Pair3DSW::pre_solve(real_t p_step) {
return false; // Never do any post solving. return false; // Never do any post solving.
} }
void Area2Pair3DSW::solve(real_t p_step) { void GodotArea2Pair3D::solve(real_t p_step) {
// Nothing to do. // Nothing to do.
} }
Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b) { GodotArea2Pair3D::GodotArea2Pair3D(GodotArea3D *p_area_a, int p_shape_a, GodotArea3D *p_area_b, int p_shape_b) {
area_a = p_area_a; area_a = p_area_a;
area_b = p_area_b; area_b = p_area_b;
shape_a = p_shape_a; shape_a = p_shape_a;
@ -172,7 +173,7 @@ Area2Pair3DSW::Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area
area_b->add_constraint(this); area_b->add_constraint(this);
} }
Area2Pair3DSW::~Area2Pair3DSW() { GodotArea2Pair3D::~GodotArea2Pair3D() {
if (colliding_a) { if (colliding_a) {
if (area_a->has_area_monitor_callback()) { if (area_a->has_area_monitor_callback()) {
area_a->remove_area_from_query(area_b, shape_b, shape_a); area_a->remove_area_from_query(area_b, shape_b, shape_a);
@ -191,11 +192,11 @@ Area2Pair3DSW::~Area2Pair3DSW() {
//////////////////////////////////////////////////// ////////////////////////////////////////////////////
bool AreaSoftBodyPair3DSW::setup(real_t p_step) { bool GodotAreaSoftBodyPair3D::setup(real_t p_step) {
bool result = false; bool result = false;
if ( if (
area->collides_with(soft_body) && area->collides_with(soft_body) &&
CollisionSolver3DSW::solve_static( GodotCollisionSolver3D::solve_static(
soft_body->get_shape(soft_body_shape), soft_body->get_shape(soft_body_shape),
soft_body->get_transform() * soft_body->get_shape_transform(soft_body_shape), soft_body->get_transform() * soft_body->get_shape_transform(soft_body_shape),
area->get_shape(area_shape), area->get_shape(area_shape),
@ -219,7 +220,7 @@ bool AreaSoftBodyPair3DSW::setup(real_t p_step) {
return process_collision; return process_collision;
} }
bool AreaSoftBodyPair3DSW::pre_solve(real_t p_step) { bool GodotAreaSoftBodyPair3D::pre_solve(real_t p_step) {
if (!process_collision) { if (!process_collision) {
return false; return false;
} }
@ -245,11 +246,11 @@ bool AreaSoftBodyPair3DSW::pre_solve(real_t p_step) {
return false; // Never do any post solving. return false; // Never do any post solving.
} }
void AreaSoftBodyPair3DSW::solve(real_t p_step) { void GodotAreaSoftBodyPair3D::solve(real_t p_step) {
// Nothing to do. // Nothing to do.
} }
AreaSoftBodyPair3DSW::AreaSoftBodyPair3DSW(SoftBody3DSW *p_soft_body, int p_soft_body_shape, Area3DSW *p_area, int p_area_shape) { GodotAreaSoftBodyPair3D::GodotAreaSoftBodyPair3D(GodotSoftBody3D *p_soft_body, int p_soft_body_shape, GodotArea3D *p_area, int p_area_shape) {
soft_body = p_soft_body; soft_body = p_soft_body;
area = p_area; area = p_area;
soft_body_shape = p_soft_body_shape; soft_body_shape = p_soft_body_shape;
@ -258,7 +259,7 @@ AreaSoftBodyPair3DSW::AreaSoftBodyPair3DSW(SoftBody3DSW *p_soft_body, int p_soft
area->add_constraint(this); area->add_constraint(this);
} }
AreaSoftBodyPair3DSW::~AreaSoftBodyPair3DSW() { GodotAreaSoftBodyPair3D::~GodotAreaSoftBodyPair3D() {
if (colliding) { if (colliding) {
if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) { if (area->get_space_override_mode() != PhysicsServer3D::AREA_SPACE_OVERRIDE_DISABLED) {
soft_body->remove_area(area); soft_body->remove_area(area);

46
servers/physics_3d/area_pair_3d_sw.h → servers/physics_3d/godot_area_pair_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* area_pair_3d_sw.h */ /* godot_area_pair_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,17 +28,17 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef AREA_PAIR_SW_H #ifndef GODOT_AREA_PAIR_3D_H
#define AREA_PAIR_SW_H #define GODOT_AREA_PAIR_3D_H
#include "area_3d_sw.h" #include "godot_area_3d.h"
#include "body_3d_sw.h" #include "godot_body_3d.h"
#include "constraint_3d_sw.h" #include "godot_constraint_3d.h"
#include "soft_body_3d_sw.h" #include "godot_soft_body_3d.h"
class AreaPair3DSW : public Constraint3DSW { class GodotAreaPair3D : public GodotConstraint3D {
Body3DSW *body; GodotBody3D *body;
Area3DSW *area; GodotArea3D *area;
int body_shape; int body_shape;
int area_shape; int area_shape;
bool colliding = false; bool colliding = false;
@ -49,13 +49,13 @@ public:
virtual bool pre_solve(real_t p_step) override; virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override; virtual void solve(real_t p_step) override;
AreaPair3DSW(Body3DSW *p_body, int p_body_shape, Area3DSW *p_area, int p_area_shape); GodotAreaPair3D(GodotBody3D *p_body, int p_body_shape, GodotArea3D *p_area, int p_area_shape);
~AreaPair3DSW(); ~GodotAreaPair3D();
}; };
class Area2Pair3DSW : public Constraint3DSW { class GodotArea2Pair3D : public GodotConstraint3D {
Area3DSW *area_a; GodotArea3D *area_a;
Area3DSW *area_b; GodotArea3D *area_b;
int shape_a; int shape_a;
int shape_b; int shape_b;
bool colliding_a = false; bool colliding_a = false;
@ -68,13 +68,13 @@ public:
virtual bool pre_solve(real_t p_step) override; virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override; virtual void solve(real_t p_step) override;
Area2Pair3DSW(Area3DSW *p_area_a, int p_shape_a, Area3DSW *p_area_b, int p_shape_b); GodotArea2Pair3D(GodotArea3D *p_area_a, int p_shape_a, GodotArea3D *p_area_b, int p_shape_b);
~Area2Pair3DSW(); ~GodotArea2Pair3D();
}; };
class AreaSoftBodyPair3DSW : public Constraint3DSW { class GodotAreaSoftBodyPair3D : public GodotConstraint3D {
SoftBody3DSW *soft_body; GodotSoftBody3D *soft_body;
Area3DSW *area; GodotArea3D *area;
int soft_body_shape; int soft_body_shape;
int area_shape; int area_shape;
bool colliding = false; bool colliding = false;
@ -85,8 +85,8 @@ public:
virtual bool pre_solve(real_t p_step) override; virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override; virtual void solve(real_t p_step) override;
AreaSoftBodyPair3DSW(SoftBody3DSW *p_sof_body, int p_soft_body_shape, Area3DSW *p_area, int p_area_shape); GodotAreaSoftBodyPair3D(GodotSoftBody3D *p_sof_body, int p_soft_body_shape, GodotArea3D *p_area, int p_area_shape);
~AreaSoftBodyPair3DSW(); ~GodotAreaSoftBodyPair3D();
}; };
#endif // AREA_PAIR__SW_H #endif // GODOT_AREA_PAIR_3D_H

111
servers/physics_3d/body_3d_sw.cpp → servers/physics_3d/godot_body_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* body_3d_sw.cpp */ /* godot_body_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,19 +28,19 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "body_3d_sw.h" #include "godot_body_3d.h"
#include "area_3d_sw.h" #include "godot_area_3d.h"
#include "body_direct_state_3d_sw.h" #include "godot_body_direct_state_3d.h"
#include "space_3d_sw.h" #include "godot_space_3d.h"
void Body3DSW::_mass_properties_changed() { void GodotBody3D::_mass_properties_changed() {
if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) { if (get_space() && !mass_properties_update_list.in_list() && (calculate_inertia || calculate_center_of_mass)) {
get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list); get_space()->body_add_to_mass_properties_update_list(&mass_properties_update_list);
} }
} }
void Body3DSW::_update_transform_dependant() { void GodotBody3D::_update_transform_dependant() {
center_of_mass = get_transform().basis.xform(center_of_mass_local); center_of_mass = get_transform().basis.xform(center_of_mass_local);
principal_inertia_axes = get_transform().basis * principal_inertia_axes_local; principal_inertia_axes = get_transform().basis * principal_inertia_axes_local;
@ -52,7 +52,7 @@ void Body3DSW::_update_transform_dependant() {
_inv_inertia_tensor = tb * diag * tbt; _inv_inertia_tensor = tb * diag * tbt;
} }
void Body3DSW::update_mass_properties() { void GodotBody3D::update_mass_properties() {
// Update shapes and motions. // Update shapes and motions.
switch (mode) { switch (mode) {
@ -106,7 +106,7 @@ void Body3DSW::update_mass_properties() {
inertia_set = true; inertia_set = true;
const Shape3DSW *shape = get_shape(i); const GodotShape3D *shape = get_shape(i);
real_t mass = area * this->mass / total_area; real_t mass = area * this->mass / total_area;
@ -164,13 +164,13 @@ void Body3DSW::update_mass_properties() {
_update_transform_dependant(); _update_transform_dependant();
} }
void Body3DSW::reset_mass_properties() { void GodotBody3D::reset_mass_properties() {
calculate_inertia = true; calculate_inertia = true;
calculate_center_of_mass = true; calculate_center_of_mass = true;
_mass_properties_changed(); _mass_properties_changed();
} }
void Body3DSW::set_active(bool p_active) { void GodotBody3D::set_active(bool p_active) {
if (active == p_active) { if (active == p_active) {
return; return;
} }
@ -189,7 +189,7 @@ void Body3DSW::set_active(bool p_active) {
} }
} }
void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value) { void GodotBody3D::set_param(PhysicsServer3D::BodyParameter p_param, const Variant &p_value) {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::BODY_PARAM_BOUNCE: { case PhysicsServer3D::BODY_PARAM_BOUNCE: {
bounce = p_value; bounce = p_value;
@ -240,7 +240,7 @@ void Body3DSW::set_param(PhysicsServer3D::BodyParameter p_param, const Variant &
} }
} }
Variant Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const { Variant GodotBody3D::get_param(PhysicsServer3D::BodyParameter p_param) const {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::BODY_PARAM_BOUNCE: { case PhysicsServer3D::BODY_PARAM_BOUNCE: {
return bounce; return bounce;
@ -278,7 +278,7 @@ Variant Body3DSW::get_param(PhysicsServer3D::BodyParameter p_param) const {
return 0; return 0;
} }
void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) { void GodotBody3D::set_mode(PhysicsServer3D::BodyMode p_mode) {
PhysicsServer3D::BodyMode prev = mode; PhysicsServer3D::BodyMode prev = mode;
mode = p_mode; mode = p_mode;
@ -321,15 +321,15 @@ void Body3DSW::set_mode(PhysicsServer3D::BodyMode p_mode) {
} }
} }
PhysicsServer3D::BodyMode Body3DSW::get_mode() const { PhysicsServer3D::BodyMode GodotBody3D::get_mode() const {
return mode; return mode;
} }
void Body3DSW::_shapes_changed() { void GodotBody3D::_shapes_changed() {
_mass_properties_changed(); _mass_properties_changed();
} }
void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { void GodotBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
switch (p_state) { switch (p_state) {
case PhysicsServer3D::BODY_STATE_TRANSFORM: { case PhysicsServer3D::BODY_STATE_TRANSFORM: {
if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { if (mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
@ -395,7 +395,7 @@ void Body3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_va
} }
} }
Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const { Variant GodotBody3D::get_state(PhysicsServer3D::BodyState p_state) const {
switch (p_state) { switch (p_state) {
case PhysicsServer3D::BODY_STATE_TRANSFORM: { case PhysicsServer3D::BODY_STATE_TRANSFORM: {
return get_transform(); return get_transform();
@ -417,7 +417,7 @@ Variant Body3DSW::get_state(PhysicsServer3D::BodyState p_state) const {
return Variant(); return Variant();
} }
void Body3DSW::set_space(Space3DSW *p_space) { void GodotBody3D::set_space(GodotSpace3D *p_space) {
if (get_space()) { if (get_space()) {
if (mass_properties_update_list.in_list()) { if (mass_properties_update_list.in_list()) {
get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list); get_space()->body_remove_from_mass_properties_update_list(&mass_properties_update_list);
@ -440,7 +440,7 @@ void Body3DSW::set_space(Space3DSW *p_space) {
} }
} }
void Body3DSW::_compute_area_gravity_and_damping(const Area3DSW *p_area) { void GodotBody3D::_compute_area_gravity_and_damping(const GodotArea3D *p_area) {
Vector3 area_gravity; Vector3 area_gravity;
p_area->compute_gravity(get_transform().get_origin(), area_gravity); p_area->compute_gravity(get_transform().get_origin(), area_gravity);
gravity += area_gravity; gravity += area_gravity;
@ -449,7 +449,7 @@ void Body3DSW::_compute_area_gravity_and_damping(const Area3DSW *p_area) {
area_angular_damp += p_area->get_angular_damp(); area_angular_damp += p_area->get_angular_damp();
} }
void Body3DSW::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) { void GodotBody3D::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) {
if (lock) { if (lock) {
locked_axis |= p_axis; locked_axis |= p_axis;
} else { } else {
@ -457,17 +457,16 @@ void Body3DSW::set_axis_lock(PhysicsServer3D::BodyAxis p_axis, bool lock) {
} }
} }
bool Body3DSW::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const { bool GodotBody3D::is_axis_locked(PhysicsServer3D::BodyAxis p_axis) const {
return locked_axis & p_axis; return locked_axis & p_axis;
} }
void Body3DSW::integrate_forces(real_t p_step) { void GodotBody3D::integrate_forces(real_t p_step) {
if (mode == PhysicsServer3D::BODY_MODE_STATIC) { if (mode == PhysicsServer3D::BODY_MODE_STATIC) {
return; return;
} }
Area3DSW *def_area = get_space()->get_default_area(); GodotArea3D *def_area = get_space()->get_default_area();
// AreaSW *damp_area = def_area;
ERR_FAIL_COND(!def_area); ERR_FAIL_COND(!def_area);
@ -591,7 +590,7 @@ void Body3DSW::integrate_forces(real_t p_step) {
contact_count = 0; contact_count = 0;
} }
void Body3DSW::integrate_velocities(real_t p_step) { void GodotBody3D::integrate_velocities(real_t p_step) {
if (mode == PhysicsServer3D::BODY_MODE_STATIC) { if (mode == PhysicsServer3D::BODY_MODE_STATIC) {
return; return;
} }
@ -655,49 +654,17 @@ void Body3DSW::integrate_velocities(real_t p_step) {
_update_transform_dependant(); _update_transform_dependant();
} }
/* void GodotBody3D::wakeup_neighbours() {
void BodySW::simulate_motion(const Transform3D& p_xform,real_t p_step) { for (const KeyValue<GodotConstraint3D *, int> &E : constraint_map) {
Transform3D inv_xform = p_xform.affine_inverse(); const GodotConstraint3D *c = E.key;
if (!get_space()) { GodotBody3D **n = c->get_body_ptr();
_set_transform(p_xform);
_set_inv_transform(inv_xform);
return;
}
//compute a FAKE linear velocity - this is easy
linear_velocity=(p_xform.origin - get_transform().origin)/p_step;
//compute a FAKE angular velocity, not so easy
Basis rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized();
Vector3 axis;
real_t angle;
rot.get_axis_angle(axis,angle);
axis.normalize();
angular_velocity=axis.normalized() * (angle/p_step);
linear_velocity = (p_xform.origin - get_transform().origin)/p_step;
if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query))
get_space()->body_add_to_state_query_list(&direct_state_query_list);
simulated_motion=true;
_set_transform(p_xform);
}
*/
void Body3DSW::wakeup_neighbours() {
for (const KeyValue<Constraint3DSW *, int> &E : constraint_map) {
const Constraint3DSW *c = E.key;
Body3DSW **n = c->get_body_ptr();
int bc = c->get_body_count(); int bc = c->get_body_count();
for (int i = 0; i < bc; i++) { for (int i = 0; i < bc; i++) {
if (i == E.value) { if (i == E.value) {
continue; continue;
} }
Body3DSW *b = n[i]; GodotBody3D *b = n[i];
if (b->mode < PhysicsServer3D::BODY_MODE_DYNAMIC) { if (b->mode < PhysicsServer3D::BODY_MODE_DYNAMIC) {
continue; continue;
} }
@ -709,7 +676,7 @@ void Body3DSW::wakeup_neighbours() {
} }
} }
void Body3DSW::call_queries() { void GodotBody3D::call_queries() {
if (fi_callback_data) { if (fi_callback_data) {
if (!fi_callback_data->callable.get_object()) { if (!fi_callback_data->callable.get_object()) {
set_force_integration_callback(Callable()); set_force_integration_callback(Callable());
@ -729,7 +696,7 @@ void Body3DSW::call_queries() {
} }
} }
bool Body3DSW::sleep_test(real_t p_step) { bool GodotBody3D::sleep_test(real_t p_step) {
if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) { if (mode == PhysicsServer3D::BODY_MODE_STATIC || mode == PhysicsServer3D::BODY_MODE_KINEMATIC) {
return true; return true;
} else if (!can_sleep) { } else if (!can_sleep) {
@ -746,12 +713,12 @@ bool Body3DSW::sleep_test(real_t p_step) {
} }
} }
void Body3DSW::set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback) { void GodotBody3D::set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback) {
body_state_callback_instance = p_instance; body_state_callback_instance = p_instance;
body_state_callback = p_callback; body_state_callback = p_callback;
} }
void Body3DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) { void GodotBody3D::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
if (p_callable.get_object()) { if (p_callable.get_object()) {
if (!fi_callback_data) { if (!fi_callback_data) {
fi_callback_data = memnew(ForceIntegrationCallbackData); fi_callback_data = memnew(ForceIntegrationCallbackData);
@ -764,23 +731,23 @@ void Body3DSW::set_force_integration_callback(const Callable &p_callable, const
} }
} }
PhysicsDirectBodyState3DSW *Body3DSW::get_direct_state() { GodotPhysicsDirectBodyState3D *GodotBody3D::get_direct_state() {
if (!direct_state) { if (!direct_state) {
direct_state = memnew(PhysicsDirectBodyState3DSW); direct_state = memnew(GodotPhysicsDirectBodyState3D);
direct_state->body = this; direct_state->body = this;
} }
return direct_state; return direct_state;
} }
Body3DSW::Body3DSW() : GodotBody3D::GodotBody3D() :
CollisionObject3DSW(TYPE_BODY), GodotCollisionObject3D(TYPE_BODY),
active_list(this), active_list(this),
mass_properties_update_list(this), mass_properties_update_list(this),
direct_state_query_list(this) { direct_state_query_list(this) {
_set_static(false); _set_static(false);
} }
Body3DSW::~Body3DSW() { GodotBody3D::~GodotBody3D() {
if (fi_callback_data) { if (fi_callback_data) {
memdelete(fi_callback_data); memdelete(fi_callback_data);
} }

53
servers/physics_3d/body_3d_sw.h → servers/physics_3d/godot_body_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* body_3d_sw.h */ /* godot_body_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,17 +28,18 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef BODY_3D_SW_H #ifndef GODOT_BODY_3D_H
#define BODY_3D_SW_H #define GODOT_BODY_3D_H
#include "godot_area_3d.h"
#include "godot_collision_object_3d.h"
#include "area_3d_sw.h"
#include "collision_object_3d_sw.h"
#include "core/templates/vset.h" #include "core/templates/vset.h"
class Constraint3DSW; class GodotConstraint3D;
class PhysicsDirectBodyState3DSW; class GodotPhysicsDirectBodyState3D;
class Body3DSW : public CollisionObject3DSW { class GodotBody3D : public GodotCollisionObject3D {
PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_DYNAMIC; PhysicsServer3D::BodyMode mode = PhysicsServer3D::BODY_MODE_DYNAMIC;
Vector3 linear_velocity; Vector3 linear_velocity;
@ -85,9 +86,9 @@ class Body3DSW : public CollisionObject3DSW {
real_t area_angular_damp = 0.0; real_t area_angular_damp = 0.0;
real_t area_linear_damp = 0.0; real_t area_linear_damp = 0.0;
SelfList<Body3DSW> active_list; SelfList<GodotBody3D> active_list;
SelfList<Body3DSW> mass_properties_update_list; SelfList<GodotBody3D> mass_properties_update_list;
SelfList<Body3DSW> direct_state_query_list; SelfList<GodotBody3D> direct_state_query_list;
VSet<RID> exceptions; VSet<RID> exceptions;
bool omit_force_integration = false; bool omit_force_integration = false;
@ -101,7 +102,7 @@ class Body3DSW : public CollisionObject3DSW {
virtual void _shapes_changed(); virtual void _shapes_changed();
Transform3D new_transform; Transform3D new_transform;
Map<Constraint3DSW *, int> constraint_map; Map<GodotConstraint3D *, int> constraint_map;
Vector<AreaCMP> areas; Vector<AreaCMP> areas;
@ -130,23 +131,23 @@ class Body3DSW : public CollisionObject3DSW {
ForceIntegrationCallbackData *fi_callback_data = nullptr; ForceIntegrationCallbackData *fi_callback_data = nullptr;
PhysicsDirectBodyState3DSW *direct_state = nullptr; GodotPhysicsDirectBodyState3D *direct_state = nullptr;
uint64_t island_step = 0; uint64_t island_step = 0;
_FORCE_INLINE_ void _compute_area_gravity_and_damping(const Area3DSW *p_area); _FORCE_INLINE_ void _compute_area_gravity_and_damping(const GodotArea3D *p_area);
_FORCE_INLINE_ void _update_transform_dependant(); _FORCE_INLINE_ void _update_transform_dependant();
friend class PhysicsDirectBodyState3DSW; // i give up, too many functions to expose friend class GodotPhysicsDirectBodyState3D; // i give up, too many functions to expose
public: public:
void set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback); void set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback);
void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant()); void set_force_integration_callback(const Callable &p_callable, const Variant &p_udata = Variant());
PhysicsDirectBodyState3DSW *get_direct_state(); GodotPhysicsDirectBodyState3D *get_direct_state();
_FORCE_INLINE_ void add_area(Area3DSW *p_area) { _FORCE_INLINE_ void add_area(GodotArea3D *p_area) {
int index = areas.find(AreaCMP(p_area)); int index = areas.find(AreaCMP(p_area));
if (index > -1) { if (index > -1) {
areas.write[index].refCount += 1; areas.write[index].refCount += 1;
@ -155,7 +156,7 @@ public:
} }
} }
_FORCE_INLINE_ void remove_area(Area3DSW *p_area) { _FORCE_INLINE_ void remove_area(GodotArea3D *p_area) {
int index = areas.find(AreaCMP(p_area)); int index = areas.find(AreaCMP(p_area));
if (index > -1) { if (index > -1) {
areas.write[index].refCount -= 1; areas.write[index].refCount -= 1;
@ -185,9 +186,9 @@ public:
_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _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 set_island_step(uint64_t p_step) { island_step = p_step; }
_FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; } _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint, int p_pos) { constraint_map[p_constraint] = p_pos; }
_FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraint_map.erase(p_constraint); } _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraint_map.erase(p_constraint); }
const Map<Constraint3DSW *, int> &get_constraint_map() const { return constraint_map; } const Map<GodotConstraint3D *, int> &get_constraint_map() const { return constraint_map; }
_FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); } _FORCE_INLINE_ void clear_constraint_map() { constraint_map.clear(); }
_FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; } _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration = p_omit_force_integration; }
@ -275,7 +276,7 @@ public:
_FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; } _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd = p_enable; }
_FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; } _FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }
void set_space(Space3DSW *p_space); void set_space(GodotSpace3D *p_space);
void update_mass_properties(); void update_mass_properties();
void reset_mass_properties(); void reset_mass_properties();
@ -317,13 +318,13 @@ public:
bool sleep_test(real_t p_step); bool sleep_test(real_t p_step);
Body3DSW(); GodotBody3D();
~Body3DSW(); ~GodotBody3D();
}; };
//add contact inline //add contact inline
void Body3DSW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) { void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) {
int c_max = contacts.size(); int c_max = contacts.size();
if (c_max == 0) { if (c_max == 0) {
@ -365,4 +366,4 @@ void Body3DSW::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_no
c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos; c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
} }
#endif // BODY_3D_SW_H #endif // GODOT_BODY_3D_H

78
servers/physics_3d/body_direct_state_3d_sw.cpp → servers/physics_3d/godot_body_direct_state_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* body_direct_state_3d_sw.cpp */ /* godot_body_direct_state_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,163 +28,163 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "body_direct_state_3d_sw.h" #include "godot_body_direct_state_3d.h"
#include "body_3d_sw.h" #include "godot_body_3d.h"
#include "space_3d_sw.h" #include "godot_space_3d.h"
Vector3 PhysicsDirectBodyState3DSW::get_total_gravity() const { Vector3 GodotPhysicsDirectBodyState3D::get_total_gravity() const {
return body->gravity; return body->gravity;
} }
real_t PhysicsDirectBodyState3DSW::get_total_angular_damp() const { real_t GodotPhysicsDirectBodyState3D::get_total_angular_damp() const {
return body->area_angular_damp; return body->area_angular_damp;
} }
real_t PhysicsDirectBodyState3DSW::get_total_linear_damp() const { real_t GodotPhysicsDirectBodyState3D::get_total_linear_damp() const {
return body->area_linear_damp; return body->area_linear_damp;
} }
Vector3 PhysicsDirectBodyState3DSW::get_center_of_mass() const { Vector3 GodotPhysicsDirectBodyState3D::get_center_of_mass() const {
return body->get_center_of_mass(); return body->get_center_of_mass();
} }
Basis PhysicsDirectBodyState3DSW::get_principal_inertia_axes() const { Basis GodotPhysicsDirectBodyState3D::get_principal_inertia_axes() const {
return body->get_principal_inertia_axes(); return body->get_principal_inertia_axes();
} }
real_t PhysicsDirectBodyState3DSW::get_inverse_mass() const { real_t GodotPhysicsDirectBodyState3D::get_inverse_mass() const {
return body->get_inv_mass(); return body->get_inv_mass();
} }
Vector3 PhysicsDirectBodyState3DSW::get_inverse_inertia() const { Vector3 GodotPhysicsDirectBodyState3D::get_inverse_inertia() const {
return body->get_inv_inertia(); return body->get_inv_inertia();
} }
Basis PhysicsDirectBodyState3DSW::get_inverse_inertia_tensor() const { Basis GodotPhysicsDirectBodyState3D::get_inverse_inertia_tensor() const {
return body->get_inv_inertia_tensor(); return body->get_inv_inertia_tensor();
} }
void PhysicsDirectBodyState3DSW::set_linear_velocity(const Vector3 &p_velocity) { void GodotPhysicsDirectBodyState3D::set_linear_velocity(const Vector3 &p_velocity) {
body->wakeup(); body->wakeup();
body->set_linear_velocity(p_velocity); body->set_linear_velocity(p_velocity);
} }
Vector3 PhysicsDirectBodyState3DSW::get_linear_velocity() const { Vector3 GodotPhysicsDirectBodyState3D::get_linear_velocity() const {
return body->get_linear_velocity(); return body->get_linear_velocity();
} }
void PhysicsDirectBodyState3DSW::set_angular_velocity(const Vector3 &p_velocity) { void GodotPhysicsDirectBodyState3D::set_angular_velocity(const Vector3 &p_velocity) {
body->wakeup(); body->wakeup();
body->set_angular_velocity(p_velocity); body->set_angular_velocity(p_velocity);
} }
Vector3 PhysicsDirectBodyState3DSW::get_angular_velocity() const { Vector3 GodotPhysicsDirectBodyState3D::get_angular_velocity() const {
return body->get_angular_velocity(); return body->get_angular_velocity();
} }
void PhysicsDirectBodyState3DSW::set_transform(const Transform3D &p_transform) { void GodotPhysicsDirectBodyState3D::set_transform(const Transform3D &p_transform) {
body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform);
} }
Transform3D PhysicsDirectBodyState3DSW::get_transform() const { Transform3D GodotPhysicsDirectBodyState3D::get_transform() const {
return body->get_transform(); return body->get_transform();
} }
Vector3 PhysicsDirectBodyState3DSW::get_velocity_at_local_position(const Vector3 &p_position) const { Vector3 GodotPhysicsDirectBodyState3D::get_velocity_at_local_position(const Vector3 &p_position) const {
return body->get_velocity_in_local_point(p_position); return body->get_velocity_in_local_point(p_position);
} }
void PhysicsDirectBodyState3DSW::add_central_force(const Vector3 &p_force) { void GodotPhysicsDirectBodyState3D::add_central_force(const Vector3 &p_force) {
body->wakeup(); body->wakeup();
body->add_central_force(p_force); body->add_central_force(p_force);
} }
void PhysicsDirectBodyState3DSW::add_force(const Vector3 &p_force, const Vector3 &p_position) { void GodotPhysicsDirectBodyState3D::add_force(const Vector3 &p_force, const Vector3 &p_position) {
body->wakeup(); body->wakeup();
body->add_force(p_force, p_position); body->add_force(p_force, p_position);
} }
void PhysicsDirectBodyState3DSW::add_torque(const Vector3 &p_torque) { void GodotPhysicsDirectBodyState3D::add_torque(const Vector3 &p_torque) {
body->wakeup(); body->wakeup();
body->add_torque(p_torque); body->add_torque(p_torque);
} }
void PhysicsDirectBodyState3DSW::apply_central_impulse(const Vector3 &p_impulse) { void GodotPhysicsDirectBodyState3D::apply_central_impulse(const Vector3 &p_impulse) {
body->wakeup(); body->wakeup();
body->apply_central_impulse(p_impulse); body->apply_central_impulse(p_impulse);
} }
void PhysicsDirectBodyState3DSW::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) { void GodotPhysicsDirectBodyState3D::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
body->wakeup(); body->wakeup();
body->apply_impulse(p_impulse, p_position); body->apply_impulse(p_impulse, p_position);
} }
void PhysicsDirectBodyState3DSW::apply_torque_impulse(const Vector3 &p_impulse) { void GodotPhysicsDirectBodyState3D::apply_torque_impulse(const Vector3 &p_impulse) {
body->wakeup(); body->wakeup();
body->apply_torque_impulse(p_impulse); body->apply_torque_impulse(p_impulse);
} }
void PhysicsDirectBodyState3DSW::set_sleep_state(bool p_sleep) { void GodotPhysicsDirectBodyState3D::set_sleep_state(bool p_sleep) {
body->set_active(!p_sleep); body->set_active(!p_sleep);
} }
bool PhysicsDirectBodyState3DSW::is_sleeping() const { bool GodotPhysicsDirectBodyState3D::is_sleeping() const {
return !body->is_active(); return !body->is_active();
} }
int PhysicsDirectBodyState3DSW::get_contact_count() const { int GodotPhysicsDirectBodyState3D::get_contact_count() const {
return body->contact_count; return body->contact_count;
} }
Vector3 PhysicsDirectBodyState3DSW::get_contact_local_position(int p_contact_idx) const { Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
return body->contacts[p_contact_idx].local_pos; return body->contacts[p_contact_idx].local_pos;
} }
Vector3 PhysicsDirectBodyState3DSW::get_contact_local_normal(int p_contact_idx) const { Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_normal(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
return body->contacts[p_contact_idx].local_normal; return body->contacts[p_contact_idx].local_normal;
} }
real_t PhysicsDirectBodyState3DSW::get_contact_impulse(int p_contact_idx) const { real_t GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const {
return 0.0f; // Only implemented for bullet return 0.0f; // Only implemented for bullet
} }
int PhysicsDirectBodyState3DSW::get_contact_local_shape(int p_contact_idx) const { int GodotPhysicsDirectBodyState3D::get_contact_local_shape(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
return body->contacts[p_contact_idx].local_shape; return body->contacts[p_contact_idx].local_shape;
} }
RID PhysicsDirectBodyState3DSW::get_contact_collider(int p_contact_idx) const { RID GodotPhysicsDirectBodyState3D::get_contact_collider(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID()); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
return body->contacts[p_contact_idx].collider; return body->contacts[p_contact_idx].collider;
} }
Vector3 PhysicsDirectBodyState3DSW::get_contact_collider_position(int p_contact_idx) const { Vector3 GodotPhysicsDirectBodyState3D::get_contact_collider_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
return body->contacts[p_contact_idx].collider_pos; return body->contacts[p_contact_idx].collider_pos;
} }
ObjectID PhysicsDirectBodyState3DSW::get_contact_collider_id(int p_contact_idx) const { ObjectID GodotPhysicsDirectBodyState3D::get_contact_collider_id(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID()); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID());
return body->contacts[p_contact_idx].collider_instance_id; return body->contacts[p_contact_idx].collider_instance_id;
} }
int PhysicsDirectBodyState3DSW::get_contact_collider_shape(int p_contact_idx) const { int GodotPhysicsDirectBodyState3D::get_contact_collider_shape(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
return body->contacts[p_contact_idx].collider_shape; return body->contacts[p_contact_idx].collider_shape;
} }
Vector3 PhysicsDirectBodyState3DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const { Vector3 GodotPhysicsDirectBodyState3D::get_contact_collider_velocity_at_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3()); ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
return body->contacts[p_contact_idx].collider_velocity_at_pos; return body->contacts[p_contact_idx].collider_velocity_at_pos;
} }
PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() { PhysicsDirectSpaceState3D *GodotPhysicsDirectBodyState3D::get_space_state() {
return body->get_space()->get_direct_state(); return body->get_space()->get_direct_state();
} }
real_t PhysicsDirectBodyState3DSW::get_step() const { real_t GodotPhysicsDirectBodyState3D::get_step() const {
return body->get_space()->get_last_step(); return body->get_space()->get_last_step();
} }

16
servers/physics_3d/body_direct_state_3d_sw.h → servers/physics_3d/godot_body_direct_state_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* body_direct_state_3d_sw.h */ /* godot_body_direct_state_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,18 +28,18 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef BODY_DIRECT_STATE_3D_SW_H #ifndef GODOT_BODY_DIRECT_STATE_3D_H
#define BODY_DIRECT_STATE_3D_SW_H #define GODOT_BODY_DIRECT_STATE_3D_H
#include "servers/physics_server_3d.h" #include "servers/physics_server_3d.h"
class Body3DSW; class GodotBody3D;
class PhysicsDirectBodyState3DSW : public PhysicsDirectBodyState3D { class GodotPhysicsDirectBodyState3D : public PhysicsDirectBodyState3D {
GDCLASS(PhysicsDirectBodyState3DSW, PhysicsDirectBodyState3D); GDCLASS(GodotPhysicsDirectBodyState3D, PhysicsDirectBodyState3D);
public: public:
Body3DSW *body = nullptr; GodotBody3D *body = nullptr;
virtual Vector3 get_total_gravity() const override; virtual Vector3 get_total_gravity() const override;
virtual real_t get_total_angular_damp() const override; virtual real_t get_total_angular_damp() const override;
@ -91,4 +91,4 @@ public:
virtual real_t get_step() const override; virtual real_t get_step() const override;
}; };
#endif // BODY_DIRECT_STATE_3D_SW_H #endif // GODOT_BODY_DIRECT_STATE_3D_H

73
servers/physics_3d/body_pair_3d_sw.cpp → servers/physics_3d/godot_body_pair_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* body_pair_3d_sw.cpp */ /* godot_body_pair_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,11 +28,12 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "body_pair_3d_sw.h" #include "godot_body_pair_3d.h"
#include "godot_collision_solver_3d.h"
#include "godot_space_3d.h"
#include "collision_solver_3d_sw.h"
#include "core/os/os.h" #include "core/os/os.h"
#include "space_3d_sw.h"
/* /*
#define NO_ACCUMULATE_IMPULSES #define NO_ACCUMULATE_IMPULSES
@ -49,12 +50,12 @@
#define MIN_VELOCITY 0.0001 #define MIN_VELOCITY 0.0001
#define MAX_BIAS_ROTATION (Math_PI / 8) #define MAX_BIAS_ROTATION (Math_PI / 8)
void BodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
BodyPair3DSW *pair = (BodyPair3DSW *)p_userdata; GodotBodyPair3D *pair = (GodotBodyPair3D *)p_userdata;
pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B); pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B);
} }
void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) { void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
// check if we already have the contact // check if we already have the contact
//Vector3 local_A = A->get_inv_transform().xform(p_point_A); //Vector3 local_A = A->get_inv_transform().xform(p_point_A);
@ -135,7 +136,7 @@ void BodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int p_index_
} }
} }
void BodyPair3DSW::validate_contacts() { void GodotBodyPair3D::validate_contacts() {
//make sure to erase contacts that are no longer valid //make sure to erase contacts that are no longer valid
real_t contact_max_separation = space->get_contact_max_separation(); real_t contact_max_separation = space->get_contact_max_separation();
@ -161,7 +162,7 @@ void BodyPair3DSW::validate_contacts() {
} }
} }
bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform3D &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform3D &p_xform_B) { bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B) {
Vector3 motion = p_A->get_linear_velocity() * p_step; Vector3 motion = p_A->get_linear_velocity() * p_step;
real_t mlen = motion.length(); real_t mlen = motion.length();
if (mlen < CMP_EPSILON) { if (mlen < CMP_EPSILON) {
@ -203,15 +204,15 @@ bool BodyPair3DSW::_test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const
return true; return true;
} }
real_t combine_bounce(Body3DSW *A, Body3DSW *B) { real_t combine_bounce(GodotBody3D *A, GodotBody3D *B) {
return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1); return CLAMP(A->get_bounce() + B->get_bounce(), 0, 1);
} }
real_t combine_friction(Body3DSW *A, Body3DSW *B) { real_t combine_friction(GodotBody3D *A, GodotBody3D *B) {
return ABS(MIN(A->get_friction(), B->get_friction())); return ABS(MIN(A->get_friction(), B->get_friction()));
} }
bool BodyPair3DSW::setup(real_t p_step) { bool GodotBodyPair3D::setup(real_t p_step) {
if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) { if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
collided = false; collided = false;
return false; return false;
@ -242,10 +243,10 @@ bool BodyPair3DSW::setup(real_t p_step) {
xform_Bu.origin -= offset_A; xform_Bu.origin -= offset_A;
Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B); Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B);
Shape3DSW *shape_A_ptr = A->get_shape(shape_A); GodotShape3D *shape_A_ptr = A->get_shape(shape_A);
Shape3DSW *shape_B_ptr = B->get_shape(shape_B); GodotShape3D *shape_B_ptr = B->get_shape(shape_B);
collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
if (!collided) { if (!collided) {
//test ccd (currently just a raycast) //test ccd (currently just a raycast)
@ -264,7 +265,7 @@ bool BodyPair3DSW::setup(real_t p_step) {
return true; return true;
} }
bool BodyPair3DSW::pre_solve(real_t p_step) { bool GodotBodyPair3D::pre_solve(real_t p_step) {
if (!collided) { if (!collided) {
return false; return false;
} }
@ -273,8 +274,8 @@ bool BodyPair3DSW::pre_solve(real_t p_step) {
real_t bias = (real_t)0.3; real_t bias = (real_t)0.3;
Shape3DSW *shape_A_ptr = A->get_shape(shape_A); GodotShape3D *shape_A_ptr = A->get_shape(shape_A);
Shape3DSW *shape_B_ptr = B->get_shape(shape_B); GodotShape3D *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() || shape_B_ptr->get_custom_bias()) {
if (shape_A_ptr->get_custom_bias() == 0) { if (shape_A_ptr->get_custom_bias() == 0) {
@ -380,7 +381,7 @@ bool BodyPair3DSW::pre_solve(real_t p_step) {
return do_process; return do_process;
} }
void BodyPair3DSW::solve(real_t p_step) { void GodotBodyPair3D::solve(real_t p_step) {
if (!collided) { if (!collided) {
return; return;
} }
@ -523,8 +524,8 @@ void BodyPair3DSW::solve(real_t p_step) {
} }
} }
BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B) : GodotBodyPair3D::GodotBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotBody3D *p_B, int p_shape_B) :
BodyContact3DSW(_arr, 2) { GodotBodyContact3D(_arr, 2) {
A = p_A; A = p_A;
B = p_B; B = p_B;
shape_A = p_shape_A; shape_A = p_shape_A;
@ -534,17 +535,17 @@ BodyPair3DSW::BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_sh
B->add_constraint(this, 1); B->add_constraint(this, 1);
} }
BodyPair3DSW::~BodyPair3DSW() { GodotBodyPair3D::~GodotBodyPair3D() {
A->remove_constraint(this); A->remove_constraint(this);
B->remove_constraint(this); B->remove_constraint(this);
} }
void BodySoftBodyPair3DSW::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { void GodotBodySoftBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
BodySoftBodyPair3DSW *pair = (BodySoftBodyPair3DSW *)p_userdata; GodotBodySoftBodyPair3D *pair = (GodotBodySoftBodyPair3D *)p_userdata;
pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B); pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B);
} }
void BodySoftBodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) { void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
Vector3 local_A = body->get_inv_transform().xform(p_point_A); Vector3 local_A = body->get_inv_transform().xform(p_point_A);
Vector3 local_B = p_point_B - soft_body->get_node_position(p_index_B); Vector3 local_B = p_point_B - soft_body->get_node_position(p_index_B);
@ -582,7 +583,7 @@ void BodySoftBodyPair3DSW::contact_added_callback(const Vector3 &p_point_A, int
contacts.push_back(contact); contacts.push_back(contact);
} }
void BodySoftBodyPair3DSW::validate_contacts() { void GodotBodySoftBodyPair3D::validate_contacts() {
// Make sure to erase contacts that are no longer valid. // Make sure to erase contacts that are no longer valid.
const Transform3D &transform_A = body->get_transform(); const Transform3D &transform_A = body->get_transform();
@ -612,7 +613,7 @@ void BodySoftBodyPair3DSW::validate_contacts() {
contacts.resize(contact_count); contacts.resize(contact_count);
} }
bool BodySoftBodyPair3DSW::setup(real_t p_step) { bool GodotBodySoftBodyPair3D::setup(real_t p_step) {
if (!body->interacts_with(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) { if (!body->interacts_with(soft_body) || body->has_exception(soft_body->get_self()) || soft_body->has_exception(body->get_self())) {
collided = false; collided = false;
return false; return false;
@ -638,15 +639,15 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
validate_contacts(); validate_contacts();
Shape3DSW *shape_A_ptr = body->get_shape(body_shape); GodotShape3D *shape_A_ptr = body->get_shape(body_shape);
Shape3DSW *shape_B_ptr = soft_body->get_shape(0); GodotShape3D *shape_B_ptr = soft_body->get_shape(0);
collided = CollisionSolver3DSW::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis); collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
return collided; return collided;
} }
bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) { bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
if (!collided) { if (!collided) {
return false; return false;
} }
@ -655,7 +656,7 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) {
real_t bias = (real_t)0.3; real_t bias = (real_t)0.3;
Shape3DSW *shape_A_ptr = body->get_shape(body_shape); GodotShape3D *shape_A_ptr = body->get_shape(body_shape);
if (shape_A_ptr->get_custom_bias()) { if (shape_A_ptr->get_custom_bias()) {
bias = shape_A_ptr->get_custom_bias(); bias = shape_A_ptr->get_custom_bias();
@ -753,7 +754,7 @@ bool BodySoftBodyPair3DSW::pre_solve(real_t p_step) {
return do_process; return do_process;
} }
void BodySoftBodyPair3DSW::solve(real_t p_step) { void GodotBodySoftBodyPair3D::solve(real_t p_step) {
if (!collided) { if (!collided) {
return; return;
} }
@ -891,8 +892,8 @@ void BodySoftBodyPair3DSW::solve(real_t p_step) {
} }
} }
BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B) : GodotBodySoftBodyPair3D::GodotBodySoftBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotSoftBody3D *p_B) :
BodyContact3DSW(&body, 1) { GodotBodyContact3D(&body, 1) {
body = p_A; body = p_A;
soft_body = p_B; soft_body = p_B;
body_shape = p_shape_A; body_shape = p_shape_A;
@ -901,7 +902,7 @@ BodySoftBodyPair3DSW::BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBod
soft_body->add_constraint(this); soft_body->add_constraint(this);
} }
BodySoftBodyPair3DSW::~BodySoftBodyPair3DSW() { GodotBodySoftBodyPair3D::~GodotBodySoftBodyPair3D() {
body->remove_constraint(this); body->remove_constraint(this);
soft_body->remove_constraint(this); soft_body->remove_constraint(this);
} }

49
servers/physics_3d/body_pair_3d_sw.h → servers/physics_3d/godot_body_pair_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* body_pair_3d_sw.h */ /* godot_body_pair_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,15 +28,16 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef BODY_PAIR_3D_SW_H #ifndef GODOT_BODY_PAIR_3D_H
#define BODY_PAIR_3D_SW_H #define GODOT_BODY_PAIR_3D_H
#include "godot_body_3d.h"
#include "godot_constraint_3d.h"
#include "godot_soft_body_3d.h"
#include "body_3d_sw.h"
#include "constraint_3d_sw.h"
#include "core/templates/local_vector.h" #include "core/templates/local_vector.h"
#include "soft_body_3d_sw.h"
class BodyContact3DSW : public Constraint3DSW { class GodotBodyContact3D : public GodotConstraint3D {
protected: protected:
struct Contact { struct Contact {
Vector3 position; Vector3 position;
@ -59,25 +60,25 @@ protected:
Vector3 sep_axis; Vector3 sep_axis;
bool collided = false; bool collided = false;
Space3DSW *space = nullptr; GodotSpace3D *space = nullptr;
BodyContact3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : GodotBodyContact3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) :
Constraint3DSW(p_body_ptr, p_body_count) { GodotConstraint3D(p_body_ptr, p_body_count) {
} }
}; };
class BodyPair3DSW : public BodyContact3DSW { class GodotBodyPair3D : public GodotBodyContact3D {
enum { enum {
MAX_CONTACTS = 4 MAX_CONTACTS = 4
}; };
union { union {
struct { struct {
Body3DSW *A; GodotBody3D *A;
Body3DSW *B; GodotBody3D *B;
}; };
Body3DSW *_arr[2] = { nullptr, nullptr }; GodotBody3D *_arr[2] = { nullptr, nullptr };
}; };
int shape_A = 0; int shape_A = 0;
@ -98,20 +99,20 @@ class BodyPair3DSW : public BodyContact3DSW {
void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B); void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B);
void validate_contacts(); void validate_contacts();
bool _test_ccd(real_t p_step, Body3DSW *p_A, int p_shape_A, const Transform3D &p_xform_A, Body3DSW *p_B, int p_shape_B, const Transform3D &p_xform_B); bool _test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B);
public: public:
virtual bool setup(real_t p_step) override; virtual bool setup(real_t p_step) override;
virtual bool pre_solve(real_t p_step) override; virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override; virtual void solve(real_t p_step) override;
BodyPair3DSW(Body3DSW *p_A, int p_shape_A, Body3DSW *p_B, int p_shape_B); GodotBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotBody3D *p_B, int p_shape_B);
~BodyPair3DSW(); ~GodotBodyPair3D();
}; };
class BodySoftBodyPair3DSW : public BodyContact3DSW { class GodotBodySoftBodyPair3D : public GodotBodyContact3D {
Body3DSW *body = nullptr; GodotBody3D *body = nullptr;
SoftBody3DSW *soft_body = nullptr; GodotSoftBody3D *soft_body = nullptr;
int body_shape = 0; int body_shape = 0;
@ -133,11 +134,11 @@ public:
virtual bool pre_solve(real_t p_step) override; virtual bool pre_solve(real_t p_step) override;
virtual void solve(real_t p_step) override; virtual void solve(real_t p_step) override;
virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const override { return soft_body; } virtual GodotSoftBody3D *get_soft_body_ptr(int p_index) const override { return soft_body; }
virtual int get_soft_body_count() const override { return 1; } virtual int get_soft_body_count() const override { return 1; }
BodySoftBodyPair3DSW(Body3DSW *p_A, int p_shape_A, SoftBody3DSW *p_B); GodotBodySoftBodyPair3D(GodotBody3D *p_A, int p_shape_A, GodotSoftBody3D *p_B);
~BodySoftBodyPair3DSW(); ~GodotBodySoftBodyPair3D();
}; };
#endif // BODY_PAIR_3D_SW_H #endif // GODOT_BODY_PAIR_3D_H

8
servers/physics_3d/broad_phase_3d_sw.cpp → servers/physics_3d/godot_broad_phase_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* broad_phase_3d_sw.cpp */ /* godot_broad_phase_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,9 +28,9 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "broad_phase_3d_sw.h" #include "godot_broad_phase_3d.h"
BroadPhase3DSW::CreateFunction BroadPhase3DSW::create_func = nullptr; GodotBroadPhase3D::CreateFunction GodotBroadPhase3D::create_func = nullptr;
BroadPhase3DSW::~BroadPhase3DSW() { GodotBroadPhase3D::~GodotBroadPhase3D() {
} }

30
servers/physics_3d/broad_phase_3d_sw.h → servers/physics_3d/godot_broad_phase_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* broad_phase_3d_sw.h */ /* godot_broad_phase_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,45 +28,45 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef BROAD_PHASE_SW_H #ifndef GODOT_BROAD_PHASE_3D_H
#define BROAD_PHASE_SW_H #define GODOT_BROAD_PHASE_3D_H
#include "core/math/aabb.h" #include "core/math/aabb.h"
#include "core/math/math_funcs.h" #include "core/math/math_funcs.h"
class CollisionObject3DSW; class GodotCollisionObject3D;
class BroadPhase3DSW { class GodotBroadPhase3D {
public: public:
typedef BroadPhase3DSW *(*CreateFunction)(); typedef GodotBroadPhase3D *(*CreateFunction)();
static CreateFunction create_func; static CreateFunction create_func;
typedef uint32_t ID; typedef uint32_t ID;
typedef void *(*PairCallback)(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_userdata); typedef void *(*PairCallback)(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_userdata);
typedef void (*UnpairCallback)(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_userdata); typedef void (*UnpairCallback)(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_userdata);
// 0 is an invalid ID // 0 is an invalid ID
virtual ID create(CollisionObject3DSW *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0; virtual ID create(GodotCollisionObject3D *p_object_, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false) = 0;
virtual void move(ID p_id, const AABB &p_aabb) = 0; virtual void move(ID p_id, const AABB &p_aabb) = 0;
virtual void set_static(ID p_id, bool p_static) = 0; virtual void set_static(ID p_id, bool p_static) = 0;
virtual void remove(ID p_id) = 0; virtual void remove(ID p_id) = 0;
virtual CollisionObject3DSW *get_object(ID p_id) const = 0; virtual GodotCollisionObject3D *get_object(ID p_id) const = 0;
virtual bool is_static(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 get_subindex(ID p_id) const = 0;
virtual int cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; virtual int cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr) = 0;
virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr) = 0; virtual int cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **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_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 set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) = 0;
virtual void update() = 0; virtual void update() = 0;
virtual ~BroadPhase3DSW(); virtual ~GodotBroadPhase3D();
}; };
#endif // BROAD_PHASE__SW_H #endif // GODOT_BROAD_PHASE_3D_H

51
servers/physics_3d/broad_phase_3d_bvh.cpp → servers/physics_3d/godot_broad_phase_3d_bvh.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* broad_phase_3d_bvh.cpp */ /* godot_broad_phase_3d_bvh.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,55 +28,56 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "broad_phase_3d_bvh.h" #include "godot_broad_phase_3d_bvh.h"
#include "collision_object_3d_sw.h"
BroadPhase3DBVH::ID BroadPhase3DBVH::create(CollisionObject3DSW *p_object, int p_subindex, const AABB &p_aabb, bool p_static) { #include "godot_collision_object_3d.h"
GodotBroadPhase3DBVH::ID GodotBroadPhase3DBVH::create(GodotCollisionObject3D *p_object, int p_subindex, const AABB &p_aabb, bool p_static) {
ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care? ID oid = bvh.create(p_object, true, p_aabb, p_subindex, !p_static, 1 << p_object->get_type(), p_static ? 0 : 0xFFFFF); // Pair everything, don't care?
return oid + 1; return oid + 1;
} }
void BroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) { void GodotBroadPhase3DBVH::move(ID p_id, const AABB &p_aabb) {
bvh.move(p_id - 1, p_aabb); bvh.move(p_id - 1, p_aabb);
} }
void BroadPhase3DBVH::set_static(ID p_id, bool p_static) { void GodotBroadPhase3DBVH::set_static(ID p_id, bool p_static) {
CollisionObject3DSW *it = bvh.get(p_id - 1); GodotCollisionObject3D *it = bvh.get(p_id - 1);
bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care? bvh.set_pairable(p_id - 1, !p_static, 1 << it->get_type(), p_static ? 0 : 0xFFFFF, false); // Pair everything, don't care?
} }
void BroadPhase3DBVH::remove(ID p_id) { void GodotBroadPhase3DBVH::remove(ID p_id) {
bvh.erase(p_id - 1); bvh.erase(p_id - 1);
} }
CollisionObject3DSW *BroadPhase3DBVH::get_object(ID p_id) const { GodotCollisionObject3D *GodotBroadPhase3DBVH::get_object(ID p_id) const {
CollisionObject3DSW *it = bvh.get(p_id - 1); GodotCollisionObject3D *it = bvh.get(p_id - 1);
ERR_FAIL_COND_V(!it, nullptr); ERR_FAIL_COND_V(!it, nullptr);
return it; return it;
} }
bool BroadPhase3DBVH::is_static(ID p_id) const { bool GodotBroadPhase3DBVH::is_static(ID p_id) const {
return !bvh.is_pairable(p_id - 1); return !bvh.is_pairable(p_id - 1);
} }
int BroadPhase3DBVH::get_subindex(ID p_id) const { int GodotBroadPhase3DBVH::get_subindex(ID p_id) const {
return bvh.get_subindex(p_id - 1); return bvh.get_subindex(p_id - 1);
} }
int BroadPhase3DBVH::cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { int GodotBroadPhase3DBVH::cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) {
return bvh.cull_point(p_point, p_results, p_max_results, p_result_indices); return bvh.cull_point(p_point, p_results, p_max_results, p_result_indices);
} }
int BroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { int GodotBroadPhase3DBVH::cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) {
return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices); return bvh.cull_segment(p_from, p_to, p_results, p_max_results, p_result_indices);
} }
int BroadPhase3DBVH::cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices) { int GodotBroadPhase3DBVH::cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices) {
return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices); return bvh.cull_aabb(p_aabb, p_results, p_max_results, p_result_indices);
} }
void *BroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B) { void *GodotBroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B) {
BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self); GodotBroadPhase3DBVH *bpo = (GodotBroadPhase3DBVH *)(self);
if (!bpo->pair_callback) { if (!bpo->pair_callback) {
return nullptr; return nullptr;
} }
@ -84,8 +85,8 @@ void *BroadPhase3DBVH::_pair_callback(void *self, uint32_t p_A, CollisionObject3
return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata); return bpo->pair_callback(p_object_A, subindex_A, p_object_B, subindex_B, bpo->pair_userdata);
} }
void BroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject3DSW *p_object_A, int subindex_A, uint32_t p_B, CollisionObject3DSW *p_object_B, int subindex_B, void *pairdata) { void GodotBroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, GodotCollisionObject3D *p_object_A, int subindex_A, uint32_t p_B, GodotCollisionObject3D *p_object_B, int subindex_B, void *pairdata) {
BroadPhase3DBVH *bpo = (BroadPhase3DBVH *)(self); GodotBroadPhase3DBVH *bpo = (GodotBroadPhase3DBVH *)(self);
if (!bpo->unpair_callback) { if (!bpo->unpair_callback) {
return; return;
} }
@ -93,25 +94,25 @@ void BroadPhase3DBVH::_unpair_callback(void *self, uint32_t p_A, CollisionObject
bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata); bpo->unpair_callback(p_object_A, subindex_A, p_object_B, subindex_B, pairdata, bpo->unpair_userdata);
} }
void BroadPhase3DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) { void GodotBroadPhase3DBVH::set_pair_callback(PairCallback p_pair_callback, void *p_userdata) {
pair_callback = p_pair_callback; pair_callback = p_pair_callback;
pair_userdata = p_userdata; pair_userdata = p_userdata;
} }
void BroadPhase3DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) { void GodotBroadPhase3DBVH::set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata) {
unpair_callback = p_unpair_callback; unpair_callback = p_unpair_callback;
unpair_userdata = p_userdata; unpair_userdata = p_userdata;
} }
void BroadPhase3DBVH::update() { void GodotBroadPhase3DBVH::update() {
bvh.update(); bvh.update();
} }
BroadPhase3DSW *BroadPhase3DBVH::_create() { GodotBroadPhase3D *GodotBroadPhase3DBVH::_create() {
return memnew(BroadPhase3DBVH); return memnew(GodotBroadPhase3DBVH);
} }
BroadPhase3DBVH::BroadPhase3DBVH() { GodotBroadPhase3DBVH::GodotBroadPhase3DBVH() {
bvh.set_pair_callback(_pair_callback, this); bvh.set_pair_callback(_pair_callback, this);
bvh.set_unpair_callback(_unpair_callback, this); bvh.set_unpair_callback(_unpair_callback, this);
} }

33
servers/physics_3d/broad_phase_3d_bvh.h → servers/physics_3d/godot_broad_phase_3d_bvh.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* broad_phase_3d_bvh.h */ /* godot_broad_phase_3d_bvh.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,17 +28,18 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef BROAD_PHASE_3D_BVH_H #ifndef GODOT_BROAD_PHASE_3D_BVH_H
#define BROAD_PHASE_3D_BVH_H #define GODOT_BROAD_PHASE_3D_BVH_H
#include "godot_broad_phase_3d.h"
#include "broad_phase_3d_sw.h"
#include "core/math/bvh.h" #include "core/math/bvh.h"
class BroadPhase3DBVH : public BroadPhase3DSW { class GodotBroadPhase3DBVH : public GodotBroadPhase3D {
BVH_Manager<CollisionObject3DSW, true, 128> bvh; BVH_Manager<GodotCollisionObject3D, true, 128> bvh;
static void *_pair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int); static void *_pair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int);
static void _unpair_callback(void *, uint32_t, CollisionObject3DSW *, int, uint32_t, CollisionObject3DSW *, int, void *); static void _unpair_callback(void *, uint32_t, GodotCollisionObject3D *, int, uint32_t, GodotCollisionObject3D *, int, void *);
PairCallback pair_callback = nullptr; PairCallback pair_callback = nullptr;
void *pair_userdata = nullptr; void *pair_userdata = nullptr;
@ -47,26 +48,26 @@ class BroadPhase3DBVH : public BroadPhase3DSW {
public: public:
// 0 is an invalid ID // 0 is an invalid ID
virtual ID create(CollisionObject3DSW *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false); virtual ID create(GodotCollisionObject3D *p_object, int p_subindex = 0, const AABB &p_aabb = AABB(), bool p_static = false);
virtual void move(ID p_id, const AABB &p_aabb); virtual void move(ID p_id, const AABB &p_aabb);
virtual void set_static(ID p_id, bool p_static); virtual void set_static(ID p_id, bool p_static);
virtual void remove(ID p_id); virtual void remove(ID p_id);
virtual CollisionObject3DSW *get_object(ID p_id) const; virtual GodotCollisionObject3D *get_object(ID p_id) const;
virtual bool is_static(ID p_id) const; virtual bool is_static(ID p_id) const;
virtual int get_subindex(ID p_id) const; virtual int get_subindex(ID p_id) const;
virtual int cull_point(const Vector3 &p_point, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); virtual int cull_point(const Vector3 &p_point, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr);
virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); virtual int cull_segment(const Vector3 &p_from, const Vector3 &p_to, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr);
virtual int cull_aabb(const AABB &p_aabb, CollisionObject3DSW **p_results, int p_max_results, int *p_result_indices = nullptr); virtual int cull_aabb(const AABB &p_aabb, GodotCollisionObject3D **p_results, int p_max_results, int *p_result_indices = nullptr);
virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata); virtual void set_pair_callback(PairCallback p_pair_callback, void *p_userdata);
virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata); virtual void set_unpair_callback(UnpairCallback p_unpair_callback, void *p_userdata);
virtual void update(); virtual void update();
static BroadPhase3DSW *_create(); static GodotBroadPhase3D *_create();
BroadPhase3DBVH(); GodotBroadPhase3DBVH();
}; };
#endif // BROAD_PHASE_3D_BVH_H #endif // GODOT_BROAD_PHASE_3D_BVH_H

49
servers/physics_3d/collision_object_3d_sw.cpp → servers/physics_3d/godot_collision_object_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* collision_object_3d_sw.cpp */ /* godot_collision_object_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,11 +28,12 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "collision_object_3d_sw.h" #include "godot_collision_object_3d.h"
#include "servers/physics_3d/physics_server_3d_sw.h"
#include "space_3d_sw.h"
void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform3D &p_transform, bool p_disabled) { #include "godot_physics_server_3d.h"
#include "godot_space_3d.h"
void GodotCollisionObject3D::add_shape(GodotShape3D *p_shape, const Transform3D &p_transform, bool p_disabled) {
Shape s; Shape s;
s.shape = p_shape; s.shape = p_shape;
s.xform = p_transform; s.xform = p_transform;
@ -43,35 +44,35 @@ void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform3D &p_tra
p_shape->add_owner(this); p_shape->add_owner(this);
if (!pending_shape_update_list.in_list()) { if (!pending_shape_update_list.in_list()) {
PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
} }
} }
void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) { void GodotCollisionObject3D::set_shape(int p_index, GodotShape3D *p_shape) {
ERR_FAIL_INDEX(p_index, shapes.size()); ERR_FAIL_INDEX(p_index, shapes.size());
shapes[p_index].shape->remove_owner(this); shapes[p_index].shape->remove_owner(this);
shapes.write[p_index].shape = p_shape; shapes.write[p_index].shape = p_shape;
p_shape->add_owner(this); p_shape->add_owner(this);
if (!pending_shape_update_list.in_list()) { if (!pending_shape_update_list.in_list()) {
PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
} }
} }
void CollisionObject3DSW::set_shape_transform(int p_index, const Transform3D &p_transform) { void GodotCollisionObject3D::set_shape_transform(int p_index, const Transform3D &p_transform) {
ERR_FAIL_INDEX(p_index, shapes.size()); ERR_FAIL_INDEX(p_index, shapes.size());
shapes.write[p_index].xform = p_transform; shapes.write[p_index].xform = p_transform;
shapes.write[p_index].xform_inv = p_transform.affine_inverse(); shapes.write[p_index].xform_inv = p_transform.affine_inverse();
if (!pending_shape_update_list.in_list()) { if (!pending_shape_update_list.in_list()) {
PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
} }
} }
void CollisionObject3DSW::set_shape_disabled(int p_idx, bool p_disabled) { void GodotCollisionObject3D::set_shape_disabled(int p_idx, bool p_disabled) {
ERR_FAIL_INDEX(p_idx, shapes.size()); ERR_FAIL_INDEX(p_idx, shapes.size());
CollisionObject3DSW::Shape &shape = shapes.write[p_idx]; GodotCollisionObject3D::Shape &shape = shapes.write[p_idx];
if (shape.disabled == p_disabled) { if (shape.disabled == p_disabled) {
return; return;
} }
@ -86,16 +87,16 @@ void CollisionObject3DSW::set_shape_disabled(int p_idx, bool p_disabled) {
space->get_broadphase()->remove(shape.bpid); space->get_broadphase()->remove(shape.bpid);
shape.bpid = 0; shape.bpid = 0;
if (!pending_shape_update_list.in_list()) { if (!pending_shape_update_list.in_list()) {
PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
} }
} else if (!p_disabled && shape.bpid == 0) { } else if (!p_disabled && shape.bpid == 0) {
if (!pending_shape_update_list.in_list()) { if (!pending_shape_update_list.in_list()) {
PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
} }
} }
} }
void CollisionObject3DSW::remove_shape(Shape3DSW *p_shape) { void GodotCollisionObject3D::remove_shape(GodotShape3D *p_shape) {
//remove a shape, all the times it appears //remove a shape, all the times it appears
for (int i = 0; i < shapes.size(); i++) { for (int i = 0; i < shapes.size(); i++) {
if (shapes[i].shape == p_shape) { if (shapes[i].shape == p_shape) {
@ -105,7 +106,7 @@ void CollisionObject3DSW::remove_shape(Shape3DSW *p_shape) {
} }
} }
void CollisionObject3DSW::remove_shape(int p_index) { void GodotCollisionObject3D::remove_shape(int p_index) {
//remove anything from shape to be erased to end, so subindices don't change //remove anything from shape to be erased to end, so subindices don't change
ERR_FAIL_INDEX(p_index, shapes.size()); ERR_FAIL_INDEX(p_index, shapes.size());
for (int i = p_index; i < shapes.size(); i++) { for (int i = p_index; i < shapes.size(); i++) {
@ -120,11 +121,11 @@ void CollisionObject3DSW::remove_shape(int p_index) {
shapes.remove(p_index); shapes.remove(p_index);
if (!pending_shape_update_list.in_list()) { if (!pending_shape_update_list.in_list()) {
PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); GodotPhysicsServer3D::godot_singleton->pending_shape_update_list.add(&pending_shape_update_list);
} }
} }
void CollisionObject3DSW::_set_static(bool p_static) { void GodotCollisionObject3D::_set_static(bool p_static) {
if (_static == p_static) { if (_static == p_static) {
return; return;
} }
@ -141,7 +142,7 @@ void CollisionObject3DSW::_set_static(bool p_static) {
} }
} }
void CollisionObject3DSW::_unregister_shapes() { void GodotCollisionObject3D::_unregister_shapes() {
for (int i = 0; i < shapes.size(); i++) { for (int i = 0; i < shapes.size(); i++) {
Shape &s = shapes.write[i]; Shape &s = shapes.write[i];
if (s.bpid > 0) { if (s.bpid > 0) {
@ -151,7 +152,7 @@ void CollisionObject3DSW::_unregister_shapes() {
} }
} }
void CollisionObject3DSW::_update_shapes() { void GodotCollisionObject3D::_update_shapes() {
if (!space) { if (!space) {
return; return;
} }
@ -181,7 +182,7 @@ void CollisionObject3DSW::_update_shapes() {
} }
} }
void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) { void GodotCollisionObject3D::_update_shapes_with_motion(const Vector3 &p_motion) {
if (!space) { if (!space) {
return; return;
} }
@ -208,7 +209,7 @@ void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) {
} }
} }
void CollisionObject3DSW::_set_space(Space3DSW *p_space) { void GodotCollisionObject3D::_set_space(GodotSpace3D *p_space) {
if (space) { if (space) {
space->remove_object(this); space->remove_object(this);
@ -229,12 +230,12 @@ void CollisionObject3DSW::_set_space(Space3DSW *p_space) {
} }
} }
void CollisionObject3DSW::_shape_changed() { void GodotCollisionObject3D::_shape_changed() {
_update_shapes(); _update_shapes();
_shapes_changed(); _shapes_changed();
} }
CollisionObject3DSW::CollisionObject3DSW(Type p_type) : GodotCollisionObject3D::GodotCollisionObject3D(Type p_type) :
pending_shape_update_list(this) { pending_shape_update_list(this) {
type = p_type; type = p_type;
} }

47
servers/physics_3d/collision_object_3d_sw.h → servers/physics_3d/godot_collision_object_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* collision_object_3d_sw.h */ /* godot_collision_object_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,13 +28,14 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef COLLISION_OBJECT_SW_H #ifndef GODOT_COLLISION_OBJECT_3D_H
#define COLLISION_OBJECT_SW_H #define GODOT_COLLISION_OBJECT_3D_H
#include "godot_broad_phase_3d.h"
#include "godot_shape_3d.h"
#include "broad_phase_3d_sw.h"
#include "core/templates/self_list.h" #include "core/templates/self_list.h"
#include "servers/physics_server_3d.h" #include "servers/physics_server_3d.h"
#include "shape_3d_sw.h"
#ifdef DEBUG_ENABLED #ifdef DEBUG_ENABLED
#define MAX_OBJECT_DISTANCE 3.1622776601683791e+18 #define MAX_OBJECT_DISTANCE 3.1622776601683791e+18
@ -42,9 +43,9 @@
#define MAX_OBJECT_DISTANCE_X2 (MAX_OBJECT_DISTANCE * MAX_OBJECT_DISTANCE) #define MAX_OBJECT_DISTANCE_X2 (MAX_OBJECT_DISTANCE * MAX_OBJECT_DISTANCE)
#endif #endif
class Space3DSW; class GodotSpace3D;
class CollisionObject3DSW : public ShapeOwner3DSW { class GodotCollisionObject3D : public GodotShapeOwner3D {
public: public:
enum Type { enum Type {
TYPE_AREA, TYPE_AREA,
@ -62,20 +63,20 @@ private:
struct Shape { struct Shape {
Transform3D xform; Transform3D xform;
Transform3D xform_inv; Transform3D xform_inv;
BroadPhase3DSW::ID bpid; GodotBroadPhase3D::ID bpid;
AABB aabb_cache; //for rayqueries AABB aabb_cache; //for rayqueries
real_t area_cache = 0.0; real_t area_cache = 0.0;
Shape3DSW *shape = nullptr; GodotShape3D *shape = nullptr;
bool disabled = false; bool disabled = false;
}; };
Vector<Shape> shapes; Vector<Shape> shapes;
Space3DSW *space = nullptr; GodotSpace3D *space = nullptr;
Transform3D transform; Transform3D transform;
Transform3D inv_transform; Transform3D inv_transform;
bool _static = true; bool _static = true;
SelfList<CollisionObject3DSW> pending_shape_update_list; SelfList<GodotCollisionObject3D> pending_shape_update_list;
void _update_shapes(); void _update_shapes();
@ -98,11 +99,11 @@ protected:
void _set_static(bool p_static); void _set_static(bool p_static);
virtual void _shapes_changed() = 0; virtual void _shapes_changed() = 0;
void _set_space(Space3DSW *p_space); void _set_space(GodotSpace3D *p_space);
bool ray_pickable = true; bool ray_pickable = true;
CollisionObject3DSW(Type p_type); GodotCollisionObject3D(Type p_type);
public: public:
_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
@ -114,11 +115,11 @@ public:
void _shape_changed(); void _shape_changed();
_FORCE_INLINE_ Type get_type() const { return type; } _FORCE_INLINE_ Type get_type() const { return type; }
void add_shape(Shape3DSW *p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false); void add_shape(GodotShape3D *p_shape, const Transform3D &p_transform = Transform3D(), bool p_disabled = false);
void set_shape(int p_index, Shape3DSW *p_shape); void set_shape(int p_index, GodotShape3D *p_shape);
void set_shape_transform(int p_index, const Transform3D &p_transform); void set_shape_transform(int p_index, const Transform3D &p_transform);
_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
_FORCE_INLINE_ Shape3DSW *get_shape(int p_index) const { _FORCE_INLINE_ GodotShape3D *get_shape(int p_index) const {
CRASH_BAD_INDEX(p_index, shapes.size()); CRASH_BAD_INDEX(p_index, shapes.size());
return shapes[p_index].shape; return shapes[p_index].shape;
} }
@ -141,7 +142,7 @@ public:
_FORCE_INLINE_ const Transform3D &get_transform() const { return transform; } _FORCE_INLINE_ const Transform3D &get_transform() const { return transform; }
_FORCE_INLINE_ const Transform3D &get_inv_transform() const { return inv_transform; } _FORCE_INLINE_ const Transform3D &get_inv_transform() const { return inv_transform; }
_FORCE_INLINE_ Space3DSW *get_space() const { return space; } _FORCE_INLINE_ GodotSpace3D *get_space() const { return space; }
_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; } _FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; } _FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
@ -164,22 +165,22 @@ public:
} }
_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; } _FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
_FORCE_INLINE_ bool collides_with(CollisionObject3DSW *p_other) const { _FORCE_INLINE_ bool collides_with(GodotCollisionObject3D *p_other) const {
return p_other->collision_layer & collision_mask; return p_other->collision_layer & collision_mask;
} }
_FORCE_INLINE_ bool interacts_with(CollisionObject3DSW *p_other) const { _FORCE_INLINE_ bool interacts_with(GodotCollisionObject3D *p_other) const {
return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask; return collision_layer & p_other->collision_mask || p_other->collision_layer & collision_mask;
} }
void remove_shape(Shape3DSW *p_shape); void remove_shape(GodotShape3D *p_shape);
void remove_shape(int p_index); void remove_shape(int p_index);
virtual void set_space(Space3DSW *p_space) = 0; virtual void set_space(GodotSpace3D *p_space) = 0;
_FORCE_INLINE_ bool is_static() const { return _static; } _FORCE_INLINE_ bool is_static() const { return _static; }
virtual ~CollisionObject3DSW() {} virtual ~GodotCollisionObject3D() {}
}; };
#endif // COLLISION_OBJECT_SW_H #endif // GODOT_COLLISION_OBJECT_3D_H

70
servers/physics_3d/collision_solver_3d_sw.cpp → servers/physics_3d/godot_collision_solver_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* collision_solver_3d_sw.cpp */ /* godot_collision_solver_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,17 +28,17 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "collision_solver_3d_sw.h" #include "godot_collision_solver_3d.h"
#include "collision_solver_3d_sat.h" #include "godot_collision_solver_3d_sat.h"
#include "soft_body_3d_sw.h" #include "godot_soft_body_3d.h"
#include "gjk_epa.h" #include "gjk_epa.h"
#define collision_solver sat_calculate_penetration #define collision_solver sat_calculate_penetration
//#define collision_solver gjk_epa_calculate_penetration //#define collision_solver gjk_epa_calculate_penetration
bool CollisionSolver3DSW::solve_static_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
const WorldBoundaryShape3DSW *world_boundary = static_cast<const WorldBoundaryShape3DSW *>(p_shape_A); const GodotWorldBoundaryShape3D *world_boundary = static_cast<const GodotWorldBoundaryShape3D *>(p_shape_A);
if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) {
return false; return false;
} }
@ -47,10 +47,10 @@ bool CollisionSolver3DSW::solve_static_world_boundary(const Shape3DSW *p_shape_A
static const int max_supports = 16; static const int max_supports = 16;
Vector3 supports[max_supports]; Vector3 supports[max_supports];
int support_count; int support_count;
Shape3DSW::FeatureType support_type; GodotShape3D::FeatureType support_type;
p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type);
if (support_type == Shape3DSW::FEATURE_CIRCLE) { if (support_type == GodotShape3D::FEATURE_CIRCLE) {
ERR_FAIL_COND_V(support_count != 3, false); ERR_FAIL_COND_V(support_count != 3, false);
Vector3 circle_pos = supports[0]; Vector3 circle_pos = supports[0];
@ -89,8 +89,8 @@ bool CollisionSolver3DSW::solve_static_world_boundary(const Shape3DSW *p_shape_A
return found; return found;
} }
bool CollisionSolver3DSW::solve_separation_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) { bool GodotCollisionSolver3D::solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin) {
const SeparationRayShape3DSW *ray = static_cast<const SeparationRayShape3DSW *>(p_shape_A); const GodotSeparationRayShape3D *ray = static_cast<const GodotSeparationRayShape3D *>(p_shape_A);
Vector3 from = p_transform_A.origin; Vector3 from = p_transform_A.origin;
Vector3 to = from + p_transform_A.basis.get_axis(2) * (ray->get_length() + p_margin); Vector3 to = from + p_transform_A.basis.get_axis(2) * (ray->get_length() + p_margin);
@ -134,13 +134,13 @@ bool CollisionSolver3DSW::solve_separation_ray(const Shape3DSW *p_shape_A, const
struct _SoftBodyContactCollisionInfo { struct _SoftBodyContactCollisionInfo {
int node_index = 0; int node_index = 0;
CollisionSolver3DSW::CallbackResult result_callback = nullptr; GodotCollisionSolver3D::CallbackResult result_callback = nullptr;
void *userdata = nullptr; void *userdata = nullptr;
bool swap_result = false; bool swap_result = false;
int contact_count = 0; int contact_count = 0;
}; };
void CollisionSolver3DSW::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) { void GodotCollisionSolver3D::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
_SoftBodyContactCollisionInfo &cinfo = *(_SoftBodyContactCollisionInfo *)(p_userdata); _SoftBodyContactCollisionInfo &cinfo = *(_SoftBodyContactCollisionInfo *)(p_userdata);
++cinfo.contact_count; ++cinfo.contact_count;
@ -157,9 +157,9 @@ void CollisionSolver3DSW::soft_body_contact_callback(const Vector3 &p_point_A, i
} }
struct _SoftBodyQueryInfo { struct _SoftBodyQueryInfo {
SoftBody3DSW *soft_body = nullptr; GodotSoftBody3D *soft_body = nullptr;
const Shape3DSW *shape_A = nullptr; const GodotShape3D *shape_A = nullptr;
const Shape3DSW *shape_B = nullptr; const GodotShape3D *shape_B = nullptr;
Transform3D transform_A; Transform3D transform_A;
Transform3D node_transform; Transform3D node_transform;
_SoftBodyContactCollisionInfo contact_info; _SoftBodyContactCollisionInfo contact_info;
@ -169,7 +169,7 @@ struct _SoftBodyQueryInfo {
#endif #endif
}; };
bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void *p_userdata) { bool GodotCollisionSolver3D::soft_body_query_callback(uint32_t p_node_index, void *p_userdata) {
_SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata); _SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata);
Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index); Vector3 node_position = query_cinfo.soft_body->get_node_position(p_node_index);
@ -188,7 +188,7 @@ bool CollisionSolver3DSW::soft_body_query_callback(uint32_t p_node_index, void *
return (collided && !query_cinfo.contact_info.result_callback); return (collided && !query_cinfo.contact_info.result_callback);
} }
bool CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex) { bool GodotCollisionSolver3D::soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex) {
_SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata); _SoftBodyQueryInfo &query_cinfo = *(_SoftBodyQueryInfo *)(p_userdata);
query_cinfo.shape_A = p_convex; query_cinfo.shape_A = p_convex;
@ -220,15 +220,15 @@ bool CollisionSolver3DSW::soft_body_concave_callback(void *p_userdata, Shape3DSW
return (collided && !query_cinfo.contact_info.result_callback); return (collided && !query_cinfo.contact_info.result_callback);
} }
bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) { bool GodotCollisionSolver3D::solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result) {
const SoftBodyShape3DSW *soft_body_shape_B = static_cast<const SoftBodyShape3DSW *>(p_shape_B); const GodotSoftBodyShape3D *soft_body_shape_B = static_cast<const GodotSoftBodyShape3D *>(p_shape_B);
SoftBody3DSW *soft_body = soft_body_shape_B->get_soft_body(); GodotSoftBody3D *soft_body = soft_body_shape_B->get_soft_body();
const Transform3D &world_to_local = soft_body->get_inv_transform(); const Transform3D &world_to_local = soft_body->get_inv_transform();
const real_t collision_margin = soft_body->get_collision_margin(); const real_t collision_margin = soft_body->get_collision_margin();
SphereShape3DSW sphere_shape; GodotSphereShape3D sphere_shape;
sphere_shape.set_data(collision_margin); sphere_shape.set_data(collision_margin);
_SoftBodyQueryInfo query_cinfo; _SoftBodyQueryInfo query_cinfo;
@ -243,7 +243,7 @@ bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Tran
if (p_shape_A->is_concave()) { if (p_shape_A->is_concave()) {
// In case of concave shape, query convex shapes first. // In case of concave shape, query convex shapes first.
const ConcaveShape3DSW *concave_shape_A = static_cast<const ConcaveShape3DSW *>(p_shape_A); const GodotConcaveShape3D *concave_shape_A = static_cast<const GodotConcaveShape3D *>(p_shape_A);
AABB soft_body_aabb = soft_body->get_bounds(); AABB soft_body_aabb = soft_body->get_bounds();
soft_body_aabb.grow_by(collision_margin); soft_body_aabb.grow_by(collision_margin);
@ -277,9 +277,9 @@ bool CollisionSolver3DSW::solve_soft_body(const Shape3DSW *p_shape_A, const Tran
struct _ConcaveCollisionInfo { struct _ConcaveCollisionInfo {
const Transform3D *transform_A; const Transform3D *transform_A;
const Shape3DSW *shape_A; const GodotShape3D *shape_A;
const Transform3D *transform_B; const Transform3D *transform_B;
CollisionSolver3DSW::CallbackResult result_callback; GodotCollisionSolver3D::CallbackResult result_callback;
void *userdata; void *userdata;
bool swap_result; bool swap_result;
bool collided; bool collided;
@ -291,7 +291,7 @@ struct _ConcaveCollisionInfo {
Vector3 close_A, close_B; Vector3 close_A, close_B;
}; };
bool CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex) { bool GodotCollisionSolver3D::concave_callback(void *p_userdata, GodotShape3D *p_convex) {
_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
cinfo.aabb_tests++; cinfo.aabb_tests++;
@ -307,8 +307,8 @@ bool CollisionSolver3DSW::concave_callback(void *p_userdata, Shape3DSW *p_convex
return !cinfo.result_callback; return !cinfo.result_callback;
} }
bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) { bool GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A, real_t p_margin_B) {
const ConcaveShape3DSW *concave_B = static_cast<const ConcaveShape3DSW *>(p_shape_B); const GodotConcaveShape3D *concave_B = static_cast<const GodotConcaveShape3D *>(p_shape_B);
_ConcaveCollisionInfo cinfo; _ConcaveCollisionInfo cinfo;
cinfo.transform_A = &p_transform_A; cinfo.transform_A = &p_transform_A;
@ -351,7 +351,7 @@ bool CollisionSolver3DSW::solve_concave(const Shape3DSW *p_shape_A, const Transf
return cinfo.collided; return cinfo.collided;
} }
bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) { bool GodotCollisionSolver3D::solve_static(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis, real_t p_margin_A, real_t p_margin_B) {
PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); PhysicsServer3D::ShapeType type_A = p_shape_A->get_type();
PhysicsServer3D::ShapeType type_B = p_shape_B->get_type(); PhysicsServer3D::ShapeType type_B = p_shape_B->get_type();
bool concave_A = p_shape_A->is_concave(); bool concave_A = p_shape_A->is_concave();
@ -421,7 +421,7 @@ bool CollisionSolver3DSW::solve_static(const Shape3DSW *p_shape_A, const Transfo
} }
} }
bool CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW *p_convex) { bool GodotCollisionSolver3D::concave_distance_callback(void *p_userdata, GodotShape3D *p_convex) {
_ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata); _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo *)(p_userdata);
cinfo.aabb_tests++; cinfo.aabb_tests++;
@ -443,8 +443,8 @@ bool CollisionSolver3DSW::concave_distance_callback(void *p_userdata, Shape3DSW
return false; return false;
} }
bool CollisionSolver3DSW::solve_distance_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) { bool GodotCollisionSolver3D::solve_distance_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B) {
const WorldBoundaryShape3DSW *world_boundary = static_cast<const WorldBoundaryShape3DSW *>(p_shape_A); const GodotWorldBoundaryShape3D *world_boundary = static_cast<const GodotWorldBoundaryShape3D *>(p_shape_A);
if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) { if (p_shape_B->get_type() == PhysicsServer3D::SHAPE_WORLD_BOUNDARY) {
return false; return false;
} }
@ -453,11 +453,11 @@ bool CollisionSolver3DSW::solve_distance_world_boundary(const Shape3DSW *p_shape
static const int max_supports = 16; static const int max_supports = 16;
Vector3 supports[max_supports]; Vector3 supports[max_supports];
int support_count; int support_count;
Shape3DSW::FeatureType support_type; GodotShape3D::FeatureType support_type;
p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type);
if (support_type == Shape3DSW::FEATURE_CIRCLE) { if (support_type == GodotShape3D::FEATURE_CIRCLE) {
ERR_FAIL_COND_V(support_count != 3, false); ERR_FAIL_COND_V(support_count != 3, false);
Vector3 circle_pos = supports[0]; Vector3 circle_pos = supports[0];
@ -495,7 +495,7 @@ bool CollisionSolver3DSW::solve_distance_world_boundary(const Shape3DSW *p_shape
return collided; return collided;
} }
bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) { bool GodotCollisionSolver3D::solve_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis) {
if (p_shape_A->is_concave()) { if (p_shape_A->is_concave()) {
return false; return false;
} }
@ -512,7 +512,7 @@ bool CollisionSolver3DSW::solve_distance(const Shape3DSW *p_shape_A, const Trans
return false; return false;
} }
const ConcaveShape3DSW *concave_B = static_cast<const ConcaveShape3DSW *>(p_shape_B); const GodotConcaveShape3D *concave_B = static_cast<const GodotConcaveShape3D *>(p_shape_B);
_ConcaveCollisionInfo cinfo; _ConcaveCollisionInfo cinfo;
cinfo.transform_A = &p_transform_A; cinfo.transform_A = &p_transform_A;

32
servers/physics_3d/collision_solver_3d_sw.h → servers/physics_3d/godot_collision_solver_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* collision_solver_3d_sw.h */ /* godot_collision_solver_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,30 +28,30 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef COLLISION_SOLVER_SW_H #ifndef GODOT_COLLISION_SOLVER_3D_H
#define COLLISION_SOLVER_SW_H #define GODOT_COLLISION_SOLVER_3D_H
#include "shape_3d_sw.h" #include "godot_shape_3d.h"
class CollisionSolver3DSW { class GodotCollisionSolver3D {
public: public:
typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
private: private:
static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata); static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata);
static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata); static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
static bool soft_body_concave_callback(void *p_userdata, Shape3DSW *p_convex); static bool soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex);
static bool concave_callback(void *p_userdata, Shape3DSW *p_convex); static bool concave_callback(void *p_userdata, GodotShape3D *p_convex);
static bool solve_static_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_separation_ray(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0); static bool solve_separation_ray(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0);
static bool solve_soft_body(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result); static bool solve_soft_body(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result);
static bool solve_concave(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0); static bool solve_concave(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin_A = 0, real_t p_margin_B = 0);
static bool concave_distance_callback(void *p_userdata, Shape3DSW *p_convex); static bool concave_distance_callback(void *p_userdata, GodotShape3D *p_convex);
static bool solve_distance_world_boundary(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B); static bool solve_distance_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B);
public: public:
static bool solve_static(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0); static bool solve_static(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, Vector3 *r_sep_axis = nullptr, real_t p_margin_A = 0, real_t p_margin_B = 0);
static bool solve_distance(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr); static bool solve_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_point_A, Vector3 &r_point_B, const AABB &p_concave_hint, Vector3 *r_sep_axis = nullptr);
}; };
#endif // COLLISION_SOLVER__SW_H #endif // GODOT_COLLISION_SOLVER_3D_H

189
servers/physics_3d/collision_solver_3d_sat.cpp → servers/physics_3d/godot_collision_solver_3d_sat.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* collision_solver_3d_sat.cpp */ /* godot_collision_solver_3d_sat.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,11 +28,12 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "collision_solver_3d_sat.h" #include "godot_collision_solver_3d_sat.h"
#include "core/math/geometry_3d.h"
#include "gjk_epa.h" #include "gjk_epa.h"
#include "core/math/geometry_3d.h"
#define fallback_collision_solver gjk_epa_calculate_penetration #define fallback_collision_solver gjk_epa_calculate_penetration
// Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders. // Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders.
@ -65,7 +66,7 @@
*************************************************************************/ *************************************************************************/
struct _CollectorCallback { struct _CollectorCallback {
CollisionSolver3DSW::CallbackResult callback; GodotCollisionSolver3D::CallbackResult callback;
void *userdata = nullptr; void *userdata = nullptr;
bool swap = false; bool swap = false;
bool collided = false; bool collided = false;
@ -539,7 +540,7 @@ static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_po
} }
} }
static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, Shape3DSW::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, Shape3DSW::FeatureType p_feature_type_B, _CollectorCallback *p_callback) { static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, GodotShape3D::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, GodotShape3D::FeatureType p_feature_type_B, _CollectorCallback *p_callback) {
#ifdef DEBUG_ENABLED #ifdef DEBUG_ENABLED
ERR_FAIL_COND(p_point_count_A < 1); ERR_FAIL_COND(p_point_count_A < 1);
ERR_FAIL_COND(p_point_count_B < 1); ERR_FAIL_COND(p_point_count_B < 1);
@ -713,7 +714,7 @@ public:
Vector3 supports_A[max_supports]; Vector3 supports_A[max_supports];
int support_count_A; int support_count_A;
Shape3DSW::FeatureType support_type_A; GodotShape3D::FeatureType support_type_A;
shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A, support_type_A); shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A, support_type_A);
for (int i = 0; i < support_count_A; i++) { for (int i = 0; i < support_count_A; i++) {
supports_A[i] = transform_A->xform(supports_A[i]); supports_A[i] = transform_A->xform(supports_A[i]);
@ -727,7 +728,7 @@ public:
Vector3 supports_B[max_supports]; Vector3 supports_B[max_supports];
int support_count_B; int support_count_B;
Shape3DSW::FeatureType support_type_B; GodotShape3D::FeatureType support_type_B;
shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B, support_type_B); shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B, support_type_B);
for (int i = 0; i < support_count_B; i++) { for (int i = 0; i < support_count_B; i++) {
supports_B[i] = transform_B->xform(supports_B[i]); supports_B[i] = transform_B->xform(supports_B[i]);
@ -761,14 +762,14 @@ public:
/****** SAT TESTS *******/ /****** SAT TESTS *******/
typedef void (*CollisionFunc)(const Shape3DSW *, const Transform3D &, const Shape3DSW *, const Transform3D &, _CollectorCallback *p_callback, real_t, real_t); typedef void (*CollisionFunc)(const GodotShape3D *, const Transform3D &, const GodotShape3D *, const Transform3D &, _CollectorCallback *p_callback, real_t, real_t);
template <bool withMargin> template <bool withMargin>
static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_sphere_sphere(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
const SphereShape3DSW *sphere_B = static_cast<const SphereShape3DSW *>(p_b); const GodotSphereShape3D *sphere_B = static_cast<const GodotSphereShape3D *>(p_b);
SeparatorAxisTest<SphereShape3DSW, SphereShape3DSW, withMargin> separator(sphere_A, p_transform_a, sphere_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotSphereShape3D, GodotSphereShape3D, withMargin> separator(sphere_A, p_transform_a, sphere_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
// previous axis // previous axis
@ -784,11 +785,11 @@ static void _collision_sphere_sphere(const Shape3DSW *p_a, const Transform3D &p_
} }
template <bool withMargin> template <bool withMargin>
static void _collision_sphere_box(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b); const GodotBoxShape3D *box_B = static_cast<const GodotBoxShape3D *>(p_b);
SeparatorAxisTest<SphereShape3DSW, BoxShape3DSW, withMargin> separator(sphere_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotSphereShape3D, GodotBoxShape3D, withMargin> separator(sphere_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -835,11 +836,11 @@ static void _collision_sphere_box(const Shape3DSW *p_a, const Transform3D &p_tra
} }
template <bool withMargin> template <bool withMargin>
static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
SeparatorAxisTest<SphereShape3DSW, CapsuleShape3DSW, withMargin> separator(sphere_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotSphereShape3D, GodotCapsuleShape3D, withMargin> separator(sphere_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -877,11 +878,11 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform3D &p
} }
template <bool withMargin> template <bool withMargin>
static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_sphere_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b);
SeparatorAxisTest<SphereShape3DSW, CylinderShape3DSW, withMargin> separator(sphere_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotSphereShape3D, GodotCylinderShape3D, withMargin> separator(sphere_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -936,11 +937,11 @@ static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform3D &
} }
template <bool withMargin> template <bool withMargin>
static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_sphere_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
SeparatorAxisTest<SphereShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotSphereShape3D, GodotConvexPolygonShape3D, withMargin> separator(sphere_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -999,11 +1000,11 @@ static void _collision_sphere_convex_polygon(const Shape3DSW *p_a, const Transfo
} }
template <bool withMargin> template <bool withMargin>
static void _collision_sphere_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_sphere_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
SeparatorAxisTest<SphereShape3DSW, FaceShape3DSW, withMargin> separator(sphere_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotSphereShape3D, GodotFaceShape3D, withMargin> separator(sphere_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
Vector3 vertex[3] = { Vector3 vertex[3] = {
p_transform_b.xform(face_B->vertex[0]), p_transform_b.xform(face_B->vertex[0]),
@ -1044,11 +1045,11 @@ static void _collision_sphere_face(const Shape3DSW *p_a, const Transform3D &p_tr
} }
template <bool withMargin> template <bool withMargin>
static void _collision_box_box(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_box_box(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
const BoxShape3DSW *box_B = static_cast<const BoxShape3DSW *>(p_b); const GodotBoxShape3D *box_B = static_cast<const GodotBoxShape3D *>(p_b);
SeparatorAxisTest<BoxShape3DSW, BoxShape3DSW, withMargin> separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotBoxShape3D, GodotBoxShape3D, withMargin> separator(box_A, p_transform_a, box_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -1142,11 +1143,11 @@ static void _collision_box_box(const Shape3DSW *p_a, const Transform3D &p_transf
} }
template <bool withMargin> template <bool withMargin>
static void _collision_box_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_box_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
SeparatorAxisTest<BoxShape3DSW, CapsuleShape3DSW, withMargin> separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotBoxShape3D, GodotCapsuleShape3D, withMargin> separator(box_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -1240,11 +1241,11 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform3D &p_tr
} }
template <bool withMargin> template <bool withMargin>
static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_box_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b);
SeparatorAxisTest<BoxShape3DSW, CylinderShape3DSW, withMargin> separator(box_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotBoxShape3D, GodotCylinderShape3D, withMargin> separator(box_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -1353,11 +1354,11 @@ static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform3D &p_t
} }
template <bool withMargin> template <bool withMargin>
static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_box_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
SeparatorAxisTest<BoxShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotBoxShape3D, GodotConvexPolygonShape3D, withMargin> separator(box_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -1471,11 +1472,11 @@ static void _collision_box_convex_polygon(const Shape3DSW *p_a, const Transform3
} }
template <bool withMargin> template <bool withMargin>
static void _collision_box_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); const GodotBoxShape3D *box_A = static_cast<const GodotBoxShape3D *>(p_a);
const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
SeparatorAxisTest<BoxShape3DSW, FaceShape3DSW, withMargin> separator(box_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotBoxShape3D, GodotFaceShape3D, withMargin> separator(box_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
Vector3 vertex[3] = { Vector3 vertex[3] = {
p_transform_b.xform(face_B->vertex[0]), p_transform_b.xform(face_B->vertex[0]),
@ -1594,11 +1595,11 @@ static void _collision_box_face(const Shape3DSW *p_a, const Transform3D &p_trans
} }
template <bool withMargin> template <bool withMargin>
static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
const CapsuleShape3DSW *capsule_B = static_cast<const CapsuleShape3DSW *>(p_b); const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
SeparatorAxisTest<CapsuleShape3DSW, CapsuleShape3DSW, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotCapsuleShape3D, GodotCapsuleShape3D, withMargin> separator(capsule_A, p_transform_a, capsule_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -1658,11 +1659,11 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform3D &
} }
template <bool withMargin> template <bool withMargin>
static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_capsule_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b);
SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin> separator(capsule_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotCapsuleShape3D, GodotCylinderShape3D, withMargin> separator(capsule_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -1709,7 +1710,7 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform3D
return; return;
} }
CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points; GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest<GodotCapsuleShape3D, GodotCylinderShape3D, withMargin>::test_contact_points;
// Fallback to generic algorithm to find the best separating axis. // Fallback to generic algorithm to find the best separating axis.
if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) {
@ -1720,11 +1721,11 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform3D
} }
template <bool withMargin> template <bool withMargin>
static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_capsule_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
SeparatorAxisTest<CapsuleShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotCapsuleShape3D, GodotConvexPolygonShape3D, withMargin> separator(capsule_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -1787,11 +1788,11 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf
} }
template <bool withMargin> template <bool withMargin>
static void _collision_capsule_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
SeparatorAxisTest<CapsuleShape3DSW, FaceShape3DSW, withMargin> separator(capsule_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotCapsuleShape3D, GodotFaceShape3D, withMargin> separator(capsule_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
Vector3 vertex[3] = { Vector3 vertex[3] = {
p_transform_b.xform(face_B->vertex[0]), p_transform_b.xform(face_B->vertex[0]),
@ -1861,11 +1862,11 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform3D &p_t
} }
template <bool withMargin> template <bool withMargin>
static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_cylinder_cylinder(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); const GodotCylinderShape3D *cylinder_A = static_cast<const GodotCylinderShape3D *>(p_a);
const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); const GodotCylinderShape3D *cylinder_B = static_cast<const GodotCylinderShape3D *>(p_b);
SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotCylinderShape3D, GodotCylinderShape3D, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
Vector3 cylinder_A_axis = p_transform_a.basis.get_axis(1); Vector3 cylinder_A_axis = p_transform_a.basis.get_axis(1);
Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1); Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1);
@ -1904,7 +1905,7 @@ static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform3D
return; return;
} }
CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points; GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest<GodotCylinderShape3D, GodotCylinderShape3D, withMargin>::test_contact_points;
// Fallback to generic algorithm to find the best separating axis. // Fallback to generic algorithm to find the best separating axis.
if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) {
@ -1915,13 +1916,13 @@ static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform3D
} }
template <bool withMargin> template <bool withMargin>
static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_cylinder_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); const GodotCylinderShape3D *cylinder_A = static_cast<const GodotCylinderShape3D *>(p_a);
const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotCylinderShape3D, GodotConvexPolygonShape3D, withMargin> separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin>::test_contact_points; GodotCollisionSolver3D::CallbackResult callback = SeparatorAxisTest<GodotCylinderShape3D, GodotConvexPolygonShape3D, withMargin>::test_contact_points;
// Fallback to generic algorithm to find the best separating axis. // Fallback to generic algorithm to find the best separating axis.
if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) {
@ -1932,11 +1933,11 @@ static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Trans
} }
template <bool withMargin> template <bool withMargin>
static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); const GodotCylinderShape3D *cylinder_A = static_cast<const GodotCylinderShape3D *>(p_a);
const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
SeparatorAxisTest<CylinderShape3DSW, FaceShape3DSW, withMargin> separator(cylinder_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotCylinderShape3D, GodotFaceShape3D, withMargin> separator(cylinder_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -2037,11 +2038,11 @@ static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform3D &p_
} }
template <bool withMargin> template <bool withMargin>
static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_convex_polygon_convex_polygon(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a); const GodotConvexPolygonShape3D *convex_polygon_A = static_cast<const GodotConvexPolygonShape3D *>(p_a);
const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); const GodotConvexPolygonShape3D *convex_polygon_B = static_cast<const GodotConvexPolygonShape3D *>(p_b);
SeparatorAxisTest<ConvexPolygonShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotConvexPolygonShape3D, GodotConvexPolygonShape3D, withMargin> separator(convex_polygon_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
if (!separator.test_previous_axis()) { if (!separator.test_previous_axis()) {
return; return;
@ -2150,11 +2151,11 @@ static void _collision_convex_polygon_convex_polygon(const Shape3DSW *p_a, const
} }
template <bool withMargin> template <bool withMargin>
static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform3D &p_transform_a, const Shape3DSW *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transform3D &p_transform_a, const GodotShape3D *p_b, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
const ConvexPolygonShape3DSW *convex_polygon_A = static_cast<const ConvexPolygonShape3DSW *>(p_a); const GodotConvexPolygonShape3D *convex_polygon_A = static_cast<const GodotConvexPolygonShape3D *>(p_a);
const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); const GodotFaceShape3D *face_B = static_cast<const GodotFaceShape3D *>(p_b);
SeparatorAxisTest<ConvexPolygonShape3DSW, FaceShape3DSW, withMargin> separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); SeparatorAxisTest<GodotConvexPolygonShape3D, GodotFaceShape3D, withMargin> separator(convex_polygon_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
const Geometry3D::MeshData &mesh = convex_polygon_A->get_mesh(); const Geometry3D::MeshData &mesh = convex_polygon_A->get_mesh();
@ -2268,7 +2269,7 @@ static void _collision_convex_polygon_face(const Shape3DSW *p_a, const Transform
separator.generate_contacts(); separator.generate_contacts();
} }
bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) { bool sat_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, Vector3 *r_prev_axis, real_t p_margin_a, real_t p_margin_b) {
PhysicsServer3D::ShapeType type_A = p_shape_A->get_type(); PhysicsServer3D::ShapeType type_A = p_shape_A->get_type();
ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false); ERR_FAIL_COND_V(type_A == PhysicsServer3D::SHAPE_WORLD_BOUNDARY, false);
@ -2366,8 +2367,8 @@ bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_
callback.collided = false; callback.collided = false;
callback.prev_axis = r_prev_axis; callback.prev_axis = r_prev_axis;
const Shape3DSW *A = p_shape_A; const GodotShape3D *A = p_shape_A;
const Shape3DSW *B = p_shape_B; const GodotShape3D *B = p_shape_B;
const Transform3D *transform_A = &p_transform_A; const Transform3D *transform_A = &p_transform_A;
const Transform3D *transform_B = &p_transform_B; const Transform3D *transform_B = &p_transform_B;
real_t margin_A = p_margin_a; real_t margin_A = p_margin_a;

12
servers/physics_3d/collision_solver_3d_sat.h → servers/physics_3d/godot_collision_solver_3d_sat.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* collision_solver_3d_sat.h */ /* godot_collision_solver_3d_sat.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,11 +28,11 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef COLLISION_SOLVER_SAT_H #ifndef GODOT_COLLISION_SOLVER_SAT_H
#define COLLISION_SOLVER_SAT_H #define GODOT_COLLISION_SOLVER_SAT_H
#include "collision_solver_3d_sw.h" #include "godot_collision_solver_3d.h"
bool sat_calculate_penetration(const Shape3DSW *p_shape_A, const Transform3D &p_transform_A, const Shape3DSW *p_shape_B, const Transform3D &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0); bool sat_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, Vector3 *r_prev_axis = nullptr, real_t p_margin_a = 0, real_t p_margin_b = 0);
#endif // COLLISION_SOLVER_SAT_H #endif // GODOT_COLLISION_SOLVER_SAT_H

24
servers/physics_3d/constraint_3d_sw.h → servers/physics_3d/godot_constraint_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* constraint_3d_sw.h */ /* godot_constraint_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,14 +28,14 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef CONSTRAINT_SW_H #ifndef GODOT_CONSTRAINT_3D_H
#define CONSTRAINT_SW_H #define GODOT_CONSTRAINT_3D_H
class Body3DSW; class GodotBody3D;
class SoftBody3DSW; class GodotSoftBody3D;
class Constraint3DSW { class GodotConstraint3D {
Body3DSW **_body_ptr; GodotBody3D **_body_ptr;
int _body_count; int _body_count;
uint64_t island_step; uint64_t island_step;
int priority; int priority;
@ -44,7 +44,7 @@ class Constraint3DSW {
RID self; RID self;
protected: protected:
Constraint3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) { GodotConstraint3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) {
_body_ptr = p_body_ptr; _body_ptr = p_body_ptr;
_body_count = p_body_count; _body_count = p_body_count;
island_step = 0; island_step = 0;
@ -59,10 +59,10 @@ public:
_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _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 set_island_step(uint64_t p_step) { island_step = p_step; }
_FORCE_INLINE_ Body3DSW **get_body_ptr() const { return _body_ptr; } _FORCE_INLINE_ GodotBody3D **get_body_ptr() const { return _body_ptr; }
_FORCE_INLINE_ int get_body_count() const { return _body_count; } _FORCE_INLINE_ int get_body_count() const { return _body_count; }
virtual SoftBody3DSW *get_soft_body_ptr(int p_index) const { return nullptr; } virtual GodotSoftBody3D *get_soft_body_ptr(int p_index) const { return nullptr; }
virtual int get_soft_body_count() const { return 0; } virtual int get_soft_body_count() const { return 0; }
_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; } _FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
@ -75,7 +75,7 @@ public:
virtual bool pre_solve(real_t p_step) = 0; virtual bool pre_solve(real_t p_step) = 0;
virtual void solve(real_t p_step) = 0; virtual void solve(real_t p_step) = 0;
virtual ~Constraint3DSW() {} virtual ~GodotConstraint3D() {}
}; };
#endif // CONSTRAINT__SW_H #endif // GODOT_CONSTRAINT_3D_H

24
servers/physics_3d/joints_3d_sw.h → servers/physics_3d/godot_joint_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* joints_3d_sw.h */ /* godot_joint_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,13 +28,13 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef JOINTS_SW_H #ifndef GODOT_JOINT_3D_H
#define JOINTS_SW_H #define GODOT_JOINT_3D_H
#include "body_3d_sw.h" #include "godot_body_3d.h"
#include "constraint_3d_sw.h" #include "godot_constraint_3d.h"
class Joint3DSW : public Constraint3DSW { class GodotJoint3D : public GodotConstraint3D {
protected: protected:
bool dynamic_A = false; bool dynamic_A = false;
bool dynamic_B = false; bool dynamic_B = false;
@ -44,20 +44,20 @@ public:
virtual bool pre_solve(real_t p_step) override { return true; } virtual bool pre_solve(real_t p_step) override { return true; }
virtual void solve(real_t p_step) override {} virtual void solve(real_t p_step) override {}
void copy_settings_from(Joint3DSW *p_joint) { void copy_settings_from(GodotJoint3D *p_joint) {
set_self(p_joint->get_self()); set_self(p_joint->get_self());
set_priority(p_joint->get_priority()); set_priority(p_joint->get_priority());
disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies());
} }
virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_MAX; } virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_MAX; }
_FORCE_INLINE_ Joint3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : _FORCE_INLINE_ GodotJoint3D(GodotBody3D **p_body_ptr = nullptr, int p_body_count = 0) :
Constraint3DSW(p_body_ptr, p_body_count) { GodotConstraint3D(p_body_ptr, p_body_count) {
} }
virtual ~Joint3DSW() { virtual ~GodotJoint3D() {
for (int i = 0; i < get_body_count(); i++) { for (int i = 0; i < get_body_count(); i++) {
Body3DSW *body = get_body_ptr()[i]; GodotBody3D *body = get_body_ptr()[i];
if (body) { if (body) {
body->remove_constraint(this); body->remove_constraint(this);
} }
@ -65,4 +65,4 @@ public:
} }
}; };
#endif // JOINTS_SW_H #endif // GODOT_JOINT_3D_H

1749
servers/physics_3d/godot_physics_server_3d.cpp Normal file
View file

File diff suppressed because it is too large Load diff

49
servers/physics_3d/physics_server_3d_sw.h → servers/physics_3d/godot_physics_server_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* physics_server_3d_sw.h */ /* godot_physics_server_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,20 +28,21 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef PHYSICS_SERVER_SW #ifndef GODOT_PHYSICS_SERVER_3D_H
#define PHYSICS_SERVER_SW #define GODOT_PHYSICS_SERVER_3D_H
#include "godot_joint_3d.h"
#include "godot_shape_3d.h"
#include "godot_space_3d.h"
#include "godot_step_3d.h"
#include "core/templates/rid_owner.h" #include "core/templates/rid_owner.h"
#include "joints_3d_sw.h"
#include "servers/physics_server_3d.h" #include "servers/physics_server_3d.h"
#include "shape_3d_sw.h"
#include "space_3d_sw.h"
#include "step_3d_sw.h"
class PhysicsServer3DSW : public PhysicsServer3D { class GodotPhysicsServer3D : public PhysicsServer3D {
GDCLASS(PhysicsServer3DSW, PhysicsServer3D); GDCLASS(GodotPhysicsServer3D, PhysicsServer3D);
friend class PhysicsDirectSpaceState3DSW; friend class GodotPhysicsDirectSpaceState3D;
bool active = true; bool active = true;
int iterations = 0; int iterations = 0;
@ -53,22 +54,22 @@ class PhysicsServer3DSW : public PhysicsServer3D {
bool doing_sync = false; bool doing_sync = false;
bool flushing_queries = false; bool flushing_queries = false;
Step3DSW *stepper = nullptr; GodotStep3D *stepper = nullptr;
Set<const Space3DSW *> active_spaces; Set<const GodotSpace3D *> active_spaces;
mutable RID_PtrOwner<Shape3DSW, true> shape_owner; mutable RID_PtrOwner<GodotShape3D, true> shape_owner;
mutable RID_PtrOwner<Space3DSW, true> space_owner; mutable RID_PtrOwner<GodotSpace3D, true> space_owner;
mutable RID_PtrOwner<Area3DSW, true> area_owner; mutable RID_PtrOwner<GodotArea3D, true> area_owner;
mutable RID_PtrOwner<Body3DSW, true> body_owner; mutable RID_PtrOwner<GodotBody3D, true> body_owner;
mutable RID_PtrOwner<SoftBody3DSW, true> soft_body_owner; mutable RID_PtrOwner<GodotSoftBody3D, true> soft_body_owner;
mutable RID_PtrOwner<Joint3DSW, true> joint_owner; mutable RID_PtrOwner<GodotJoint3D, true> joint_owner;
//void _clear_query(QuerySW *p_query); //void _clear_query(QuerySW *p_query);
friend class CollisionObject3DSW; friend class GodotCollisionObject3D;
SelfList<CollisionObject3DSW>::List pending_shape_update_list; SelfList<GodotCollisionObject3D>::List pending_shape_update_list;
void _update_shapes(); void _update_shapes();
static PhysicsServer3DSW *singletonsw; static GodotPhysicsServer3D *godot_singleton;
public: public:
struct CollCbkData { struct CollCbkData {
@ -372,8 +373,8 @@ public:
int get_process_info(ProcessInfo p_info) override; int get_process_info(ProcessInfo p_info) override;
PhysicsServer3DSW(bool p_using_threads = false); GodotPhysicsServer3D(bool p_using_threads = false);
~PhysicsServer3DSW() {} ~GodotPhysicsServer3D() {}
}; };
#endif #endif // GODOT_PHYSICS_SERVER_3D_H

332
servers/physics_3d/shape_3d_sw.cpp → servers/physics_3d/godot_shape_3d.cpp
View file

File diff suppressed because it is too large Load diff

94
servers/physics_3d/shape_3d_sw.h → servers/physics_3d/godot_shape_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* shape_3d_sw.h */ /* godot_shape_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,30 +28,30 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef SHAPE_SW_H #ifndef GODOT_SHAPE_3D_H
#define SHAPE_SW_H #define GODOT_SHAPE_3D_H
#include "core/math/geometry_3d.h" #include "core/math/geometry_3d.h"
#include "core/templates/local_vector.h" #include "core/templates/local_vector.h"
#include "servers/physics_server_3d.h" #include "servers/physics_server_3d.h"
class Shape3DSW; class GodotShape3D;
class ShapeOwner3DSW { class GodotShapeOwner3D {
public: public:
virtual void _shape_changed() = 0; virtual void _shape_changed() = 0;
virtual void remove_shape(Shape3DSW *p_shape) = 0; virtual void remove_shape(GodotShape3D *p_shape) = 0;
virtual ~ShapeOwner3DSW() {} virtual ~GodotShapeOwner3D() {}
}; };
class Shape3DSW { class GodotShape3D {
RID self; RID self;
AABB aabb; AABB aabb;
bool configured = false; bool configured = false;
real_t custom_bias = 0.0; real_t custom_bias = 0.0;
Map<ShapeOwner3DSW *, int> owners; Map<GodotShapeOwner3D *, int> owners;
protected: protected:
void configure(const AABB &p_aabb); void configure(const AABB &p_aabb);
@ -90,29 +90,29 @@ public:
_FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias = p_bias; } _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; } _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
void add_owner(ShapeOwner3DSW *p_owner); void add_owner(GodotShapeOwner3D *p_owner);
void remove_owner(ShapeOwner3DSW *p_owner); void remove_owner(GodotShapeOwner3D *p_owner);
bool is_owner(ShapeOwner3DSW *p_owner) const; bool is_owner(GodotShapeOwner3D *p_owner) const;
const Map<ShapeOwner3DSW *, int> &get_owners() const; const Map<GodotShapeOwner3D *, int> &get_owners() const;
Shape3DSW() {} GodotShape3D() {}
virtual ~Shape3DSW(); virtual ~GodotShape3D();
}; };
class ConcaveShape3DSW : public Shape3DSW { class GodotConcaveShape3D : public GodotShape3D {
public: public:
virtual bool is_concave() const override { return true; } virtual bool is_concave() const override { return true; }
virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; } virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const override { r_amount = 0; }
// Returns true to stop the query. // Returns true to stop the query.
typedef bool (*QueryCallback)(void *p_userdata, Shape3DSW *p_convex); typedef bool (*QueryCallback)(void *p_userdata, GodotShape3D *p_convex);
virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0;
ConcaveShape3DSW() {} GodotConcaveShape3D() {}
}; };
class WorldBoundaryShape3DSW : public Shape3DSW { class GodotWorldBoundaryShape3D : public GodotShape3D {
Plane plane; Plane plane;
void _setup(const Plane &p_plane); void _setup(const Plane &p_plane);
@ -134,10 +134,10 @@ public:
virtual void set_data(const Variant &p_data) override; virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override; virtual Variant get_data() const override;
WorldBoundaryShape3DSW(); GodotWorldBoundaryShape3D();
}; };
class SeparationRayShape3DSW : public Shape3DSW { class GodotSeparationRayShape3D : public GodotShape3D {
real_t length = 1.0; real_t length = 1.0;
bool slide_on_slope = false; bool slide_on_slope = false;
@ -162,10 +162,10 @@ public:
virtual void set_data(const Variant &p_data) override; virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override; virtual Variant get_data() const override;
SeparationRayShape3DSW(); GodotSeparationRayShape3D();
}; };
class SphereShape3DSW : public Shape3DSW { class GodotSphereShape3D : public GodotShape3D {
real_t radius = 0.0; real_t radius = 0.0;
void _setup(real_t p_radius); void _setup(real_t p_radius);
@ -189,10 +189,10 @@ public:
virtual void set_data(const Variant &p_data) override; virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override; virtual Variant get_data() const override;
SphereShape3DSW(); GodotSphereShape3D();
}; };
class BoxShape3DSW : public Shape3DSW { class GodotBoxShape3D : public GodotShape3D {
Vector3 half_extents; Vector3 half_extents;
void _setup(const Vector3 &p_half_extents); void _setup(const Vector3 &p_half_extents);
@ -214,10 +214,10 @@ public:
virtual void set_data(const Variant &p_data) override; virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override; virtual Variant get_data() const override;
BoxShape3DSW(); GodotBoxShape3D();
}; };
class CapsuleShape3DSW : public Shape3DSW { class GodotCapsuleShape3D : public GodotShape3D {
real_t height = 0.0; real_t height = 0.0;
real_t radius = 0.0; real_t radius = 0.0;
@ -243,10 +243,10 @@ public:
virtual void set_data(const Variant &p_data) override; virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override; virtual Variant get_data() const override;
CapsuleShape3DSW(); GodotCapsuleShape3D();
}; };
class CylinderShape3DSW : public Shape3DSW { class GodotCylinderShape3D : public GodotShape3D {
real_t height = 0.0; real_t height = 0.0;
real_t radius = 0.0; real_t radius = 0.0;
@ -272,10 +272,10 @@ public:
virtual void set_data(const Variant &p_data) override; virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override; virtual Variant get_data() const override;
CylinderShape3DSW(); GodotCylinderShape3D();
}; };
struct ConvexPolygonShape3DSW : public Shape3DSW { struct GodotConvexPolygonShape3D : public GodotShape3D {
Geometry3D::MeshData mesh; Geometry3D::MeshData mesh;
void _setup(const Vector<Vector3> &p_vertices); void _setup(const Vector<Vector3> &p_vertices);
@ -297,13 +297,13 @@ public:
virtual void set_data(const Variant &p_data) override; virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override; virtual Variant get_data() const override;
ConvexPolygonShape3DSW(); GodotConvexPolygonShape3D();
}; };
struct _VolumeSW_BVH; struct _Volume_BVH;
struct FaceShape3DSW; struct GodotFaceShape3D;
struct ConcavePolygonShape3DSW : public ConcaveShape3DSW { struct GodotConcavePolygonShape3D : public GodotConcaveShape3D {
// always a trimesh // always a trimesh
struct Face { struct Face {
@ -331,7 +331,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
const Face *faces = nullptr; const Face *faces = nullptr;
const Vector3 *vertices = nullptr; const Vector3 *vertices = nullptr;
const BVH *bvh = nullptr; const BVH *bvh = nullptr;
FaceShape3DSW *face = nullptr; GodotFaceShape3D *face = nullptr;
}; };
struct _SegmentCullParams { struct _SegmentCullParams {
@ -341,7 +341,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
const Face *faces = nullptr; const Face *faces = nullptr;
const Vector3 *vertices = nullptr; const Vector3 *vertices = nullptr;
const BVH *bvh = nullptr; const BVH *bvh = nullptr;
FaceShape3DSW *face = nullptr; GodotFaceShape3D *face = nullptr;
Vector3 result; Vector3 result;
Vector3 normal; Vector3 normal;
@ -354,7 +354,7 @@ struct ConcavePolygonShape3DSW : public ConcaveShape3DSW {
void _cull_segment(int p_idx, _SegmentCullParams *p_params) const; void _cull_segment(int p_idx, _SegmentCullParams *p_params) const;
bool _cull(int p_idx, _CullParams *p_params) const; bool _cull(int p_idx, _CullParams *p_params) const;
void _fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx); void _fill_bvh(_Volume_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx);
void _setup(const Vector<Vector3> &p_faces, bool p_backface_collision); void _setup(const Vector<Vector3> &p_faces, bool p_backface_collision);
@ -377,10 +377,10 @@ public:
virtual void set_data(const Variant &p_data) override; virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override; virtual Variant get_data() const override;
ConcavePolygonShape3DSW(); GodotConcavePolygonShape3D();
}; };
struct HeightMapShape3DSW : public ConcaveShape3DSW { struct GodotHeightMapShape3D : public GodotConcaveShape3D {
Vector<real_t> heights; Vector<real_t> heights;
int width = 0; int width = 0;
int depth = 0; int depth = 0;
@ -440,11 +440,11 @@ public:
virtual void set_data(const Variant &p_data) override; virtual void set_data(const Variant &p_data) override;
virtual Variant get_data() const override; virtual Variant get_data() const override;
HeightMapShape3DSW(); GodotHeightMapShape3D();
}; };
//used internally //used internally
struct FaceShape3DSW : public Shape3DSW { struct GodotFaceShape3D : public GodotShape3D {
Vector3 normal; //cache Vector3 normal; //cache
Vector3 vertex[3]; Vector3 vertex[3];
bool backface_collision = false; bool backface_collision = false;
@ -465,11 +465,11 @@ struct FaceShape3DSW : public Shape3DSW {
virtual void set_data(const Variant &p_data) override {} virtual void set_data(const Variant &p_data) override {}
virtual Variant get_data() const override { return Variant(); } virtual Variant get_data() const override { return Variant(); }
FaceShape3DSW(); GodotFaceShape3D();
}; };
struct MotionShape3DSW : public Shape3DSW { struct GodotMotionShape3D : public GodotShape3D {
Shape3DSW *shape = nullptr; GodotShape3D *shape = nullptr;
Vector3 motion; Vector3 motion;
virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; } virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONVEX_POLYGON; }
@ -504,7 +504,7 @@ struct MotionShape3DSW : public Shape3DSW {
virtual void set_data(const Variant &p_data) override {} virtual void set_data(const Variant &p_data) override {}
virtual Variant get_data() const override { return Variant(); } virtual Variant get_data() const override { return Variant(); }
MotionShape3DSW() { configure(AABB()); } GodotMotionShape3D() { configure(AABB()); }
}; };
#endif // SHAPE_SW_H #endif // GODOT_SHAPE_3D_H

149
servers/physics_3d/soft_body_3d_sw.cpp → servers/physics_3d/godot_soft_body_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* soft_body_3d_sw.cpp */ /* godot_soft_body_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,8 +28,9 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "soft_body_3d_sw.h" #include "godot_soft_body_3d.h"
#include "space_3d_sw.h"
#include "godot_space_3d.h"
#include "core/math/geometry_3d.h" #include "core/math/geometry_3d.h"
#include "core/templates/map.h" #include "core/templates/map.h"
@ -53,16 +54,16 @@ subject to the following restrictions:
*/ */
///btSoftBody implementation by Nathanael Presson ///btSoftBody implementation by Nathanael Presson
SoftBody3DSW::SoftBody3DSW() : GodotSoftBody3D::GodotSoftBody3D() :
CollisionObject3DSW(TYPE_SOFT_BODY), GodotCollisionObject3D(TYPE_SOFT_BODY),
active_list(this) { active_list(this) {
_set_static(false); _set_static(false);
} }
void SoftBody3DSW::_shapes_changed() { void GodotSoftBody3D::_shapes_changed() {
} }
void SoftBody3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) { void GodotSoftBody3D::set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant) {
switch (p_state) { switch (p_state) {
case PhysicsServer3D::BODY_STATE_TRANSFORM: { case PhysicsServer3D::BODY_STATE_TRANSFORM: {
_set_transform(p_variant); _set_transform(p_variant);
@ -87,7 +88,7 @@ void SoftBody3DSW::set_state(PhysicsServer3D::BodyState p_state, const Variant &
} }
} }
Variant SoftBody3DSW::get_state(PhysicsServer3D::BodyState p_state) const { Variant GodotSoftBody3D::get_state(PhysicsServer3D::BodyState p_state) const {
switch (p_state) { switch (p_state) {
case PhysicsServer3D::BODY_STATE_TRANSFORM: { case PhysicsServer3D::BODY_STATE_TRANSFORM: {
return get_transform(); return get_transform();
@ -110,7 +111,7 @@ Variant SoftBody3DSW::get_state(PhysicsServer3D::BodyState p_state) const {
return Variant(); return Variant();
} }
void SoftBody3DSW::set_space(Space3DSW *p_space) { void GodotSoftBody3D::set_space(GodotSpace3D *p_space) {
if (get_space()) { if (get_space()) {
get_space()->soft_body_remove_from_active_list(&active_list); get_space()->soft_body_remove_from_active_list(&active_list);
@ -128,7 +129,7 @@ void SoftBody3DSW::set_space(Space3DSW *p_space) {
} }
} }
void SoftBody3DSW::set_mesh(RID p_mesh) { void GodotSoftBody3D::set_mesh(RID p_mesh) {
destroy(); destroy();
soft_mesh = p_mesh; soft_mesh = p_mesh;
@ -145,7 +146,7 @@ void SoftBody3DSW::set_mesh(RID p_mesh) {
} }
} }
void SoftBody3DSW::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) { void GodotSoftBody3D::update_rendering_server(RenderingServerHandler *p_rendering_server_handler) {
if (soft_mesh.is_null()) { if (soft_mesh.is_null()) {
return; return;
} }
@ -164,7 +165,7 @@ void SoftBody3DSW::update_rendering_server(RenderingServerHandler *p_rendering_s
p_rendering_server_handler->set_aabb(bounds); p_rendering_server_handler->set_aabb(bounds);
} }
void SoftBody3DSW::update_normals_and_centroids() { void GodotSoftBody3D::update_normals_and_centroids() {
uint32_t i, ni; uint32_t i, ni;
for (i = 0, ni = nodes.size(); i < ni; ++i) { for (i = 0, ni = nodes.size(); i < ni; ++i) {
@ -191,7 +192,7 @@ void SoftBody3DSW::update_normals_and_centroids() {
} }
} }
void SoftBody3DSW::update_bounds() { void GodotSoftBody3D::update_bounds() {
AABB prev_bounds = bounds; AABB prev_bounds = bounds;
prev_bounds.grow_by(collision_margin); prev_bounds.grow_by(collision_margin);
@ -223,13 +224,13 @@ void SoftBody3DSW::update_bounds() {
} }
} }
void SoftBody3DSW::update_constants() { void GodotSoftBody3D::update_constants() {
reset_link_rest_lengths(); reset_link_rest_lengths();
update_link_constants(); update_link_constants();
update_area(); update_area();
} }
void SoftBody3DSW::update_area() { void GodotSoftBody3D::update_area() {
int i, ni; int i, ni;
// Face area. // Face area.
@ -275,7 +276,7 @@ void SoftBody3DSW::update_area() {
} }
} }
void SoftBody3DSW::reset_link_rest_lengths() { void GodotSoftBody3D::reset_link_rest_lengths() {
for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { for (uint32_t i = 0, ni = links.size(); i < ni; ++i) {
Link &link = links[i]; Link &link = links[i];
link.rl = (link.n[0]->x - link.n[1]->x).length(); link.rl = (link.n[0]->x - link.n[1]->x).length();
@ -283,7 +284,7 @@ void SoftBody3DSW::reset_link_rest_lengths() {
} }
} }
void SoftBody3DSW::update_link_constants() { void GodotSoftBody3D::update_link_constants() {
real_t inv_linear_stiffness = 1.0 / linear_stiffness; real_t inv_linear_stiffness = 1.0 / linear_stiffness;
for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { for (uint32_t i = 0, ni = links.size(); i < ni; ++i) {
Link &link = links[i]; Link &link = links[i];
@ -291,7 +292,7 @@ void SoftBody3DSW::update_link_constants() {
} }
} }
void SoftBody3DSW::apply_nodes_transform(const Transform3D &p_transform) { void GodotSoftBody3D::apply_nodes_transform(const Transform3D &p_transform) {
if (soft_mesh.is_null()) { if (soft_mesh.is_null()) {
return; return;
} }
@ -317,7 +318,7 @@ void SoftBody3DSW::apply_nodes_transform(const Transform3D &p_transform) {
update_constants(); update_constants();
} }
Vector3 SoftBody3DSW::get_vertex_position(int p_index) const { Vector3 GodotSoftBody3D::get_vertex_position(int p_index) const {
ERR_FAIL_COND_V(p_index < 0, Vector3()); ERR_FAIL_COND_V(p_index < 0, Vector3());
if (soft_mesh.is_null()) { if (soft_mesh.is_null()) {
@ -331,7 +332,7 @@ Vector3 SoftBody3DSW::get_vertex_position(int p_index) const {
return nodes[node_index].x; return nodes[node_index].x;
} }
void SoftBody3DSW::set_vertex_position(int p_index, const Vector3 &p_position) { void GodotSoftBody3D::set_vertex_position(int p_index, const Vector3 &p_position) {
ERR_FAIL_COND(p_index < 0); ERR_FAIL_COND(p_index < 0);
if (soft_mesh.is_null()) { if (soft_mesh.is_null()) {
@ -347,7 +348,7 @@ void SoftBody3DSW::set_vertex_position(int p_index, const Vector3 &p_position) {
node.x = p_position; node.x = p_position;
} }
void SoftBody3DSW::pin_vertex(int p_index) { void GodotSoftBody3D::pin_vertex(int p_index) {
ERR_FAIL_COND(p_index < 0); ERR_FAIL_COND(p_index < 0);
if (is_vertex_pinned(p_index)) { if (is_vertex_pinned(p_index)) {
@ -366,7 +367,7 @@ void SoftBody3DSW::pin_vertex(int p_index) {
} }
} }
void SoftBody3DSW::unpin_vertex(int p_index) { void GodotSoftBody3D::unpin_vertex(int p_index) {
ERR_FAIL_COND(p_index < 0); ERR_FAIL_COND(p_index < 0);
uint32_t pinned_count = pinned_vertices.size(); uint32_t pinned_count = pinned_vertices.size();
@ -390,7 +391,7 @@ void SoftBody3DSW::unpin_vertex(int p_index) {
} }
} }
void SoftBody3DSW::unpin_all_vertices() { void GodotSoftBody3D::unpin_all_vertices() {
if (!soft_mesh.is_null()) { if (!soft_mesh.is_null()) {
real_t inv_node_mass = nodes.size() * inv_total_mass; real_t inv_node_mass = nodes.size() * inv_total_mass;
uint32_t pinned_count = pinned_vertices.size(); uint32_t pinned_count = pinned_vertices.size();
@ -409,7 +410,7 @@ void SoftBody3DSW::unpin_all_vertices() {
pinned_vertices.clear(); pinned_vertices.clear();
} }
bool SoftBody3DSW::is_vertex_pinned(int p_index) const { bool GodotSoftBody3D::is_vertex_pinned(int p_index) const {
ERR_FAIL_COND_V(p_index < 0, false); ERR_FAIL_COND_V(p_index < 0, false);
uint32_t pinned_count = pinned_vertices.size(); uint32_t pinned_count = pinned_vertices.size();
@ -422,47 +423,47 @@ bool SoftBody3DSW::is_vertex_pinned(int p_index) const {
return false; return false;
} }
uint32_t SoftBody3DSW::get_node_count() const { uint32_t GodotSoftBody3D::get_node_count() const {
return nodes.size(); return nodes.size();
} }
real_t SoftBody3DSW::get_node_inv_mass(uint32_t p_node_index) const { real_t GodotSoftBody3D::get_node_inv_mass(uint32_t p_node_index) const {
ERR_FAIL_COND_V(p_node_index >= nodes.size(), 0.0); ERR_FAIL_COND_V(p_node_index >= nodes.size(), 0.0);
return nodes[p_node_index].im; return nodes[p_node_index].im;
} }
Vector3 SoftBody3DSW::get_node_position(uint32_t p_node_index) const { Vector3 GodotSoftBody3D::get_node_position(uint32_t p_node_index) const {
ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3());
return nodes[p_node_index].x; return nodes[p_node_index].x;
} }
Vector3 SoftBody3DSW::get_node_velocity(uint32_t p_node_index) const { Vector3 GodotSoftBody3D::get_node_velocity(uint32_t p_node_index) const {
ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3());
return nodes[p_node_index].v; return nodes[p_node_index].v;
} }
Vector3 SoftBody3DSW::get_node_biased_velocity(uint32_t p_node_index) const { Vector3 GodotSoftBody3D::get_node_biased_velocity(uint32_t p_node_index) const {
ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3()); ERR_FAIL_COND_V(p_node_index >= nodes.size(), Vector3());
return nodes[p_node_index].bv; return nodes[p_node_index].bv;
} }
void SoftBody3DSW::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { void GodotSoftBody3D::apply_node_impulse(uint32_t p_node_index, const Vector3 &p_impulse) {
ERR_FAIL_COND(p_node_index >= nodes.size()); ERR_FAIL_COND(p_node_index >= nodes.size());
Node &node = nodes[p_node_index]; Node &node = nodes[p_node_index];
node.v += p_impulse * node.im; node.v += p_impulse * node.im;
} }
void SoftBody3DSW::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) { void GodotSoftBody3D::apply_node_bias_impulse(uint32_t p_node_index, const Vector3 &p_impulse) {
ERR_FAIL_COND(p_node_index >= nodes.size()); ERR_FAIL_COND(p_node_index >= nodes.size());
Node &node = nodes[p_node_index]; Node &node = nodes[p_node_index];
node.bv += p_impulse * node.im; node.bv += p_impulse * node.im;
} }
uint32_t SoftBody3DSW::get_face_count() const { uint32_t GodotSoftBody3D::get_face_count() const {
return faces.size(); return faces.size();
} }
void SoftBody3DSW::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const { void GodotSoftBody3D::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Vector3 &r_point_2, Vector3 &r_point_3) const {
ERR_FAIL_COND(p_face_index >= faces.size()); ERR_FAIL_COND(p_face_index >= faces.size());
const Face &face = faces[p_face_index]; const Face &face = faces[p_face_index];
r_point_1 = face.n[0]->x; r_point_1 = face.n[0]->x;
@ -470,12 +471,12 @@ void SoftBody3DSW::get_face_points(uint32_t p_face_index, Vector3 &r_point_1, Ve
r_point_3 = face.n[2]->x; r_point_3 = face.n[2]->x;
} }
Vector3 SoftBody3DSW::get_face_normal(uint32_t p_face_index) const { Vector3 GodotSoftBody3D::get_face_normal(uint32_t p_face_index) const {
ERR_FAIL_COND_V(p_face_index >= faces.size(), Vector3()); ERR_FAIL_COND_V(p_face_index >= faces.size(), Vector3());
return faces[p_face_index].normal; return faces[p_face_index].normal;
} }
bool SoftBody3DSW::create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices) { bool GodotSoftBody3D::create_from_trimesh(const Vector<int> &p_indices, const Vector<Vector3> &p_vertices) {
ERR_FAIL_COND_V(p_indices.is_empty(), false); ERR_FAIL_COND_V(p_indices.is_empty(), false);
ERR_FAIL_COND_V(p_vertices.is_empty(), false); ERR_FAIL_COND_V(p_vertices.is_empty(), false);
@ -595,7 +596,7 @@ bool SoftBody3DSW::create_from_trimesh(const Vector<int> &p_indices, const Vecto
return true; return true;
} }
void SoftBody3DSW::generate_bending_constraints(int p_distance) { void GodotSoftBody3D::generate_bending_constraints(int p_distance) {
uint32_t i, j; uint32_t i, j;
if (p_distance > 1) { if (p_distance > 1) {
@ -714,7 +715,7 @@ public:
}; };
typedef LinkDeps *LinkDepsPtr; typedef LinkDeps *LinkDepsPtr;
void SoftBody3DSW::reoptimize_link_order() { void GodotSoftBody3D::reoptimize_link_order() {
const int reop_not_dependent = -1; const int reop_not_dependent = -1;
const int reop_node_complete = -2; const int reop_node_complete = -2;
@ -823,7 +824,7 @@ void SoftBody3DSW::reoptimize_link_order() {
memdelete_arr(link_buffer); memdelete_arr(link_buffer);
} }
void SoftBody3DSW::append_link(uint32_t p_node1, uint32_t p_node2) { void GodotSoftBody3D::append_link(uint32_t p_node1, uint32_t p_node2) {
if (p_node1 == p_node2) { if (p_node1 == p_node2) {
return; return;
} }
@ -839,7 +840,7 @@ void SoftBody3DSW::append_link(uint32_t p_node1, uint32_t p_node2) {
links.push_back(link); links.push_back(link);
} }
void SoftBody3DSW::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3) { void GodotSoftBody3D::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_node3) {
if (p_node1 == p_node2) { if (p_node1 == p_node2) {
return; return;
} }
@ -864,11 +865,11 @@ void SoftBody3DSW::append_face(uint32_t p_node1, uint32_t p_node2, uint32_t p_no
faces.push_back(face); faces.push_back(face);
} }
void SoftBody3DSW::set_iteration_count(int p_val) { void GodotSoftBody3D::set_iteration_count(int p_val) {
iteration_count = p_val; iteration_count = p_val;
} }
void SoftBody3DSW::set_total_mass(real_t p_val) { void GodotSoftBody3D::set_total_mass(real_t p_val) {
ERR_FAIL_COND(p_val < 0.0); ERR_FAIL_COND(p_val < 0.0);
inv_total_mass = 1.0 / p_val; inv_total_mass = 1.0 / p_val;
@ -884,27 +885,27 @@ void SoftBody3DSW::set_total_mass(real_t p_val) {
update_constants(); update_constants();
} }
void SoftBody3DSW::set_collision_margin(real_t p_val) { void GodotSoftBody3D::set_collision_margin(real_t p_val) {
collision_margin = p_val; collision_margin = p_val;
} }
void SoftBody3DSW::set_linear_stiffness(real_t p_val) { void GodotSoftBody3D::set_linear_stiffness(real_t p_val) {
linear_stiffness = p_val; linear_stiffness = p_val;
} }
void SoftBody3DSW::set_pressure_coefficient(real_t p_val) { void GodotSoftBody3D::set_pressure_coefficient(real_t p_val) {
pressure_coefficient = p_val; pressure_coefficient = p_val;
} }
void SoftBody3DSW::set_damping_coefficient(real_t p_val) { void GodotSoftBody3D::set_damping_coefficient(real_t p_val) {
damping_coefficient = p_val; damping_coefficient = p_val;
} }
void SoftBody3DSW::set_drag_coefficient(real_t p_val) { void GodotSoftBody3D::set_drag_coefficient(real_t p_val) {
drag_coefficient = p_val; drag_coefficient = p_val;
} }
void SoftBody3DSW::add_velocity(const Vector3 &p_velocity) { void GodotSoftBody3D::add_velocity(const Vector3 &p_velocity) {
for (uint32_t i = 0, ni = nodes.size(); i < ni; ++i) { for (uint32_t i = 0, ni = nodes.size(); i < ni; ++i) {
Node &node = nodes[i]; Node &node = nodes[i];
if (node.im > 0) { if (node.im > 0) {
@ -913,7 +914,7 @@ void SoftBody3DSW::add_velocity(const Vector3 &p_velocity) {
} }
} }
void SoftBody3DSW::apply_forces(bool p_has_wind_forces) { void GodotSoftBody3D::apply_forces(bool p_has_wind_forces) {
int ac = areas.size(); int ac = areas.size();
if (nodes.is_empty()) { if (nodes.is_empty()) {
@ -977,13 +978,13 @@ void SoftBody3DSW::apply_forces(bool p_has_wind_forces) {
} }
} }
void SoftBody3DSW::_compute_area_gravity(const Area3DSW *p_area) { void GodotSoftBody3D::_compute_area_gravity(const GodotArea3D *p_area) {
Vector3 area_gravity; Vector3 area_gravity;
p_area->compute_gravity(get_transform().get_origin(), area_gravity); p_area->compute_gravity(get_transform().get_origin(), area_gravity);
gravity += area_gravity; gravity += area_gravity;
} }
Vector3 SoftBody3DSW::_compute_area_windforce(const Area3DSW *p_area, const Face *p_face) { Vector3 GodotSoftBody3D::_compute_area_windforce(const GodotArea3D *p_area, const Face *p_face) {
real_t wfm = p_area->get_wind_force_magnitude(); real_t wfm = p_area->get_wind_force_magnitude();
real_t waf = p_area->get_wind_attenuation_factor(); real_t waf = p_area->get_wind_attenuation_factor();
const Vector3 &wd = p_area->get_wind_direction(); const Vector3 &wd = p_area->get_wind_direction();
@ -995,12 +996,12 @@ Vector3 SoftBody3DSW::_compute_area_windforce(const Area3DSW *p_area, const Face
return nodal_force_magnitude * p_face->normal; return nodal_force_magnitude * p_face->normal;
} }
void SoftBody3DSW::predict_motion(real_t p_delta) { void GodotSoftBody3D::predict_motion(real_t p_delta) {
const real_t inv_delta = 1.0 / p_delta; const real_t inv_delta = 1.0 / p_delta;
ERR_FAIL_COND(!get_space()); ERR_FAIL_COND(!get_space());
Area3DSW *def_area = get_space()->get_default_area(); GodotArea3D *def_area = get_space()->get_default_area();
ERR_FAIL_COND(!def_area); ERR_FAIL_COND(!def_area);
gravity = def_area->get_gravity_vector() * def_area->get_gravity(); gravity = def_area->get_gravity_vector() * def_area->get_gravity();
@ -1083,7 +1084,7 @@ void SoftBody3DSW::predict_motion(real_t p_delta) {
face_tree.optimize_incremental(1); face_tree.optimize_incremental(1);
} }
void SoftBody3DSW::solve_constraints(real_t p_delta) { void GodotSoftBody3D::solve_constraints(real_t p_delta) {
const real_t inv_delta = 1.0 / p_delta; const real_t inv_delta = 1.0 / p_delta;
uint32_t i, ni; uint32_t i, ni;
@ -1120,7 +1121,7 @@ void SoftBody3DSW::solve_constraints(real_t p_delta) {
update_normals_and_centroids(); update_normals_and_centroids();
} }
void SoftBody3DSW::solve_links(real_t kst, real_t ti) { void GodotSoftBody3D::solve_links(real_t kst, real_t ti) {
for (uint32_t i = 0, ni = links.size(); i < ni; ++i) { for (uint32_t i = 0, ni = links.size(); i < ni; ++i) {
Link &link = links[i]; Link &link = links[i];
if (link.c0 > 0) { if (link.c0 > 0) {
@ -1138,16 +1139,16 @@ void SoftBody3DSW::solve_links(real_t kst, real_t ti) {
} }
struct AABBQueryResult { struct AABBQueryResult {
const SoftBody3DSW *soft_body = nullptr; const GodotSoftBody3D *soft_body = nullptr;
void *userdata = nullptr; void *userdata = nullptr;
SoftBody3DSW::QueryResultCallback result_callback = nullptr; GodotSoftBody3D::QueryResultCallback result_callback = nullptr;
_FORCE_INLINE_ bool operator()(void *p_data) { _FORCE_INLINE_ bool operator()(void *p_data) {
return result_callback(soft_body->get_node_index(p_data), userdata); return result_callback(soft_body->get_node_index(p_data), userdata);
}; };
}; };
void SoftBody3DSW::query_aabb(const AABB &p_aabb, SoftBody3DSW::QueryResultCallback p_result_callback, void *p_userdata) { void GodotSoftBody3D::query_aabb(const AABB &p_aabb, GodotSoftBody3D::QueryResultCallback p_result_callback, void *p_userdata) {
AABBQueryResult query_result; AABBQueryResult query_result;
query_result.soft_body = this; query_result.soft_body = this;
query_result.result_callback = p_result_callback; query_result.result_callback = p_result_callback;
@ -1157,16 +1158,16 @@ void SoftBody3DSW::query_aabb(const AABB &p_aabb, SoftBody3DSW::QueryResultCallb
} }
struct RayQueryResult { struct RayQueryResult {
const SoftBody3DSW *soft_body = nullptr; const GodotSoftBody3D *soft_body = nullptr;
void *userdata = nullptr; void *userdata = nullptr;
SoftBody3DSW::QueryResultCallback result_callback = nullptr; GodotSoftBody3D::QueryResultCallback result_callback = nullptr;
_FORCE_INLINE_ bool operator()(void *p_data) { _FORCE_INLINE_ bool operator()(void *p_data) {
return result_callback(soft_body->get_face_index(p_data), userdata); return result_callback(soft_body->get_face_index(p_data), userdata);
}; };
}; };
void SoftBody3DSW::query_ray(const Vector3 &p_from, const Vector3 &p_to, SoftBody3DSW::QueryResultCallback p_result_callback, void *p_userdata) { void GodotSoftBody3D::query_ray(const Vector3 &p_from, const Vector3 &p_to, GodotSoftBody3D::QueryResultCallback p_result_callback, void *p_userdata) {
if (face_tree.is_empty()) { if (face_tree.is_empty()) {
initialize_face_tree(); initialize_face_tree();
} }
@ -1179,7 +1180,7 @@ void SoftBody3DSW::query_ray(const Vector3 &p_from, const Vector3 &p_to, SoftBod
face_tree.ray_query(p_from, p_to, query_result); face_tree.ray_query(p_from, p_to, query_result);
} }
void SoftBody3DSW::initialize_face_tree() { void GodotSoftBody3D::initialize_face_tree() {
face_tree.clear(); face_tree.clear();
for (uint32_t i = 0; i < faces.size(); ++i) { for (uint32_t i = 0; i < faces.size(); ++i) {
Face &face = faces[i]; Face &face = faces[i];
@ -1196,7 +1197,7 @@ void SoftBody3DSW::initialize_face_tree() {
} }
} }
void SoftBody3DSW::update_face_tree(real_t p_delta) { void GodotSoftBody3D::update_face_tree(real_t p_delta) {
for (uint32_t i = 0; i < faces.size(); ++i) { for (uint32_t i = 0; i < faces.size(); ++i) {
const Face &face = faces[i]; const Face &face = faces[i];
@ -1220,25 +1221,25 @@ void SoftBody3DSW::update_face_tree(real_t p_delta) {
} }
} }
void SoftBody3DSW::initialize_shape(bool p_force_move) { void GodotSoftBody3D::initialize_shape(bool p_force_move) {
if (get_shape_count() == 0) { if (get_shape_count() == 0) {
SoftBodyShape3DSW *soft_body_shape = memnew(SoftBodyShape3DSW(this)); GodotSoftBodyShape3D *soft_body_shape = memnew(GodotSoftBodyShape3D(this));
add_shape(soft_body_shape); add_shape(soft_body_shape);
} else if (p_force_move) { } else if (p_force_move) {
SoftBodyShape3DSW *soft_body_shape = static_cast<SoftBodyShape3DSW *>(get_shape(0)); GodotSoftBodyShape3D *soft_body_shape = static_cast<GodotSoftBodyShape3D *>(get_shape(0));
soft_body_shape->update_bounds(); soft_body_shape->update_bounds();
} }
} }
void SoftBody3DSW::deinitialize_shape() { void GodotSoftBody3D::deinitialize_shape() {
if (get_shape_count() > 0) { if (get_shape_count() > 0) {
Shape3DSW *shape = get_shape(0); GodotShape3D *shape = get_shape(0);
remove_shape(shape); remove_shape(shape);
memdelete(shape); memdelete(shape);
} }
} }
void SoftBody3DSW::destroy() { void GodotSoftBody3D::destroy() {
soft_mesh = RID(); soft_mesh = RID();
map_visual_to_physics.clear(); map_visual_to_physics.clear();
@ -1254,7 +1255,7 @@ void SoftBody3DSW::destroy() {
deinitialize_shape(); deinitialize_shape();
} }
void SoftBodyShape3DSW::update_bounds() { void GodotSoftBodyShape3D::update_bounds() {
ERR_FAIL_COND(!soft_body); ERR_FAIL_COND(!soft_body);
AABB collision_aabb = soft_body->get_bounds(); AABB collision_aabb = soft_body->get_bounds();
@ -1262,13 +1263,13 @@ void SoftBodyShape3DSW::update_bounds() {
configure(collision_aabb); configure(collision_aabb);
} }
SoftBodyShape3DSW::SoftBodyShape3DSW(SoftBody3DSW *p_soft_body) { GodotSoftBodyShape3D::GodotSoftBodyShape3D(GodotSoftBody3D *p_soft_body) {
soft_body = p_soft_body; soft_body = p_soft_body;
update_bounds(); update_bounds();
} }
struct _SoftBodyIntersectSegmentInfo { struct _SoftBodyIntersectSegmentInfo {
const SoftBody3DSW *soft_body = nullptr; const GodotSoftBody3D *soft_body = nullptr;
Vector3 from; Vector3 from;
Vector3 dir; Vector3 dir;
Vector3 hit_position; Vector3 hit_position;
@ -1296,7 +1297,7 @@ struct _SoftBodyIntersectSegmentInfo {
} }
}; };
bool SoftBodyShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { bool GodotSoftBodyShape3D::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
_SoftBodyIntersectSegmentInfo query_info; _SoftBodyIntersectSegmentInfo query_info;
query_info.soft_body = soft_body; query_info.soft_body = soft_body;
query_info.from = p_begin; query_info.from = p_begin;
@ -1313,10 +1314,10 @@ bool SoftBodyShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3
return false; return false;
} }
bool SoftBodyShape3DSW::intersect_point(const Vector3 &p_point) const { bool GodotSoftBodyShape3D::intersect_point(const Vector3 &p_point) const {
return false; return false;
} }
Vector3 SoftBodyShape3DSW::get_closest_point_to(const Vector3 &p_point) const { Vector3 GodotSoftBodyShape3D::get_closest_point_to(const Vector3 &p_point) const {
ERR_FAIL_V_MSG(Vector3(), "Get closest point is not supported for soft bodies."); ERR_FAIL_V_MSG(Vector3(), "Get closest point is not supported for soft bodies.");
} }

48
servers/physics_3d/soft_body_3d_sw.h → servers/physics_3d/godot_soft_body_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* soft_body_3d_sw.h */ /* godot_soft_body_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,11 +28,11 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef SOFT_BODY_3D_SW_H #ifndef GODOT_SOFT_BODY_3D_H
#define SOFT_BODY_3D_SW_H #define GODOT_SOFT_BODY_3D_H
#include "area_3d_sw.h" #include "godot_area_3d.h"
#include "collision_object_3d_sw.h" #include "godot_collision_object_3d.h"
#include "core/math/aabb.h" #include "core/math/aabb.h"
#include "core/math/dynamic_bvh.h" #include "core/math/dynamic_bvh.h"
@ -41,9 +41,9 @@
#include "core/templates/set.h" #include "core/templates/set.h"
#include "core/templates/vset.h" #include "core/templates/vset.h"
class Constraint3DSW; class GodotConstraint3D;
class SoftBody3DSW : public CollisionObject3DSW { class GodotSoftBody3D : public GodotCollisionObject3D {
RID soft_mesh; RID soft_mesh;
struct Node { struct Node {
@ -103,9 +103,9 @@ class SoftBody3DSW : public CollisionObject3DSW {
Vector3 gravity; Vector3 gravity;
SelfList<SoftBody3DSW> active_list; SelfList<GodotSoftBody3D> active_list;
Set<Constraint3DSW *> constraints; Set<GodotConstraint3D *> constraints;
Vector<AreaCMP> areas; Vector<AreaCMP> areas;
@ -113,20 +113,20 @@ class SoftBody3DSW : public CollisionObject3DSW {
uint64_t island_step = 0; uint64_t island_step = 0;
_FORCE_INLINE_ void _compute_area_gravity(const Area3DSW *p_area); _FORCE_INLINE_ void _compute_area_gravity(const GodotArea3D *p_area);
_FORCE_INLINE_ Vector3 _compute_area_windforce(const Area3DSW *p_area, const Face *p_face); _FORCE_INLINE_ Vector3 _compute_area_windforce(const GodotArea3D *p_area, const Face *p_face);
public: public:
SoftBody3DSW(); GodotSoftBody3D();
const AABB &get_bounds() const { return bounds; } const AABB &get_bounds() const { return bounds; }
void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant); void set_state(PhysicsServer3D::BodyState p_state, const Variant &p_variant);
Variant get_state(PhysicsServer3D::BodyState p_state) const; Variant get_state(PhysicsServer3D::BodyState p_state) const;
_FORCE_INLINE_ void add_constraint(Constraint3DSW *p_constraint) { constraints.insert(p_constraint); } _FORCE_INLINE_ void add_constraint(GodotConstraint3D *p_constraint) { constraints.insert(p_constraint); }
_FORCE_INLINE_ void remove_constraint(Constraint3DSW *p_constraint) { constraints.erase(p_constraint); } _FORCE_INLINE_ void remove_constraint(GodotConstraint3D *p_constraint) { constraints.erase(p_constraint); }
_FORCE_INLINE_ const Set<Constraint3DSW *> &get_constraints() const { return constraints; } _FORCE_INLINE_ const Set<GodotConstraint3D *> &get_constraints() const { return constraints; }
_FORCE_INLINE_ void clear_constraints() { constraints.clear(); } _FORCE_INLINE_ void clear_constraints() { constraints.clear(); }
_FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); } _FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
@ -137,7 +137,7 @@ public:
_FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } _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 set_island_step(uint64_t p_step) { island_step = p_step; }
_FORCE_INLINE_ void add_area(Area3DSW *p_area) { _FORCE_INLINE_ void add_area(GodotArea3D *p_area) {
int index = areas.find(AreaCMP(p_area)); int index = areas.find(AreaCMP(p_area));
if (index > -1) { if (index > -1) {
areas.write[index].refCount += 1; areas.write[index].refCount += 1;
@ -146,7 +146,7 @@ public:
} }
} }
_FORCE_INLINE_ void remove_area(Area3DSW *p_area) { _FORCE_INLINE_ void remove_area(GodotArea3D *p_area) {
int index = areas.find(AreaCMP(p_area)); int index = areas.find(AreaCMP(p_area));
if (index > -1) { if (index > -1) {
areas.write[index].refCount -= 1; areas.write[index].refCount -= 1;
@ -156,7 +156,7 @@ public:
} }
} }
virtual void set_space(Space3DSW *p_space); virtual void set_space(GodotSpace3D *p_space);
void set_mesh(RID p_mesh); void set_mesh(RID p_mesh);
@ -251,11 +251,11 @@ private:
void destroy(); void destroy();
}; };
class SoftBodyShape3DSW : public Shape3DSW { class GodotSoftBodyShape3D : public GodotShape3D {
SoftBody3DSW *soft_body = nullptr; GodotSoftBody3D *soft_body = nullptr;
public: public:
SoftBody3DSW *get_soft_body() const { return soft_body; } GodotSoftBody3D *get_soft_body() const { return soft_body; }
virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SOFT_BODY; } virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_SOFT_BODY; }
virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { r_min = r_max = 0.0; } virtual void project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { r_min = r_max = 0.0; }
@ -272,8 +272,8 @@ public:
void update_bounds(); void update_bounds();
SoftBodyShape3DSW(SoftBody3DSW *p_soft_body); GodotSoftBodyShape3D(GodotSoftBody3D *p_soft_body);
~SoftBodyShape3DSW() {} ~GodotSoftBodyShape3D() {}
}; };
#endif // SOFT_BODY_3D_SW_H #endif // GODOT_SOFT_BODY_3D_H

263
servers/physics_3d/space_3d_sw.cpp → servers/physics_3d/godot_space_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* space_3d_sw.cpp */ /* godot_space_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,37 +28,38 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "space_3d_sw.h" #include "godot_space_3d.h"
#include "godot_collision_solver_3d.h"
#include "godot_physics_server_3d.h"
#include "collision_solver_3d_sw.h"
#include "core/config/project_settings.h" #include "core/config/project_settings.h"
#include "physics_server_3d_sw.h"
#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05 #define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
_FORCE_INLINE_ static bool _can_collide_with(CollisionObject3DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { _FORCE_INLINE_ static bool _can_collide_with(GodotCollisionObject3D *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
if (!(p_object->get_collision_layer() & p_collision_mask)) { if (!(p_object->get_collision_layer() & p_collision_mask)) {
return false; return false;
} }
if (p_object->get_type() == CollisionObject3DSW::TYPE_AREA && !p_collide_with_areas) { if (p_object->get_type() == GodotCollisionObject3D::TYPE_AREA && !p_collide_with_areas) {
return false; return false;
} }
if (p_object->get_type() == CollisionObject3DSW::TYPE_BODY && !p_collide_with_bodies) { if (p_object->get_type() == GodotCollisionObject3D::TYPE_BODY && !p_collide_with_bodies) {
return false; return false;
} }
if (p_object->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY && !p_collide_with_bodies) { if (p_object->get_type() == GodotCollisionObject3D::TYPE_SOFT_BODY && !p_collide_with_bodies) {
return false; return false;
} }
return true; return true;
} }
int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { int GodotPhysicsDirectSpaceState3D::intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
ERR_FAIL_COND_V(space->locked, false); ERR_FAIL_COND_V(space->locked, false);
int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_point(p_point, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
int cc = 0; int cc = 0;
//Transform3D ai = p_xform.affine_inverse(); //Transform3D ai = p_xform.affine_inverse();
@ -78,7 +79,7 @@ int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeRe
continue; continue;
} }
const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; const GodotCollisionObject3D *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i]; int shape_idx = space->intersection_query_subindex_results[i];
Transform3D inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); Transform3D inv_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
@ -103,7 +104,7 @@ int PhysicsDirectSpaceState3DSW::intersect_point(const Vector3 &p_point, ShapeRe
return cc; return cc;
} }
bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_ray) { bool GodotPhysicsDirectSpaceState3D::intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, bool p_pick_ray) {
ERR_FAIL_COND_V(space->locked, false); ERR_FAIL_COND_V(space->locked, false);
Vector3 begin, end; Vector3 begin, end;
@ -112,14 +113,14 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec
end = p_to; end = p_to;
normal = (end - begin).normalized(); normal = (end - begin).normalized();
int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_segment(begin, end, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
//todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision //todo, create another array that references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
bool collided = false; bool collided = false;
Vector3 res_point, res_normal; Vector3 res_point, res_normal;
int res_shape; int res_shape;
const CollisionObject3DSW *res_obj; const GodotCollisionObject3D *res_obj;
real_t min_d = 1e10; real_t min_d = 1e10;
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
@ -135,7 +136,7 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec
continue; continue;
} }
const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; const GodotCollisionObject3D *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i]; int shape_idx = space->intersection_query_subindex_results[i];
Transform3D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform(); Transform3D inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
@ -143,7 +144,7 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec
Vector3 local_from = inv_xform.xform(begin); Vector3 local_from = inv_xform.xform(begin);
Vector3 local_to = inv_xform.xform(end); Vector3 local_to = inv_xform.xform(end);
const Shape3DSW *shape = col_obj->get_shape(shape_idx); const GodotShape3D *shape = col_obj->get_shape(shape_idx);
Vector3 shape_point, shape_normal; Vector3 shape_point, shape_normal;
@ -182,17 +183,17 @@ bool PhysicsDirectSpaceState3DSW::intersect_ray(const Vector3 &p_from, const Vec
return true; return true;
} }
int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { int GodotPhysicsDirectSpaceState3D::intersect_shape(const RID &p_shape, const Transform3D &p_xform, real_t p_margin, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
if (p_result_max <= 0) { if (p_result_max <= 0) {
return 0; return 0;
} }
Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.get_or_null(p_shape); GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0); ERR_FAIL_COND_V(!shape, 0);
AABB aabb = p_xform.xform(shape->get_aabb()); AABB aabb = p_xform.xform(shape->get_aabb());
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
int cc = 0; int cc = 0;
@ -213,10 +214,10 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans
continue; continue;
} }
const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; const GodotCollisionObject3D *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i]; int shape_idx = space->intersection_query_subindex_results[i];
if (!CollisionSolver3DSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_margin, 0)) { if (!GodotCollisionSolver3D::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_margin, 0)) {
continue; continue;
} }
@ -237,21 +238,21 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans
return cc; return cc;
} }
bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { bool GodotPhysicsDirectSpaceState3D::cast_motion(const RID &p_shape, const Transform3D &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) {
Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.get_or_null(p_shape); GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, false); ERR_FAIL_COND_V(!shape, false);
AABB aabb = p_xform.xform(shape->get_aabb()); AABB aabb = p_xform.xform(shape->get_aabb());
aabb = aabb.merge(AABB(aabb.position + p_motion, aabb.size)); //motion aabb = aabb.merge(AABB(aabb.position + p_motion, aabb.size)); //motion
aabb = aabb.grow(p_margin); aabb = aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
real_t best_safe = 1; real_t best_safe = 1;
real_t best_unsafe = 1; real_t best_unsafe = 1;
Transform3D xform_inv = p_xform.affine_inverse(); Transform3D xform_inv = p_xform.affine_inverse();
MotionShape3DSW mshape; GodotMotionShape3D mshape;
mshape.shape = shape; mshape.shape = shape;
mshape.motion = xform_inv.basis.xform(p_motion); mshape.motion = xform_inv.basis.xform(p_motion);
@ -270,7 +271,7 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
continue; //ignore excluded continue; //ignore excluded
} }
const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; const GodotCollisionObject3D *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i]; int shape_idx = space->intersection_query_subindex_results[i];
Vector3 point_A, point_B; Vector3 point_A, point_B;
@ -278,14 +279,14 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way? //test initial overlap, does it collide if going all the way?
if (CollisionSolver3DSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { if (GodotCollisionSolver3D::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
continue; continue;
} }
//test initial overlap, ignore objects it's inside of. //test initial overlap, ignore objects it's inside of.
sep_axis = motion_normal; sep_axis = motion_normal;
if (!CollisionSolver3DSW::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) { if (!GodotCollisionSolver3D::solve_distance(shape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, aabb, &sep_axis)) {
continue; continue;
} }
@ -300,7 +301,7 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
Vector3 lA, lB; Vector3 lA, lB;
Vector3 sep = motion_normal; //important optimization for this to work fast enough Vector3 sep = motion_normal; //important optimization for this to work fast enough
bool collided = !CollisionSolver3DSW::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep); bool collided = !GodotCollisionSolver3D::solve_distance(&mshape, p_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, aabb, &sep);
if (collided) { if (collided) {
hi = fraction; hi = fraction;
@ -342,8 +343,8 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
r_info->point = closest_B; r_info->point = closest_B;
r_info->normal = (closest_A - closest_B).normalized(); r_info->normal = (closest_A - closest_B).normalized();
best_first = false; best_first = false;
if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) { if (col_obj->get_type() == GodotCollisionObject3D::TYPE_BODY) {
const Body3DSW *body = static_cast<const Body3DSW *>(col_obj); const GodotBody3D *body = static_cast<const GodotBody3D *>(col_obj);
Vector3 rel_vec = closest_B - (body->get_transform().origin + body->get_center_of_mass()); Vector3 rel_vec = closest_B - (body->get_transform().origin + body->get_center_of_mass());
r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
} }
@ -356,36 +357,36 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
return true; return true;
} }
bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { bool GodotPhysicsDirectSpaceState3D::collide_shape(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, Vector3 *r_results, int p_result_max, int &r_result_count, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
if (p_result_max <= 0) { if (p_result_max <= 0) {
return false; return false;
} }
Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.get_or_null(p_shape); GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0); ERR_FAIL_COND_V(!shape, 0);
AABB aabb = p_shape_xform.xform(shape->get_aabb()); AABB aabb = p_shape_xform.xform(shape->get_aabb());
aabb = aabb.grow(p_margin); aabb = aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
bool collided = false; bool collided = false;
r_result_count = 0; r_result_count = 0;
PhysicsServer3DSW::CollCbkData cbk; GodotPhysicsServer3D::CollCbkData cbk;
cbk.max = p_result_max; cbk.max = p_result_max;
cbk.amount = 0; cbk.amount = 0;
cbk.ptr = r_results; cbk.ptr = r_results;
CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk; GodotCollisionSolver3D::CallbackResult cbkres = GodotPhysicsServer3D::_shape_col_cbk;
PhysicsServer3DSW::CollCbkData *cbkptr = &cbk; GodotPhysicsServer3D::CollCbkData *cbkptr = &cbk;
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) { if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
continue; continue;
} }
const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; const GodotCollisionObject3D *col_obj = space->intersection_query_results[i];
if (p_exclude.has(col_obj->get_self())) { if (p_exclude.has(col_obj->get_self())) {
continue; continue;
@ -393,7 +394,7 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform3D &
int shape_idx = space->intersection_query_subindex_results[i]; int shape_idx = space->intersection_query_subindex_results[i];
if (CollisionSolver3DSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) { if (GodotCollisionSolver3D::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) {
collided = true; collided = true;
} }
} }
@ -404,7 +405,7 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform3D &
} }
struct _RestResultData { struct _RestResultData {
const CollisionObject3DSW *object = nullptr; const GodotCollisionObject3D *object = nullptr;
int local_shape = 0; int local_shape = 0;
int shape = 0; int shape = 0;
Vector3 contact; Vector3 contact;
@ -413,7 +414,7 @@ struct _RestResultData {
}; };
struct _RestCallbackData { struct _RestCallbackData {
const CollisionObject3DSW *object = nullptr; const GodotCollisionObject3D *object = nullptr;
int local_shape = 0; int local_shape = 0;
int shape = 0; int shape = 0;
@ -486,8 +487,8 @@ static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vect
rd->best_result.local_shape = rd->local_shape; rd->best_result.local_shape = rd->local_shape;
} }
bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { bool GodotPhysicsDirectSpaceState3D::rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.get_or_null(p_shape); GodotShape3D *shape = GodotPhysicsServer3D::godot_singleton->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0); ERR_FAIL_COND_V(!shape, 0);
real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR; real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
@ -495,7 +496,7 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_sh
AABB aabb = p_shape_xform.xform(shape->get_aabb()); AABB aabb = p_shape_xform.xform(shape->get_aabb());
aabb = aabb.grow(p_margin); aabb = aabb.grow(p_margin);
int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, Space3DSW::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results); int amount = space->broadphase->cull_aabb(aabb, space->intersection_query_results, GodotSpace3D::INTERSECTION_QUERY_MAX, space->intersection_query_subindex_results);
_RestCallbackData rcd; _RestCallbackData rcd;
rcd.min_allowed_depth = min_contact_depth; rcd.min_allowed_depth = min_contact_depth;
@ -505,7 +506,7 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_sh
continue; continue;
} }
const CollisionObject3DSW *col_obj = space->intersection_query_results[i]; const GodotCollisionObject3D *col_obj = space->intersection_query_results[i];
if (p_exclude.has(col_obj->get_self())) { if (p_exclude.has(col_obj->get_self())) {
continue; continue;
@ -515,7 +516,7 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_sh
rcd.object = col_obj; rcd.object = col_obj;
rcd.shape = shape_idx; rcd.shape = shape_idx;
bool sc = CollisionSolver3DSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin); bool sc = GodotCollisionSolver3D::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin);
if (!sc) { if (!sc) {
continue; continue;
} }
@ -530,8 +531,8 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_sh
r_info->normal = rcd.best_result.normal; r_info->normal = rcd.best_result.normal;
r_info->point = rcd.best_result.contact; r_info->point = rcd.best_result.contact;
r_info->rid = rcd.best_result.object->get_self(); r_info->rid = rcd.best_result.object->get_self();
if (rcd.best_result.object->get_type() == CollisionObject3DSW::TYPE_BODY) { if (rcd.best_result.object->get_type() == GodotCollisionObject3D::TYPE_BODY) {
const Body3DSW *body = static_cast<const Body3DSW *>(rcd.best_result.object); const GodotBody3D *body = static_cast<const GodotBody3D *>(rcd.best_result.object);
Vector3 rel_vec = rcd.best_result.contact - (body->get_transform().origin + body->get_center_of_mass()); Vector3 rel_vec = rcd.best_result.contact - (body->get_transform().origin + body->get_center_of_mass());
r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); r_info->linear_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
@ -542,10 +543,10 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_sh
return true; return true;
} }
Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { Vector3 GodotPhysicsDirectSpaceState3D::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const {
CollisionObject3DSW *obj = PhysicsServer3DSW::singletonsw->area_owner.get_or_null(p_object); GodotCollisionObject3D *obj = GodotPhysicsServer3D::godot_singleton->area_owner.get_or_null(p_object);
if (!obj) { if (!obj) {
obj = PhysicsServer3DSW::singletonsw->body_owner.get_or_null(p_object); obj = GodotPhysicsServer3D::godot_singleton->body_owner.get_or_null(p_object);
} }
ERR_FAIL_COND_V(!obj, Vector3()); ERR_FAIL_COND_V(!obj, Vector3());
@ -562,7 +563,7 @@ Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_ob
} }
Transform3D shape_xform = obj->get_transform() * obj->get_shape_transform(i); Transform3D shape_xform = obj->get_transform() * obj->get_shape_transform(i);
Shape3DSW *shape = obj->get_shape(i); GodotShape3D *shape = obj->get_shape(i);
Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point)); Vector3 point = shape->get_closest_point_to(shape_xform.affine_inverse().xform(p_point));
point = shape_xform.xform(point); point = shape_xform.xform(point);
@ -582,13 +583,13 @@ Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_ob
} }
} }
PhysicsDirectSpaceState3DSW::PhysicsDirectSpaceState3DSW() { GodotPhysicsDirectSpaceState3D::GodotPhysicsDirectSpaceState3D() {
space = nullptr; space = nullptr;
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////
int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) { int GodotSpace3D::_cull_aabb_for_body(GodotBody3D *p_body, const AABB &p_aabb) {
int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results); int amount = broadphase->cull_aabb(p_aabb, intersection_query_results, INTERSECTION_QUERY_MAX, intersection_query_subindex_results);
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
@ -596,13 +597,13 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) {
if (intersection_query_results[i] == p_body) { if (intersection_query_results[i] == p_body) {
keep = false; keep = false;
} else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_AREA) { } else if (intersection_query_results[i]->get_type() == GodotCollisionObject3D::TYPE_AREA) {
keep = false; keep = false;
} else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY) { } else if (intersection_query_results[i]->get_type() == GodotCollisionObject3D::TYPE_SOFT_BODY) {
keep = false; keep = false;
} else if (!p_body->collides_with(static_cast<Body3DSW *>(intersection_query_results[i]))) { } else if (!p_body->collides_with(static_cast<GodotBody3D *>(intersection_query_results[i]))) {
keep = false; keep = false;
} else if (static_cast<Body3DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) { } else if (static_cast<GodotBody3D *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) {
keep = false; keep = false;
} }
@ -620,7 +621,7 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) {
return amount; return amount;
} }
bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) { bool GodotSpace3D::test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result) {
//give me back regular physics engine logic //give me back regular physics engine logic
//this is madness //this is madness
//and most people using this function will think //and most people using this function will think
@ -679,13 +680,13 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::Motion
Vector3 sr[max_results * 2]; Vector3 sr[max_results * 2];
do { do {
PhysicsServer3DSW::CollCbkData cbk; GodotPhysicsServer3D::CollCbkData cbk;
cbk.max = max_results; cbk.max = max_results;
cbk.amount = 0; cbk.amount = 0;
cbk.ptr = sr; cbk.ptr = sr;
PhysicsServer3DSW::CollCbkData *cbkptr = &cbk; GodotPhysicsServer3D::CollCbkData *cbkptr = &cbk;
CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk; GodotCollisionSolver3D::CallbackResult cbkres = GodotPhysicsServer3D::_shape_col_cbk;
bool collided = false; bool collided = false;
@ -697,10 +698,10 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::Motion
} }
Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j);
Shape3DSW *body_shape = p_body->get_shape(j); GodotShape3D *body_shape = p_body->get_shape(j);
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
const CollisionObject3DSW *col_obj = intersection_query_results[i]; const GodotCollisionObject3D *col_obj = intersection_query_results[i];
if (p_parameters.exclude_bodies.has(col_obj->get_self())) { if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
continue; continue;
} }
@ -710,7 +711,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::Motion
int shape_idx = intersection_query_subindex_results[i]; int shape_idx = intersection_query_subindex_results[i];
if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_parameters.margin)) { if (GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_parameters.margin)) {
collided = cbk.amount > 0; collided = cbk.amount > 0;
} }
} }
@ -770,13 +771,13 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::Motion
continue; continue;
} }
Shape3DSW *body_shape = p_body->get_shape(j); GodotShape3D *body_shape = p_body->get_shape(j);
// Colliding separation rays allows to properly snap to the ground, // Colliding separation rays allows to properly snap to the ground,
// otherwise it's not needed in regular motion. // otherwise it's not needed in regular motion.
if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer3D::SHAPE_SEPARATION_RAY)) { if (!p_parameters.collide_separation_ray && (body_shape->get_type() == PhysicsServer3D::SHAPE_SEPARATION_RAY)) {
// When slide on slope is on, separation ray shape acts like a regular shape. // When slide on slope is on, separation ray shape acts like a regular shape.
if (!static_cast<SeparationRayShape3DSW *>(body_shape)->get_slide_on_slope()) { if (!static_cast<GodotSeparationRayShape3D *>(body_shape)->get_slide_on_slope()) {
continue; continue;
} }
} }
@ -784,7 +785,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::Motion
Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j); Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j);
Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse(); Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse();
MotionShape3DSW mshape; GodotMotionShape3D mshape;
mshape.shape = body_shape; mshape.shape = body_shape;
mshape.motion = body_shape_xform_inv.basis.xform(p_parameters.motion); mshape.motion = body_shape_xform_inv.basis.xform(p_parameters.motion);
@ -794,7 +795,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::Motion
real_t best_unsafe = 1; real_t best_unsafe = 1;
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
const CollisionObject3DSW *col_obj = intersection_query_results[i]; const GodotCollisionObject3D *col_obj = intersection_query_results[i];
if (p_parameters.exclude_bodies.has(col_obj->get_self())) { if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
continue; continue;
} }
@ -810,12 +811,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::Motion
Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx); Transform3D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way? //test initial overlap, does it collide if going all the way?
if (CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { if (GodotCollisionSolver3D::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
continue; continue;
} }
sep_axis = motion_normal; sep_axis = motion_normal;
if (!CollisionSolver3DSW::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) { if (!GodotCollisionSolver3D::solve_distance(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, point_A, point_B, motion_aabb, &sep_axis)) {
stuck = true; stuck = true;
break; break;
} }
@ -831,7 +832,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::Motion
Vector3 lA, lB; Vector3 lA, lB;
Vector3 sep = motion_normal; //important optimization for this to work fast enough Vector3 sep = motion_normal; //important optimization for this to work fast enough
bool collided = !CollisionSolver3DSW::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep); bool collided = !GodotCollisionSolver3D::solve_distance(&mshape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj_xform, lA, lB, motion_aabb, &sep);
if (collided) { if (collided) {
hi = fraction; hi = fraction;
@ -911,14 +912,14 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::Motion
} }
Transform3D body_shape_xform = ugt * p_body->get_shape_transform(j); Transform3D body_shape_xform = ugt * p_body->get_shape_transform(j);
Shape3DSW *body_shape = p_body->get_shape(j); GodotShape3D *body_shape = p_body->get_shape(j);
body_aabb.position += p_parameters.motion * unsafe; body_aabb.position += p_parameters.motion * unsafe;
int amount = _cull_aabb_for_body(p_body, body_aabb); int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int i = 0; i < amount; i++) { for (int i = 0; i < amount; i++) {
const CollisionObject3DSW *col_obj = intersection_query_results[i]; const GodotCollisionObject3D *col_obj = intersection_query_results[i];
if (p_parameters.exclude_bodies.has(col_obj->get_self())) { if (p_parameters.exclude_bodies.has(col_obj->get_self())) {
continue; continue;
} }
@ -930,7 +931,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::Motion
rcd.object = col_obj; rcd.object = col_obj;
rcd.shape = shape_idx; rcd.shape = shape_idx;
bool sc = CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_parameters.margin); bool sc = GodotCollisionSolver3D::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_parameters.margin);
if (!sc) { if (!sc) {
continue; continue;
} }
@ -952,7 +953,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::Motion
collision.position = result.contact; collision.position = result.contact;
collision.depth = result.len; collision.depth = result.len;
const Body3DSW *body = static_cast<const Body3DSW *>(result.object); const GodotBody3D *body = static_cast<const GodotBody3D *>(result.object);
Vector3 rel_vec = result.contact - (body->get_transform().origin + body->get_center_of_mass()); Vector3 rel_vec = result.contact - (body->get_transform().origin + body->get_center_of_mass());
collision.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec); collision.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
@ -984,44 +985,44 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const PhysicsServer3D::Motion
return collided; return collided;
} }
void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self) { void *GodotSpace3D::_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self) {
if (!A->interacts_with(B)) { if (!A->interacts_with(B)) {
return nullptr; return nullptr;
} }
CollisionObject3DSW::Type type_A = A->get_type(); GodotCollisionObject3D::Type type_A = A->get_type();
CollisionObject3DSW::Type type_B = B->get_type(); GodotCollisionObject3D::Type type_B = B->get_type();
if (type_A > type_B) { if (type_A > type_B) {
SWAP(A, B); SWAP(A, B);
SWAP(p_subindex_A, p_subindex_B); SWAP(p_subindex_A, p_subindex_B);
SWAP(type_A, type_B); SWAP(type_A, type_B);
} }
Space3DSW *self = (Space3DSW *)p_self; GodotSpace3D *self = (GodotSpace3D *)p_self;
self->collision_pairs++; self->collision_pairs++;
if (type_A == CollisionObject3DSW::TYPE_AREA) { if (type_A == GodotCollisionObject3D::TYPE_AREA) {
Area3DSW *area = static_cast<Area3DSW *>(A); GodotArea3D *area = static_cast<GodotArea3D *>(A);
if (type_B == CollisionObject3DSW::TYPE_AREA) { if (type_B == GodotCollisionObject3D::TYPE_AREA) {
Area3DSW *area_b = static_cast<Area3DSW *>(B); GodotArea3D *area_b = static_cast<GodotArea3D *>(B);
Area2Pair3DSW *area2_pair = memnew(Area2Pair3DSW(area_b, p_subindex_B, area, p_subindex_A)); GodotArea2Pair3D *area2_pair = memnew(GodotArea2Pair3D(area_b, p_subindex_B, area, p_subindex_A));
return area2_pair; return area2_pair;
} else if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) { } else if (type_B == GodotCollisionObject3D::TYPE_SOFT_BODY) {
SoftBody3DSW *softbody = static_cast<SoftBody3DSW *>(B); GodotSoftBody3D *softbody = static_cast<GodotSoftBody3D *>(B);
AreaSoftBodyPair3DSW *soft_area_pair = memnew(AreaSoftBodyPair3DSW(softbody, p_subindex_B, area, p_subindex_A)); GodotAreaSoftBodyPair3D *soft_area_pair = memnew(GodotAreaSoftBodyPair3D(softbody, p_subindex_B, area, p_subindex_A));
return soft_area_pair; return soft_area_pair;
} else { } else {
Body3DSW *body = static_cast<Body3DSW *>(B); GodotBody3D *body = static_cast<GodotBody3D *>(B);
AreaPair3DSW *area_pair = memnew(AreaPair3DSW(body, p_subindex_B, area, p_subindex_A)); GodotAreaPair3D *area_pair = memnew(GodotAreaPair3D(body, p_subindex_B, area, p_subindex_A));
return area_pair; return area_pair;
} }
} else if (type_A == CollisionObject3DSW::TYPE_BODY) { } else if (type_A == GodotCollisionObject3D::TYPE_BODY) {
if (type_B == CollisionObject3DSW::TYPE_SOFT_BODY) { if (type_B == GodotCollisionObject3D::TYPE_SOFT_BODY) {
BodySoftBodyPair3DSW *soft_pair = memnew(BodySoftBodyPair3DSW((Body3DSW *)A, p_subindex_A, (SoftBody3DSW *)B)); GodotBodySoftBodyPair3D *soft_pair = memnew(GodotBodySoftBodyPair3D((GodotBody3D *)A, p_subindex_A, (GodotSoftBody3D *)B));
return soft_pair; return soft_pair;
} else { } else {
BodyPair3DSW *b = memnew(BodyPair3DSW((Body3DSW *)A, p_subindex_A, (Body3DSW *)B, p_subindex_B)); GodotBodyPair3D *b = memnew(GodotBodyPair3D((GodotBody3D *)A, p_subindex_A, (GodotBody3D *)B, p_subindex_B));
return b; return b;
} }
} else { } else {
@ -1031,110 +1032,110 @@ void *Space3DSW::_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, Coll
return nullptr; return nullptr;
} }
void Space3DSW::_broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self) { void GodotSpace3D::_broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_self) {
if (!p_data) { if (!p_data) {
return; return;
} }
Space3DSW *self = (Space3DSW *)p_self; GodotSpace3D *self = (GodotSpace3D *)p_self;
self->collision_pairs--; self->collision_pairs--;
Constraint3DSW *c = (Constraint3DSW *)p_data; GodotConstraint3D *c = (GodotConstraint3D *)p_data;
memdelete(c); memdelete(c);
} }
const SelfList<Body3DSW>::List &Space3DSW::get_active_body_list() const { const SelfList<GodotBody3D>::List &GodotSpace3D::get_active_body_list() const {
return active_list; return active_list;
} }
void Space3DSW::body_add_to_active_list(SelfList<Body3DSW> *p_body) { void GodotSpace3D::body_add_to_active_list(SelfList<GodotBody3D> *p_body) {
active_list.add(p_body); active_list.add(p_body);
} }
void Space3DSW::body_remove_from_active_list(SelfList<Body3DSW> *p_body) { void GodotSpace3D::body_remove_from_active_list(SelfList<GodotBody3D> *p_body) {
active_list.remove(p_body); active_list.remove(p_body);
} }
void Space3DSW::body_add_to_mass_properties_update_list(SelfList<Body3DSW> *p_body) { void GodotSpace3D::body_add_to_mass_properties_update_list(SelfList<GodotBody3D> *p_body) {
mass_properties_update_list.add(p_body); mass_properties_update_list.add(p_body);
} }
void Space3DSW::body_remove_from_mass_properties_update_list(SelfList<Body3DSW> *p_body) { void GodotSpace3D::body_remove_from_mass_properties_update_list(SelfList<GodotBody3D> *p_body) {
mass_properties_update_list.remove(p_body); mass_properties_update_list.remove(p_body);
} }
BroadPhase3DSW *Space3DSW::get_broadphase() { GodotBroadPhase3D *GodotSpace3D::get_broadphase() {
return broadphase; return broadphase;
} }
void Space3DSW::add_object(CollisionObject3DSW *p_object) { void GodotSpace3D::add_object(GodotCollisionObject3D *p_object) {
ERR_FAIL_COND(objects.has(p_object)); ERR_FAIL_COND(objects.has(p_object));
objects.insert(p_object); objects.insert(p_object);
} }
void Space3DSW::remove_object(CollisionObject3DSW *p_object) { void GodotSpace3D::remove_object(GodotCollisionObject3D *p_object) {
ERR_FAIL_COND(!objects.has(p_object)); ERR_FAIL_COND(!objects.has(p_object));
objects.erase(p_object); objects.erase(p_object);
} }
const Set<CollisionObject3DSW *> &Space3DSW::get_objects() const { const Set<GodotCollisionObject3D *> &GodotSpace3D::get_objects() const {
return objects; return objects;
} }
void Space3DSW::body_add_to_state_query_list(SelfList<Body3DSW> *p_body) { void GodotSpace3D::body_add_to_state_query_list(SelfList<GodotBody3D> *p_body) {
state_query_list.add(p_body); state_query_list.add(p_body);
} }
void Space3DSW::body_remove_from_state_query_list(SelfList<Body3DSW> *p_body) { void GodotSpace3D::body_remove_from_state_query_list(SelfList<GodotBody3D> *p_body) {
state_query_list.remove(p_body); state_query_list.remove(p_body);
} }
void Space3DSW::area_add_to_monitor_query_list(SelfList<Area3DSW> *p_area) { void GodotSpace3D::area_add_to_monitor_query_list(SelfList<GodotArea3D> *p_area) {
monitor_query_list.add(p_area); monitor_query_list.add(p_area);
} }
void Space3DSW::area_remove_from_monitor_query_list(SelfList<Area3DSW> *p_area) { void GodotSpace3D::area_remove_from_monitor_query_list(SelfList<GodotArea3D> *p_area) {
monitor_query_list.remove(p_area); monitor_query_list.remove(p_area);
} }
void Space3DSW::area_add_to_moved_list(SelfList<Area3DSW> *p_area) { void GodotSpace3D::area_add_to_moved_list(SelfList<GodotArea3D> *p_area) {
area_moved_list.add(p_area); area_moved_list.add(p_area);
} }
void Space3DSW::area_remove_from_moved_list(SelfList<Area3DSW> *p_area) { void GodotSpace3D::area_remove_from_moved_list(SelfList<GodotArea3D> *p_area) {
area_moved_list.remove(p_area); area_moved_list.remove(p_area);
} }
const SelfList<Area3DSW>::List &Space3DSW::get_moved_area_list() const { const SelfList<GodotArea3D>::List &GodotSpace3D::get_moved_area_list() const {
return area_moved_list; return area_moved_list;
} }
const SelfList<SoftBody3DSW>::List &Space3DSW::get_active_soft_body_list() const { const SelfList<GodotSoftBody3D>::List &GodotSpace3D::get_active_soft_body_list() const {
return active_soft_body_list; return active_soft_body_list;
} }
void Space3DSW::soft_body_add_to_active_list(SelfList<SoftBody3DSW> *p_soft_body) { void GodotSpace3D::soft_body_add_to_active_list(SelfList<GodotSoftBody3D> *p_soft_body) {
active_soft_body_list.add(p_soft_body); active_soft_body_list.add(p_soft_body);
} }
void Space3DSW::soft_body_remove_from_active_list(SelfList<SoftBody3DSW> *p_soft_body) { void GodotSpace3D::soft_body_remove_from_active_list(SelfList<GodotSoftBody3D> *p_soft_body) {
active_soft_body_list.remove(p_soft_body); active_soft_body_list.remove(p_soft_body);
} }
void Space3DSW::call_queries() { void GodotSpace3D::call_queries() {
while (state_query_list.first()) { while (state_query_list.first()) {
Body3DSW *b = state_query_list.first()->self(); GodotBody3D *b = state_query_list.first()->self();
state_query_list.remove(state_query_list.first()); state_query_list.remove(state_query_list.first());
b->call_queries(); b->call_queries();
} }
while (monitor_query_list.first()) { while (monitor_query_list.first()) {
Area3DSW *a = monitor_query_list.first()->self(); GodotArea3D *a = monitor_query_list.first()->self();
monitor_query_list.remove(monitor_query_list.first()); monitor_query_list.remove(monitor_query_list.first());
a->call_queries(); a->call_queries();
} }
} }
void Space3DSW::setup() { void GodotSpace3D::setup() {
contact_debug_count = 0; contact_debug_count = 0;
while (mass_properties_update_list.first()) { while (mass_properties_update_list.first()) {
mass_properties_update_list.first()->self()->update_mass_properties(); mass_properties_update_list.first()->self()->update_mass_properties();
@ -1142,11 +1143,11 @@ void Space3DSW::setup() {
} }
} }
void Space3DSW::update() { void GodotSpace3D::update() {
broadphase->update(); broadphase->update();
} }
void Space3DSW::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value) { void GodotSpace3D::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value) {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
contact_recycle_radius = p_value; contact_recycle_radius = p_value;
@ -1175,7 +1176,7 @@ void Space3DSW::set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_valu
} }
} }
real_t Space3DSW::get_param(PhysicsServer3D::SpaceParameter p_param) const { real_t GodotSpace3D::get_param(PhysicsServer3D::SpaceParameter p_param) const {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: case PhysicsServer3D::SPACE_PARAM_CONTACT_RECYCLE_RADIUS:
return contact_recycle_radius; return contact_recycle_radius;
@ -1197,38 +1198,38 @@ real_t Space3DSW::get_param(PhysicsServer3D::SpaceParameter p_param) const {
return 0; return 0;
} }
void Space3DSW::lock() { void GodotSpace3D::lock() {
locked = true; locked = true;
} }
void Space3DSW::unlock() { void GodotSpace3D::unlock() {
locked = false; locked = false;
} }
bool Space3DSW::is_locked() const { bool GodotSpace3D::is_locked() const {
return locked; return locked;
} }
PhysicsDirectSpaceState3DSW *Space3DSW::get_direct_state() { GodotPhysicsDirectSpaceState3D *GodotSpace3D::get_direct_state() {
return direct_access; return direct_access;
} }
Space3DSW::Space3DSW() { GodotSpace3D::GodotSpace3D() {
body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1); body_linear_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_linear", 0.1);
body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg2rad(8.0)); body_angular_velocity_sleep_threshold = GLOBAL_DEF("physics/3d/sleep_threshold_angular", Math::deg2rad(8.0));
body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5); body_time_to_sleep = GLOBAL_DEF("physics/3d/time_before_sleep", 0.5);
ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater")); ProjectSettings::get_singleton()->set_custom_property_info("physics/3d/time_before_sleep", PropertyInfo(Variant::FLOAT, "physics/3d/time_before_sleep", PROPERTY_HINT_RANGE, "0,5,0.01,or_greater"));
body_angular_velocity_damp_ratio = 10; body_angular_velocity_damp_ratio = 10;
broadphase = BroadPhase3DSW::create_func(); broadphase = GodotBroadPhase3D::create_func();
broadphase->set_pair_callback(_broadphase_pair, this); broadphase->set_pair_callback(_broadphase_pair, this);
broadphase->set_unpair_callback(_broadphase_unpair, this); broadphase->set_unpair_callback(_broadphase_unpair, this);
direct_access = memnew(PhysicsDirectSpaceState3DSW); direct_access = memnew(GodotPhysicsDirectSpaceState3D);
direct_access->space = this; direct_access->space = this;
} }
Space3DSW::~Space3DSW() { GodotSpace3D::~GodotSpace3D() {
memdelete(broadphase); memdelete(broadphase);
memdelete(direct_access); memdelete(direct_access);
} }

113
servers/physics_3d/space_3d_sw.h → servers/physics_3d/godot_space_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* space_3d_sw.h */ /* godot_space_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,25 +28,26 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef SPACE_SW_H #ifndef GODOT_SPACE_3D_H
#define SPACE_SW_H #define GODOT_SPACE_3D_H
#include "godot_area_3d.h"
#include "godot_area_pair_3d.h"
#include "godot_body_3d.h"
#include "godot_body_pair_3d.h"
#include "godot_broad_phase_3d.h"
#include "godot_collision_object_3d.h"
#include "godot_soft_body_3d.h"
#include "area_3d_sw.h"
#include "area_pair_3d_sw.h"
#include "body_3d_sw.h"
#include "body_pair_3d_sw.h"
#include "broad_phase_3d_sw.h"
#include "collision_object_3d_sw.h"
#include "core/config/project_settings.h" #include "core/config/project_settings.h"
#include "core/templates/hash_map.h" #include "core/templates/hash_map.h"
#include "core/typedefs.h" #include "core/typedefs.h"
#include "soft_body_3d_sw.h"
class PhysicsDirectSpaceState3DSW : public PhysicsDirectSpaceState3D { class GodotPhysicsDirectSpaceState3D : public PhysicsDirectSpaceState3D {
GDCLASS(PhysicsDirectSpaceState3DSW, PhysicsDirectSpaceState3D); GDCLASS(GodotPhysicsDirectSpaceState3D, PhysicsDirectSpaceState3D);
public: public:
Space3DSW *space; GodotSpace3D *space;
virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; virtual int intersect_point(const Vector3 &p_point, ShapeResult *r_results, int p_result_max, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) override; virtual bool intersect_ray(const Vector3 &p_from, const Vector3 &p_to, RayResult &r_result, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false, bool p_pick_ray = false) override;
@ -56,10 +57,10 @@ public:
virtual bool rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override; virtual bool rest_info(RID p_shape, const Transform3D &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude = Set<RID>(), uint32_t p_collision_mask = UINT32_MAX, bool p_collide_with_bodies = true, bool p_collide_with_areas = false) override;
virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override; virtual Vector3 get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const override;
PhysicsDirectSpaceState3DSW(); GodotPhysicsDirectSpaceState3D();
}; };
class Space3DSW { class GodotSpace3D {
public: public:
enum ElapsedTime { enum ElapsedTime {
ELAPSED_TIME_INTEGRATE_FORCES, ELAPSED_TIME_INTEGRATE_FORCES,
@ -74,23 +75,23 @@ public:
private: private:
uint64_t elapsed_time[ELAPSED_TIME_MAX] = {}; uint64_t elapsed_time[ELAPSED_TIME_MAX] = {};
PhysicsDirectSpaceState3DSW *direct_access; GodotPhysicsDirectSpaceState3D *direct_access;
RID self; RID self;
BroadPhase3DSW *broadphase; GodotBroadPhase3D *broadphase;
SelfList<Body3DSW>::List active_list; SelfList<GodotBody3D>::List active_list;
SelfList<Body3DSW>::List mass_properties_update_list; SelfList<GodotBody3D>::List mass_properties_update_list;
SelfList<Body3DSW>::List state_query_list; SelfList<GodotBody3D>::List state_query_list;
SelfList<Area3DSW>::List monitor_query_list; SelfList<GodotArea3D>::List monitor_query_list;
SelfList<Area3DSW>::List area_moved_list; SelfList<GodotArea3D>::List area_moved_list;
SelfList<SoftBody3DSW>::List active_soft_body_list; SelfList<GodotSoftBody3D>::List active_soft_body_list;
static void *_broadphase_pair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_self); static void *_broadphase_pair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_self);
static void _broadphase_unpair(CollisionObject3DSW *A, int p_subindex_A, CollisionObject3DSW *B, int p_subindex_B, void *p_data, void *p_self); static void _broadphase_unpair(GodotCollisionObject3D *A, int p_subindex_A, GodotCollisionObject3D *B, int p_subindex_B, void *p_data, void *p_self);
Set<CollisionObject3DSW *> objects; Set<GodotCollisionObject3D *> objects;
Area3DSW *area = nullptr; GodotArea3D *area = nullptr;
real_t contact_recycle_radius = 0.01; real_t contact_recycle_radius = 0.01;
real_t contact_max_separation = 0.05; real_t contact_max_separation = 0.05;
@ -101,7 +102,7 @@ private:
INTERSECTION_QUERY_MAX = 2048 INTERSECTION_QUERY_MAX = 2048
}; };
CollisionObject3DSW *intersection_query_results[INTERSECTION_QUERY_MAX]; GodotCollisionObject3D *intersection_query_results[INTERSECTION_QUERY_MAX];
int intersection_query_subindex_results[INTERSECTION_QUERY_MAX]; int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];
real_t body_linear_velocity_sleep_threshold; real_t body_linear_velocity_sleep_threshold;
@ -122,41 +123,41 @@ private:
Vector<Vector3> contact_debug; Vector<Vector3> contact_debug;
int contact_debug_count = 0; int contact_debug_count = 0;
friend class PhysicsDirectSpaceState3DSW; friend class GodotPhysicsDirectSpaceState3D;
int _cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb); int _cull_aabb_for_body(GodotBody3D *p_body, const AABB &p_aabb);
public: public:
_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
_FORCE_INLINE_ RID get_self() const { return self; } _FORCE_INLINE_ RID get_self() const { return self; }
void set_default_area(Area3DSW *p_area) { area = p_area; } void set_default_area(GodotArea3D *p_area) { area = p_area; }
Area3DSW *get_default_area() const { return area; } GodotArea3D *get_default_area() const { return area; }
const SelfList<Body3DSW>::List &get_active_body_list() const; const SelfList<GodotBody3D>::List &get_active_body_list() const;
void body_add_to_active_list(SelfList<Body3DSW> *p_body); void body_add_to_active_list(SelfList<GodotBody3D> *p_body);
void body_remove_from_active_list(SelfList<Body3DSW> *p_body); void body_remove_from_active_list(SelfList<GodotBody3D> *p_body);
void body_add_to_mass_properties_update_list(SelfList<Body3DSW> *p_body); void body_add_to_mass_properties_update_list(SelfList<GodotBody3D> *p_body);
void body_remove_from_mass_properties_update_list(SelfList<Body3DSW> *p_body); void body_remove_from_mass_properties_update_list(SelfList<GodotBody3D> *p_body);
void body_add_to_state_query_list(SelfList<Body3DSW> *p_body); void body_add_to_state_query_list(SelfList<GodotBody3D> *p_body);
void body_remove_from_state_query_list(SelfList<Body3DSW> *p_body); void body_remove_from_state_query_list(SelfList<GodotBody3D> *p_body);
void area_add_to_monitor_query_list(SelfList<Area3DSW> *p_area); void area_add_to_monitor_query_list(SelfList<GodotArea3D> *p_area);
void area_remove_from_monitor_query_list(SelfList<Area3DSW> *p_area); void area_remove_from_monitor_query_list(SelfList<GodotArea3D> *p_area);
void area_add_to_moved_list(SelfList<Area3DSW> *p_area); void area_add_to_moved_list(SelfList<GodotArea3D> *p_area);
void area_remove_from_moved_list(SelfList<Area3DSW> *p_area); void area_remove_from_moved_list(SelfList<GodotArea3D> *p_area);
const SelfList<Area3DSW>::List &get_moved_area_list() const; const SelfList<GodotArea3D>::List &get_moved_area_list() const;
const SelfList<SoftBody3DSW>::List &get_active_soft_body_list() const; const SelfList<GodotSoftBody3D>::List &get_active_soft_body_list() const;
void soft_body_add_to_active_list(SelfList<SoftBody3DSW> *p_soft_body); void soft_body_add_to_active_list(SelfList<GodotSoftBody3D> *p_soft_body);
void soft_body_remove_from_active_list(SelfList<SoftBody3DSW> *p_soft_body); void soft_body_remove_from_active_list(SelfList<GodotSoftBody3D> *p_soft_body);
BroadPhase3DSW *get_broadphase(); GodotBroadPhase3D *get_broadphase();
void add_object(CollisionObject3DSW *p_object); void add_object(GodotCollisionObject3D *p_object);
void remove_object(CollisionObject3DSW *p_object); void remove_object(GodotCollisionObject3D *p_object);
const Set<CollisionObject3DSW *> &get_objects() const; const Set<GodotCollisionObject3D *> &get_objects() const;
_FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; } _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_separation() const { return contact_max_separation; }
@ -189,7 +190,7 @@ public:
int get_collision_pairs() const { return collision_pairs; } int get_collision_pairs() const { return collision_pairs; }
PhysicsDirectSpaceState3DSW *get_direct_state(); GodotPhysicsDirectSpaceState3D *get_direct_state();
void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); } 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_ bool is_debugging_contacts() const { return !contact_debug.is_empty(); }
@ -207,10 +208,10 @@ public:
void set_elapsed_time(ElapsedTime p_time, uint64_t p_msec) { elapsed_time[p_time] = p_msec; } 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]; } uint64_t get_elapsed_time(ElapsedTime p_time) const { return elapsed_time[p_time]; }
bool test_body_motion(Body3DSW *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result); bool test_body_motion(GodotBody3D *p_body, const PhysicsServer3D::MotionParameters &p_parameters, PhysicsServer3D::MotionResult *r_result);
Space3DSW(); GodotSpace3D();
~Space3DSW(); ~GodotSpace3D();
}; };
#endif // SPACE__SW_H #endif // GODOT_SPACE_3D_H

97
servers/physics_3d/step_3d_sw.cpp → servers/physics_3d/godot_step_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* step_3d_sw.cpp */ /* godot_step_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,8 +28,9 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#include "step_3d_sw.h" #include "godot_step_3d.h"
#include "joints_3d_sw.h"
#include "godot_joint_3d.h"
#include "core/os/os.h" #include "core/os/os.h"
@ -39,7 +40,7 @@
#define ISLAND_SIZE_RESERVE 512 #define ISLAND_SIZE_RESERVE 512
#define CONSTRAINT_COUNT_RESERVE 1024 #define CONSTRAINT_COUNT_RESERVE 1024
void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) { void GodotStep3D::_populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island) {
p_body->set_island_step(_step); p_body->set_island_step(_step);
if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) { if (p_body->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC) {
@ -47,8 +48,8 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_bod
p_body_island.push_back(p_body); p_body_island.push_back(p_body);
} }
for (const KeyValue<Constraint3DSW *, int> &E : p_body->get_constraint_map()) { for (const KeyValue<GodotConstraint3D *, int> &E : p_body->get_constraint_map()) {
Constraint3DSW *constraint = (Constraint3DSW *)E.key; GodotConstraint3D *constraint = (GodotConstraint3D *)E.key;
if (constraint->get_island_step() == _step) { if (constraint->get_island_step() == _step) {
continue; // Already processed. continue; // Already processed.
} }
@ -62,7 +63,7 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_bod
if (i == E.value) { if (i == E.value) {
continue; continue;
} }
Body3DSW *other_body = constraint->get_body_ptr()[i]; GodotBody3D *other_body = constraint->get_body_ptr()[i];
if (other_body->get_island_step() == _step) { if (other_body->get_island_step() == _step) {
continue; // Already processed. continue; // Already processed.
} }
@ -74,7 +75,7 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_bod
// Find connected soft bodies. // Find connected soft bodies.
for (int i = 0; i < constraint->get_soft_body_count(); i++) { for (int i = 0; i < constraint->get_soft_body_count(); i++) {
SoftBody3DSW *soft_body = constraint->get_soft_body_ptr(i); GodotSoftBody3D *soft_body = constraint->get_soft_body_ptr(i);
if (soft_body->get_island_step() == _step) { if (soft_body->get_island_step() == _step) {
continue; // Already processed. continue; // Already processed.
} }
@ -83,11 +84,11 @@ void Step3DSW::_populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_bod
} }
} }
void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island) { void GodotStep3D::_populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island) {
p_soft_body->set_island_step(_step); p_soft_body->set_island_step(_step);
for (Set<Constraint3DSW *>::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) { for (Set<GodotConstraint3D *>::Element *E = p_soft_body->get_constraints().front(); E; E = E->next()) {
Constraint3DSW *constraint = (Constraint3DSW *)E->get(); GodotConstraint3D *constraint = (GodotConstraint3D *)E->get();
if (constraint->get_island_step() == _step) { if (constraint->get_island_step() == _step) {
continue; // Already processed. continue; // Already processed.
} }
@ -98,7 +99,7 @@ void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector
// Find connected rigid bodies. // Find connected rigid bodies.
for (int i = 0; i < constraint->get_body_count(); i++) { for (int i = 0; i < constraint->get_body_count(); i++) {
Body3DSW *body = constraint->get_body_ptr()[i]; GodotBody3D *body = constraint->get_body_ptr()[i];
if (body->get_island_step() == _step) { if (body->get_island_step() == _step) {
continue; // Already processed. continue; // Already processed.
} }
@ -110,16 +111,16 @@ void Step3DSW::_populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector
} }
} }
void Step3DSW::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) { void GodotStep3D::_setup_contraint(uint32_t p_constraint_index, void *p_userdata) {
Constraint3DSW *constraint = all_constraints[p_constraint_index]; GodotConstraint3D *constraint = all_constraints[p_constraint_index];
constraint->setup(delta); constraint->setup(delta);
} }
void Step3DSW::_pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const { void GodotStep3D::_pre_solve_island(LocalVector<GodotConstraint3D *> &p_constraint_island) const {
uint32_t constraint_count = p_constraint_island.size(); uint32_t constraint_count = p_constraint_island.size();
uint32_t valid_constraint_count = 0; uint32_t valid_constraint_count = 0;
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
Constraint3DSW *constraint = p_constraint_island[constraint_index]; GodotConstraint3D *constraint = p_constraint_island[constraint_index];
if (p_constraint_island[constraint_index]->pre_solve(delta)) { if (p_constraint_island[constraint_index]->pre_solve(delta)) {
// Keep this constraint for solving. // Keep this constraint for solving.
p_constraint_island[valid_constraint_count++] = constraint; p_constraint_island[valid_constraint_count++] = constraint;
@ -128,8 +129,8 @@ void Step3DSW::_pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_isl
p_constraint_island.resize(valid_constraint_count); p_constraint_island.resize(valid_constraint_count);
} }
void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) { void GodotStep3D::_solve_island(uint32_t p_island_index, void *p_userdata) {
LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[p_island_index]; LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[p_island_index];
int current_priority = 1; int current_priority = 1;
@ -146,7 +147,7 @@ void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) {
uint32_t priority_constraint_count = 0; uint32_t priority_constraint_count = 0;
++current_priority; ++current_priority;
for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) { for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
Constraint3DSW *constraint = constraint_island[constraint_index]; GodotConstraint3D *constraint = constraint_island[constraint_index];
if (constraint->get_priority() >= current_priority) { if (constraint->get_priority() >= current_priority) {
// Keep this constraint for the next iteration. // Keep this constraint for the next iteration.
constraint_island[priority_constraint_count++] = constraint; constraint_island[priority_constraint_count++] = constraint;
@ -156,12 +157,12 @@ void Step3DSW::_solve_island(uint32_t p_island_index, void *p_userdata) {
} }
} }
void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) const { void GodotStep3D::_check_suspend(const LocalVector<GodotBody3D *> &p_body_island) const {
bool can_sleep = true; bool can_sleep = true;
uint32_t body_count = p_body_island.size(); uint32_t body_count = p_body_island.size();
for (uint32_t body_index = 0; body_index < body_count; ++body_index) { for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
Body3DSW *body = p_body_island[body_index]; GodotBody3D *body = p_body_island[body_index];
if (!body->sleep_test(delta)) { if (!body->sleep_test(delta)) {
can_sleep = false; can_sleep = false;
@ -170,7 +171,7 @@ void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) cons
// Put all to sleep or wake up everyone. // Put all to sleep or wake up everyone.
for (uint32_t body_index = 0; body_index < body_count; ++body_index) { for (uint32_t body_index = 0; body_index < body_count; ++body_index) {
Body3DSW *body = p_body_island[body_index]; GodotBody3D *body = p_body_island[body_index];
bool active = body->is_active(); bool active = body->is_active();
@ -180,7 +181,7 @@ void Step3DSW::_check_suspend(const LocalVector<Body3DSW *> &p_body_island) cons
} }
} }
void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) { void GodotStep3D::step(GodotSpace3D *p_space, real_t p_delta, int p_iterations) {
p_space->lock(); // can't access space during this p_space->lock(); // can't access space during this
p_space->setup(); //update inertias, etc p_space->setup(); //update inertias, etc
@ -190,9 +191,9 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
iterations = p_iterations; iterations = p_iterations;
delta = p_delta; delta = p_delta;
const SelfList<Body3DSW>::List *body_list = &p_space->get_active_body_list(); const SelfList<GodotBody3D>::List *body_list = &p_space->get_active_body_list();
const SelfList<SoftBody3DSW>::List *soft_body_list = &p_space->get_active_soft_body_list(); const SelfList<GodotSoftBody3D>::List *soft_body_list = &p_space->get_active_soft_body_list();
/* INTEGRATE FORCES */ /* INTEGRATE FORCES */
@ -201,7 +202,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
int active_count = 0; int active_count = 0;
const SelfList<Body3DSW> *b = body_list->first(); const SelfList<GodotBody3D> *b = body_list->first();
while (b) { while (b) {
b->self()->integrate_forces(p_delta); b->self()->integrate_forces(p_delta);
b = b->next(); b = b->next();
@ -210,7 +211,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
/* UPDATE SOFT BODY MOTION */ /* UPDATE SOFT BODY MOTION */
const SelfList<SoftBody3DSW> *sb = soft_body_list->first(); const SelfList<GodotSoftBody3D> *sb = soft_body_list->first();
while (sb) { while (sb) {
sb->self()->predict_motion(p_delta); sb->self()->predict_motion(p_delta);
sb = sb->next(); sb = sb->next();
@ -221,7 +222,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime); p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_FORCES, profile_endtime - profile_begtime);
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
@ -229,11 +230,11 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
uint32_t island_count = 0; uint32_t island_count = 0;
const SelfList<Area3DSW>::List &aml = p_space->get_moved_area_list(); const SelfList<GodotArea3D>::List &aml = p_space->get_moved_area_list();
while (aml.first()) { while (aml.first()) {
for (const Set<Constraint3DSW *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) { for (const Set<GodotConstraint3D *>::Element *E = aml.first()->self()->get_constraints().front(); E; E = E->next()) {
Constraint3DSW *constraint = E->get(); GodotConstraint3D *constraint = E->get();
if (constraint->get_island_step() == _step) { if (constraint->get_island_step() == _step) {
continue; continue;
} }
@ -244,13 +245,13 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
if (constraint_islands.size() < island_count) { if (constraint_islands.size() < island_count) {
constraint_islands.resize(island_count); constraint_islands.resize(island_count);
} }
LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1];
constraint_island.clear(); constraint_island.clear();
all_constraints.push_back(constraint); all_constraints.push_back(constraint);
constraint_island.push_back(constraint); constraint_island.push_back(constraint);
} }
p_space->area_remove_from_moved_list((SelfList<Area3DSW> *)aml.first()); //faster to remove here p_space->area_remove_from_moved_list((SelfList<GodotArea3D> *)aml.first()); //faster to remove here
} }
/* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */ /* GENERATE CONSTRAINT ISLANDS FOR ACTIVE RIGID BODIES */
@ -260,14 +261,14 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
uint32_t body_island_count = 0; uint32_t body_island_count = 0;
while (b) { while (b) {
Body3DSW *body = b->self(); GodotBody3D *body = b->self();
if (body->get_island_step() != _step) { if (body->get_island_step() != _step) {
++body_island_count; ++body_island_count;
if (body_islands.size() < body_island_count) { if (body_islands.size() < body_island_count) {
body_islands.resize(body_island_count); body_islands.resize(body_island_count);
} }
LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1]; LocalVector<GodotBody3D *> &body_island = body_islands[body_island_count - 1];
body_island.clear(); body_island.clear();
body_island.reserve(BODY_ISLAND_SIZE_RESERVE); body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
@ -275,7 +276,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
if (constraint_islands.size() < island_count) { if (constraint_islands.size() < island_count) {
constraint_islands.resize(island_count); constraint_islands.resize(island_count);
} }
LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1];
constraint_island.clear(); constraint_island.clear();
constraint_island.reserve(ISLAND_SIZE_RESERVE); constraint_island.reserve(ISLAND_SIZE_RESERVE);
@ -296,14 +297,14 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
sb = soft_body_list->first(); sb = soft_body_list->first();
while (sb) { while (sb) {
SoftBody3DSW *soft_body = sb->self(); GodotSoftBody3D *soft_body = sb->self();
if (soft_body->get_island_step() != _step) { if (soft_body->get_island_step() != _step) {
++body_island_count; ++body_island_count;
if (body_islands.size() < body_island_count) { if (body_islands.size() < body_island_count) {
body_islands.resize(body_island_count); body_islands.resize(body_island_count);
} }
LocalVector<Body3DSW *> &body_island = body_islands[body_island_count - 1]; LocalVector<GodotBody3D *> &body_island = body_islands[body_island_count - 1];
body_island.clear(); body_island.clear();
body_island.reserve(BODY_ISLAND_SIZE_RESERVE); body_island.reserve(BODY_ISLAND_SIZE_RESERVE);
@ -311,7 +312,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
if (constraint_islands.size() < island_count) { if (constraint_islands.size() < island_count) {
constraint_islands.resize(island_count); constraint_islands.resize(island_count);
} }
LocalVector<Constraint3DSW *> &constraint_island = constraint_islands[island_count - 1]; LocalVector<GodotConstraint3D *> &constraint_island = constraint_islands[island_count - 1];
constraint_island.clear(); constraint_island.clear();
constraint_island.reserve(ISLAND_SIZE_RESERVE); constraint_island.reserve(ISLAND_SIZE_RESERVE);
@ -332,18 +333,18 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime); p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_GENERATE_ISLANDS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
/* SETUP CONSTRAINTS / PROCESS COLLISIONS */ /* SETUP CONSTRAINTS / PROCESS COLLISIONS */
uint32_t total_contraint_count = all_constraints.size(); uint32_t total_contraint_count = all_constraints.size();
work_pool.do_work(total_contraint_count, this, &Step3DSW::_setup_contraint, nullptr); work_pool.do_work(total_contraint_count, this, &GodotStep3D::_setup_contraint, nullptr);
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime); p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_SETUP_CONSTRAINTS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
@ -359,14 +360,14 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
// Warning: _solve_island modifies the constraint islands for optimization purpose, // Warning: _solve_island modifies the constraint islands for optimization purpose,
// their content is not reliable after these calls and shouldn't be used anymore. // their content is not reliable after these calls and shouldn't be used anymore.
if (island_count > 1) { if (island_count > 1) {
work_pool.do_work(island_count, this, &Step3DSW::_solve_island, nullptr); work_pool.do_work(island_count, this, &GodotStep3D::_solve_island, nullptr);
} else if (island_count > 0) { } else if (island_count > 0) {
_solve_island(0); _solve_island(0);
} }
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime); p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_SOLVE_CONSTRAINTS, profile_endtime - profile_begtime);
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
@ -374,7 +375,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
b = body_list->first(); b = body_list->first();
while (b) { while (b) {
const SelfList<Body3DSW> *n = b->next(); const SelfList<GodotBody3D> *n = b->next();
b->self()->integrate_velocities(p_delta); b->self()->integrate_velocities(p_delta);
b = n; b = n;
} }
@ -395,7 +396,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
{ //profile { //profile
profile_endtime = OS::get_singleton()->get_ticks_usec(); profile_endtime = OS::get_singleton()->get_ticks_usec();
p_space->set_elapsed_time(Space3DSW::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime); p_space->set_elapsed_time(GodotSpace3D::ELAPSED_TIME_INTEGRATE_VELOCITIES, profile_endtime - profile_begtime);
profile_begtime = profile_endtime; profile_begtime = profile_endtime;
} }
@ -406,7 +407,7 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
_step++; _step++;
} }
Step3DSW::Step3DSW() { GodotStep3D::GodotStep3D() {
body_islands.reserve(BODY_ISLAND_COUNT_RESERVE); body_islands.reserve(BODY_ISLAND_COUNT_RESERVE);
constraint_islands.reserve(ISLAND_COUNT_RESERVE); constraint_islands.reserve(ISLAND_COUNT_RESERVE);
all_constraints.reserve(CONSTRAINT_COUNT_RESERVE); all_constraints.reserve(CONSTRAINT_COUNT_RESERVE);
@ -414,6 +415,6 @@ Step3DSW::Step3DSW() {
work_pool.init(); work_pool.init();
} }
Step3DSW::~Step3DSW() { GodotStep3D::~GodotStep3D() {
work_pool.finish(); work_pool.finish();
} }

32
servers/physics_3d/step_3d_sw.h → servers/physics_3d/godot_step_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* step_3d_sw.h */ /* godot_step_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -28,15 +28,15 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef STEP_SW_H #ifndef GODOT_STEP_3D_H
#define STEP_SW_H #define GODOT_STEP_3D_H
#include "space_3d_sw.h" #include "godot_space_3d.h"
#include "core/templates/local_vector.h" #include "core/templates/local_vector.h"
#include "core/templates/thread_work_pool.h" #include "core/templates/thread_work_pool.h"
class Step3DSW { class GodotStep3D {
uint64_t _step = 1; uint64_t _step = 1;
int iterations = 0; int iterations = 0;
@ -44,21 +44,21 @@ class Step3DSW {
ThreadWorkPool work_pool; ThreadWorkPool work_pool;
LocalVector<LocalVector<Body3DSW *>> body_islands; LocalVector<LocalVector<GodotBody3D *>> body_islands;
LocalVector<LocalVector<Constraint3DSW *>> constraint_islands; LocalVector<LocalVector<GodotConstraint3D *>> constraint_islands;
LocalVector<Constraint3DSW *> all_constraints; LocalVector<GodotConstraint3D *> all_constraints;
void _populate_island(Body3DSW *p_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island); void _populate_island(GodotBody3D *p_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island);
void _populate_island_soft_body(SoftBody3DSW *p_soft_body, LocalVector<Body3DSW *> &p_body_island, LocalVector<Constraint3DSW *> &p_constraint_island); void _populate_island_soft_body(GodotSoftBody3D *p_soft_body, LocalVector<GodotBody3D *> &p_body_island, LocalVector<GodotConstraint3D *> &p_constraint_island);
void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr); void _setup_contraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
void _pre_solve_island(LocalVector<Constraint3DSW *> &p_constraint_island) const; void _pre_solve_island(LocalVector<GodotConstraint3D *> &p_constraint_island) const;
void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr); void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr);
void _check_suspend(const LocalVector<Body3DSW *> &p_body_island) const; void _check_suspend(const LocalVector<GodotBody3D *> &p_body_island) const;
public: public:
void step(Space3DSW *p_space, real_t p_delta, int p_iterations); void step(GodotSpace3D *p_space, real_t p_delta, int p_iterations);
Step3DSW(); GodotStep3D();
~Step3DSW(); ~GodotStep3D();
}; };
#endif // STEP__SW_H #endif // GODOT_STEP_3D_H

18
servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp → servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* cone_twist_joint_3d_sw.cpp */ /* godot_cone_twist_joint_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -49,7 +49,7 @@ subject to the following restrictions:
Written by: Marcus Hennix Written by: Marcus Hennix
*/ */
#include "cone_twist_joint_3d_sw.h" #include "godot_cone_twist_joint_3d.h"
static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
if (Math::abs(n.z) > Math_SQRT12) { if (Math::abs(n.z) > Math_SQRT12) {
@ -84,8 +84,8 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
return (y < 0.0f) ? -angle : angle; return (y < 0.0f) ? -angle : angle;
} }
ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) : GodotConeTwistJoint3D::GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) :
Joint3DSW(_arr, 2) { GodotJoint3D(_arr, 2) {
A = rbA; A = rbA;
B = rbB; B = rbB;
@ -96,7 +96,7 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans
B->add_constraint(this, 1); B->add_constraint(this, 1);
} }
bool ConeTwistJoint3DSW::setup(real_t p_timestep) { bool GodotConeTwistJoint3D::setup(real_t p_timestep) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
@ -129,7 +129,7 @@ bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
plane_space(normal[0], normal[1], normal[2]); plane_space(normal[0], normal[1], normal[2]);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
memnew_placement(&m_jac[i], JacobianEntry3DSW( memnew_placement(&m_jac[i], GodotJacobianEntry3D(
A->get_principal_inertia_axes().transposed(), A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(), B->get_principal_inertia_axes().transposed(),
pivotAInW - A->get_transform().origin - A->get_center_of_mass(), pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
@ -230,7 +230,7 @@ bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
return true; return true;
} }
void ConeTwistJoint3DSW::solve(real_t p_timestep) { void GodotConeTwistJoint3D::solve(real_t p_timestep) {
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
@ -312,7 +312,7 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
} }
} }
void ConeTwistJoint3DSW::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) { void GodotConeTwistJoint3D::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
m_swingSpan1 = p_value; m_swingSpan1 = p_value;
@ -335,7 +335,7 @@ void ConeTwistJoint3DSW::set_param(PhysicsServer3D::ConeTwistJointParam p_param,
} }
} }
real_t ConeTwistJoint3DSW::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const { real_t GodotConeTwistJoint3D::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: { case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
return m_swingSpan1; return m_swingSpan1;

28
servers/physics_3d/joints/cone_twist_joint_3d_sw.h → servers/physics_3d/joints/godot_cone_twist_joint_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* cone_twist_joint_3d_sw.h */ /* godot_cone_twist_joint_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library.
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios GodotConeTwistJoint3D is Copyright (c) 2007 Starbreeze Studios
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
@ -49,28 +49,28 @@ subject to the following restrictions:
Written by: Marcus Hennix Written by: Marcus Hennix
*/ */
#ifndef CONE_TWIST_JOINT_SW_H #ifndef GODOT_CONE_TWIST_JOINT_3D_H
#define CONE_TWIST_JOINT_SW_H #define GODOT_CONE_TWIST_JOINT_3D_H
#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h" #include "servers/physics_3d/godot_joint_3d.h"
#include "servers/physics_3d/joints_3d_sw.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc) // GodotConeTwistJoint3D can be used to simulate ragdoll joints (upper arm, leg etc).
class ConeTwistJoint3DSW : public Joint3DSW { class GodotConeTwistJoint3D : public GodotJoint3D {
#ifdef IN_PARALLELL_SOLVER #ifdef IN_PARALLELL_SOLVER
public: public:
#endif #endif
union { union {
struct { struct {
Body3DSW *A; GodotBody3D *A;
Body3DSW *B; GodotBody3D *B;
}; };
Body3DSW *_arr[2] = { nullptr, nullptr }; GodotBody3D *_arr[2] = { nullptr, nullptr };
}; };
JacobianEntry3DSW m_jac[3] = {}; //3 orthogonal linear constraints GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints
real_t m_appliedImpulse = 0.0; real_t m_appliedImpulse = 0.0;
Transform3D m_rbAFrame; Transform3D m_rbAFrame;
@ -107,7 +107,7 @@ public:
virtual bool setup(real_t p_step) override; virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step) override; virtual void solve(real_t p_step) override;
ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame); GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame);
void setAngularOnly(bool angularOnly) { void setAngularOnly(bool angularOnly) {
m_angularOnly = angularOnly; m_angularOnly = angularOnly;
@ -139,4 +139,4 @@ public:
real_t get_param(PhysicsServer3D::ConeTwistJointParam p_param) const; real_t get_param(PhysicsServer3D::ConeTwistJointParam p_param) const;
}; };
#endif // CONE_TWIST_JOINT_SW_H #endif // GODOT_CONE_TWIST_JOINT_3D_H

69
servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp → servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* generic_6dof_joint_3d_sw.cpp */ /* godot_generic_6dof_joint_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -49,18 +49,18 @@ subject to the following restrictions:
/* /*
2007-09-09 2007-09-09
Generic6DOFJointSW Refactored by Francisco Le?n GodotGeneric6DOFJoint3D Refactored by Francisco Le?n
email: projectileman@yahoo.com email: projectileman@yahoo.com
http://gimpact.sf.net http://gimpact.sf.net
*/ */
#include "generic_6dof_joint_3d_sw.h" #include "godot_generic_6dof_joint_3d.h"
#define GENERIC_D6_DISABLE_WARMSTARTING 1 #define GENERIC_D6_DISABLE_WARMSTARTING 1
//////////////////////////// G6DOFRotationalLimitMotorSW //////////////////////////////////// //////////////////////////// GodotG6DOFRotationalLimitMotor3D ////////////////////////////////////
int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) { int GodotG6DOFRotationalLimitMotor3D::testLimitValue(real_t test_value) {
if (m_loLimit > m_hiLimit) { if (m_loLimit > m_hiLimit) {
m_currentLimit = 0; //Free from violation m_currentLimit = 0; //Free from violation
return 0; return 0;
@ -80,9 +80,9 @@ int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) {
return 0; return 0;
} }
real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits( real_t GodotG6DOFRotationalLimitMotor3D::solveAngularLimits(
real_t timeStep, Vector3 &axis, real_t jacDiagABInv, real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic) { GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic) {
if (!needApplyTorques()) { if (!needApplyTorques()) {
return 0.0f; return 0.0f;
} }
@ -148,14 +148,13 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits(
return clippedMotorImpulse; return clippedMotorImpulse;
} }
//////////////////////////// End G6DOFRotationalLimitMotorSW //////////////////////////////////// //////////////////////////// GodotG6DOFTranslationalLimitMotor3D ////////////////////////////////////
//////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// real_t GodotG6DOFTranslationalLimitMotor3D::solveLinearAxis(
real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
real_t timeStep, real_t timeStep,
real_t jacDiagABInv, real_t jacDiagABInv,
Body3DSW *body1, const Vector3 &pointInA, GodotBody3D *body1, const Vector3 &pointInA,
Body3DSW *body2, const Vector3 &pointInB, GodotBody3D *body2, const Vector3 &pointInB,
bool p_body1_dynamic, bool p_body2_dynamic, bool p_body1_dynamic, bool p_body2_dynamic,
int limit_index, int limit_index,
const Vector3 &axis_normal_on_a, const Vector3 &axis_normal_on_a,
@ -217,10 +216,10 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
return normalImpulse; return normalImpulse;
} }
//////////////////////////// G6DOFTranslationalLimitMotorSW //////////////////////////////////// //////////////////////////// GodotGeneric6DOFJoint3D ////////////////////////////////////
Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) : GodotGeneric6DOFJoint3D::GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) :
Joint3DSW(_arr, 2), GodotJoint3D(_arr, 2),
m_frameInA(frameInA), m_frameInA(frameInA),
m_frameInB(frameInB), m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA) { m_useLinearReferenceFrameA(useLinearReferenceFrameA) {
@ -230,7 +229,7 @@ Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const T
B->add_constraint(this, 1); B->add_constraint(this, 1);
} }
void Generic6DOFJoint3DSW::calculateAngleInfo() { void GodotGeneric6DOFJoint3D::calculateAngleInfo() {
Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis; Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis;
m_calculatedAxisAngleDiff = relative_frame.get_euler_xyz(); m_calculatedAxisAngleDiff = relative_frame.get_euler_xyz();
@ -270,17 +269,17 @@ void Generic6DOFJoint3DSW::calculateAngleInfo() {
*/ */
} }
void Generic6DOFJoint3DSW::calculateTransforms() { void GodotGeneric6DOFJoint3D::calculateTransforms() {
m_calculatedTransformA = A->get_transform() * m_frameInA; m_calculatedTransformA = A->get_transform() * m_frameInA;
m_calculatedTransformB = B->get_transform() * m_frameInB; m_calculatedTransformB = B->get_transform() * m_frameInB;
calculateAngleInfo(); calculateAngleInfo();
} }
void Generic6DOFJoint3DSW::buildLinearJacobian( void GodotGeneric6DOFJoint3D::buildLinearJacobian(
JacobianEntry3DSW &jacLinear, const Vector3 &normalWorld, GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld,
const Vector3 &pivotAInW, const Vector3 &pivotBInW) { const Vector3 &pivotAInW, const Vector3 &pivotBInW) {
memnew_placement(&jacLinear, JacobianEntry3DSW( memnew_placement(&jacLinear, GodotJacobianEntry3D(
A->get_principal_inertia_axes().transposed(), A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(), B->get_principal_inertia_axes().transposed(),
pivotAInW - A->get_transform().origin - A->get_center_of_mass(), pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
@ -292,16 +291,16 @@ void Generic6DOFJoint3DSW::buildLinearJacobian(
B->get_inv_mass())); B->get_inv_mass()));
} }
void Generic6DOFJoint3DSW::buildAngularJacobian( void GodotGeneric6DOFJoint3D::buildAngularJacobian(
JacobianEntry3DSW &jacAngular, const Vector3 &jointAxisW) { GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW) {
memnew_placement(&jacAngular, JacobianEntry3DSW(jointAxisW, memnew_placement(&jacAngular, GodotJacobianEntry3D(jointAxisW,
A->get_principal_inertia_axes().transposed(), A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(), B->get_principal_inertia_axes().transposed(),
A->get_inv_inertia(), A->get_inv_inertia(),
B->get_inv_inertia())); B->get_inv_inertia()));
} }
bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) { bool GodotGeneric6DOFJoint3D::testAngularLimitMotor(int axis_index) {
real_t angle = m_calculatedAxisAngleDiff[axis_index]; real_t angle = m_calculatedAxisAngleDiff[axis_index];
//test limits //test limits
@ -309,7 +308,7 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) {
return m_angularLimits[axis_index].needApplyTorques(); return m_angularLimits[axis_index].needApplyTorques();
} }
bool Generic6DOFJoint3DSW::setup(real_t p_timestep) { bool GodotGeneric6DOFJoint3D::setup(real_t p_timestep) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
@ -365,7 +364,7 @@ bool Generic6DOFJoint3DSW::setup(real_t p_timestep) {
return true; return true;
} }
void Generic6DOFJoint3DSW::solve(real_t p_timestep) { void GodotGeneric6DOFJoint3D::solve(real_t p_timestep) {
m_timeStep = p_timestep; m_timeStep = p_timestep;
//calculateTransforms(); //calculateTransforms();
@ -414,19 +413,19 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
} }
} }
void Generic6DOFJoint3DSW::updateRHS(real_t timeStep) { void GodotGeneric6DOFJoint3D::updateRHS(real_t timeStep) {
(void)timeStep; (void)timeStep;
} }
Vector3 Generic6DOFJoint3DSW::getAxis(int axis_index) const { Vector3 GodotGeneric6DOFJoint3D::getAxis(int axis_index) const {
return m_calculatedAxis[axis_index]; return m_calculatedAxis[axis_index];
} }
real_t Generic6DOFJoint3DSW::getAngle(int axis_index) const { real_t GodotGeneric6DOFJoint3D::getAngle(int axis_index) const {
return m_calculatedAxisAngleDiff[axis_index]; return m_calculatedAxisAngleDiff[axis_index];
} }
void Generic6DOFJoint3DSW::calcAnchorPos() { void GodotGeneric6DOFJoint3D::calcAnchorPos() {
real_t imA = A->get_inv_mass(); real_t imA = A->get_inv_mass();
real_t imB = B->get_inv_mass(); real_t imB = B->get_inv_mass();
real_t weight; real_t weight;
@ -438,9 +437,9 @@ void Generic6DOFJoint3DSW::calcAnchorPos() {
const Vector3 &pA = m_calculatedTransformA.origin; const Vector3 &pA = m_calculatedTransformA.origin;
const Vector3 &pB = m_calculatedTransformB.origin; const Vector3 &pB = m_calculatedTransformB.origin;
m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight); m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight);
} // Generic6DOFJointSW::calcAnchorPos() }
void Generic6DOFJoint3DSW::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) { void GodotGeneric6DOFJoint3D::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) {
ERR_FAIL_INDEX(p_axis, 3); ERR_FAIL_INDEX(p_axis, 3);
switch (p_param) { switch (p_param) {
case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
@ -527,7 +526,7 @@ void Generic6DOFJoint3DSW::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DO
} }
} }
real_t Generic6DOFJoint3DSW::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const { real_t GodotGeneric6DOFJoint3D::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const {
ERR_FAIL_INDEX_V(p_axis, 3, 0); ERR_FAIL_INDEX_V(p_axis, 3, 0);
switch (p_param) { switch (p_param) {
case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: { case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
@ -615,7 +614,7 @@ real_t Generic6DOFJoint3DSW::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6
return 0; return 0;
} }
void Generic6DOFJoint3DSW::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) { void GodotGeneric6DOFJoint3D::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) {
ERR_FAIL_INDEX(p_axis, 3); ERR_FAIL_INDEX(p_axis, 3);
switch (p_flag) { switch (p_flag) {
@ -642,7 +641,7 @@ void Generic6DOFJoint3DSW::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOF
} }
} }
bool Generic6DOFJoint3DSW::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const { bool GodotGeneric6DOFJoint3D::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const {
ERR_FAIL_INDEX_V(p_axis, 3, 0); ERR_FAIL_INDEX_V(p_axis, 3, 0);
switch (p_flag) { switch (p_flag) {
case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: { case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {

111
servers/physics_3d/joints/generic_6dof_joint_3d_sw.h → servers/physics_3d/joints/godot_generic_6dof_joint_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* generic_6dof_joint_3d_sw.h */ /* godot_generic_6dof_joint_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -32,11 +32,11 @@
Adapted to Godot from the Bullet library. Adapted to Godot from the Bullet library.
*/ */
#ifndef GENERIC_6DOF_JOINT_SW_H #ifndef GODOT_GENERIC_6DOF_JOINT_3D_H
#define GENERIC_6DOF_JOINT_SW_H #define GODOT_GENERIC_6DOF_JOINT_3D_H
#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h" #include "servers/physics_3d/godot_joint_3d.h"
#include "servers/physics_3d/joints_3d_sw.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
@ -55,13 +55,13 @@ subject to the following restrictions:
/* /*
2007-09-09 2007-09-09
Generic6DOFJointSW Refactored by Francisco Le?n GodotGeneric6DOFJoint3D Refactored by Francisco Le?n
email: projectileman@yahoo.com email: projectileman@yahoo.com
http://gimpact.sf.net http://gimpact.sf.net
*/ */
//! Rotation Limit structure for generic joints //! Rotation Limit structure for generic joints
class G6DOFRotationalLimitMotor3DSW { class GodotG6DOFRotationalLimitMotor3D {
public: public:
//! limit_parameters //! limit_parameters
//!@{ //!@{
@ -86,29 +86,25 @@ public:
real_t m_accumulatedImpulse = 0.0; real_t m_accumulatedImpulse = 0.0;
//!@} //!@}
G6DOFRotationalLimitMotor3DSW() {} GodotG6DOFRotationalLimitMotor3D() {}
//! Is limited
bool isLimited() { bool isLimited() {
return (m_loLimit < m_hiLimit); return (m_loLimit < m_hiLimit);
} }
//! Need apply correction // Need apply correction.
bool needApplyTorques() { bool needApplyTorques() {
return (m_enableMotor || m_currentLimit != 0); return (m_enableMotor || m_currentLimit != 0);
} }
//! calculates error // Calculates m_currentLimit and m_currentLimitError.
/*!
calculates m_currentLimit and m_currentLimitError.
*/
int testLimitValue(real_t test_value); int testLimitValue(real_t test_value);
//! apply the correction impulses for two bodies // Apply the correction impulses for two bodies.
real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic); real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic);
}; };
class G6DOFTranslationalLimitMotor3DSW { class GodotG6DOFTranslationalLimitMotor3D {
public: public:
Vector3 m_lowerLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint lower limits Vector3 m_lowerLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint lower limits
Vector3 m_upperLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint upper limits Vector3 m_upperLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint upper limits
@ -135,23 +131,23 @@ public:
real_t solveLinearAxis( real_t solveLinearAxis(
real_t timeStep, real_t timeStep,
real_t jacDiagABInv, real_t jacDiagABInv,
Body3DSW *body1, const Vector3 &pointInA, GodotBody3D *body1, const Vector3 &pointInA,
Body3DSW *body2, const Vector3 &pointInB, GodotBody3D *body2, const Vector3 &pointInB,
bool p_body1_dynamic, bool p_body2_dynamic, bool p_body1_dynamic, bool p_body2_dynamic,
int limit_index, int limit_index,
const Vector3 &axis_normal_on_a, const Vector3 &axis_normal_on_a,
const Vector3 &anchorPos); const Vector3 &anchorPos);
}; };
class Generic6DOFJoint3DSW : public Joint3DSW { class GodotGeneric6DOFJoint3D : public GodotJoint3D {
protected: protected:
union { union {
struct { struct {
Body3DSW *A; GodotBody3D *A;
Body3DSW *B; GodotBody3D *B;
}; };
Body3DSW *_arr[2] = { nullptr, nullptr }; GodotBody3D *_arr[2] = { nullptr, nullptr };
}; };
//! relative_frames //! relative_frames
@ -162,18 +158,18 @@ protected:
//! Jacobians //! Jacobians
//!@{ //!@{
JacobianEntry3DSW m_jacLinear[3]; //!< 3 orthogonal linear constraints GodotJacobianEntry3D m_jacLinear[3]; //!< 3 orthogonal linear constraints
JacobianEntry3DSW m_jacAng[3]; //!< 3 orthogonal angular constraints GodotJacobianEntry3D m_jacAng[3]; //!< 3 orthogonal angular constraints
//!@} //!@}
//! Linear_Limit_parameters //! Linear_Limit_parameters
//!@{ //!@{
G6DOFTranslationalLimitMotor3DSW m_linearLimits; GodotG6DOFTranslationalLimitMotor3D m_linearLimits;
//!@} //!@}
//! hinge_parameters //! hinge_parameters
//!@{ //!@{
G6DOFRotationalLimitMotor3DSW m_angularLimits[3]; GodotG6DOFRotationalLimitMotor3D m_angularLimits[3];
//!@} //!@}
protected: protected:
@ -191,45 +187,35 @@ protected:
//!@} //!@}
Generic6DOFJoint3DSW(Generic6DOFJoint3DSW const &) = delete; GodotGeneric6DOFJoint3D(GodotGeneric6DOFJoint3D const &) = delete;
void operator=(Generic6DOFJoint3DSW const &) = delete; void operator=(GodotGeneric6DOFJoint3D const &) = delete;
void buildLinearJacobian( void buildLinearJacobian(
JacobianEntry3DSW &jacLinear, const Vector3 &normalWorld, GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld,
const Vector3 &pivotAInW, const Vector3 &pivotBInW); const Vector3 &pivotAInW, const Vector3 &pivotBInW);
void buildAngularJacobian(JacobianEntry3DSW &jacAngular, const Vector3 &jointAxisW); void buildAngularJacobian(GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW);
//! calcs the euler angles between the two bodies. //! calcs the euler angles between the two bodies.
void calculateAngleInfo(); void calculateAngleInfo();
public: public:
Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA); GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA);
virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; } virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; }
virtual bool setup(real_t p_step) override; virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step) override; virtual void solve(real_t p_step) override;
//! Calcs global transform of the offsets // Calcs the global transform for the joint offset for body A an B, and also calcs the angle differences between the bodies.
/*!
Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
\sa Generic6DOFJointSW.getCalculatedTransformA , Generic6DOFJointSW.getCalculatedTransformB, Generic6DOFJointSW.calculateAngleInfo
*/
void calculateTransforms(); void calculateTransforms();
//! Gets the global transform of the offset for body A // Gets the global transform of the offset for body A.
/*!
\sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
*/
const Transform3D &getCalculatedTransformA() const { const Transform3D &getCalculatedTransformA() const {
return m_calculatedTransformA; return m_calculatedTransformA;
} }
//! Gets the global transform of the offset for body B // Gets the global transform of the offset for body B.
/*!
\sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
*/
const Transform3D &getCalculatedTransformB() const { const Transform3D &getCalculatedTransformB() const {
return m_calculatedTransformB; return m_calculatedTransformB;
} }
@ -250,27 +236,16 @@ public:
return m_frameInB; return m_frameInB;
} }
//! performs Jacobian calculation, and also calculates angle differences and axis // Performs Jacobian calculation, and also calculates angle differences and axis.
void updateRHS(real_t timeStep); void updateRHS(real_t timeStep);
//! Get the rotation axis in global coordinates // Get the rotation axis in global coordinates.
/*!
\pre Generic6DOFJointSW.buildJacobian must be called previously.
*/
Vector3 getAxis(int axis_index) const; Vector3 getAxis(int axis_index) const;
//! Get the relative Euler angle // Get the relative Euler angle.
/*!
\pre Generic6DOFJointSW.buildJacobian must be called previously.
*/
real_t getAngle(int axis_index) const; real_t getAngle(int axis_index) const;
//! Test angular limit. // Calculates angular correction and returns true if limit needs to be corrected.
/*!
Calculates angular correction and returns true if limit needs to be corrected.
\pre Generic6DOFJointSW.buildJacobian must be called previously.
*/
bool testAngularLimitMotor(int axis_index); bool testAngularLimitMotor(int axis_index);
void setLinearLowerLimit(const Vector3 &linearLower) { void setLinearLowerLimit(const Vector3 &linearLower) {
@ -293,17 +268,17 @@ public:
m_angularLimits[2].m_hiLimit = angularUpper.z; m_angularLimits[2].m_hiLimit = angularUpper.z;
} }
//! Retrieves the angular limit information. // Retrieves the angular limit information.
G6DOFRotationalLimitMotor3DSW *getRotationalLimitMotor(int index) { GodotG6DOFRotationalLimitMotor3D *getRotationalLimitMotor(int index) {
return &m_angularLimits[index]; return &m_angularLimits[index];
} }
//! Retrieves the limit information. // Retrieves the limit information.
G6DOFTranslationalLimitMotor3DSW *getTranslationalLimitMotor() { GodotG6DOFTranslationalLimitMotor3D *getTranslationalLimitMotor() {
return &m_linearLimits; return &m_linearLimits;
} }
//first 3 are linear, next 3 are angular // First 3 are linear, next 3 are angular.
void setLimit(int axis, real_t lo, real_t hi) { void setLimit(int axis, real_t lo, real_t hi) {
if (axis < 3) { if (axis < 3) {
m_linearLimits.m_lowerLimit[axis] = lo; m_linearLimits.m_lowerLimit[axis] = lo;
@ -328,10 +303,10 @@ public:
return m_angularLimits[limitIndex - 3].isLimited(); return m_angularLimits[limitIndex - 3].isLimited();
} }
const Body3DSW *getRigidBodyA() const { const GodotBody3D *getRigidBodyA() const {
return A; return A;
} }
const Body3DSW *getRigidBodyB() const { const GodotBody3D *getRigidBodyB() const {
return B; return B;
} }
@ -344,4 +319,4 @@ public:
bool get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const; bool get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const;
}; };
#endif // GENERIC_6DOF_JOINT_SW_H #endif // GODOT_GENERIC_6DOF_JOINT_3D_H

34
servers/physics_3d/joints/hinge_joint_3d_sw.cpp → servers/physics_3d/joints/godot_hinge_joint_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* hinge_joint_3d_sw.cpp */ /* godot_hinge_joint_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -47,7 +47,7 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "hinge_joint_3d_sw.h" #include "godot_hinge_joint_3d.h"
static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) { static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
if (Math::abs(n.z) > Math_SQRT12) { if (Math::abs(n.z) > Math_SQRT12) {
@ -67,8 +67,8 @@ static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
} }
} }
HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameA, const Transform3D &frameB) : GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB) :
Joint3DSW(_arr, 2) { GodotJoint3D(_arr, 2) {
A = rbA; A = rbA;
B = rbB; B = rbB;
@ -83,9 +83,9 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &
B->add_constraint(this, 1); B->add_constraint(this, 1);
} }
HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB,
const Vector3 &axisInA, const Vector3 &axisInB) : const Vector3 &axisInA, const Vector3 &axisInB) :
Joint3DSW(_arr, 2) { GodotJoint3D(_arr, 2) {
A = rbA; A = rbA;
B = rbB; B = rbB;
@ -124,7 +124,7 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo
B->add_constraint(this, 1); B->add_constraint(this, 1);
} }
bool HingeJoint3DSW::setup(real_t p_step) { bool GodotHingeJoint3D::setup(real_t p_step) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
@ -149,7 +149,7 @@ bool HingeJoint3DSW::setup(real_t p_step) {
plane_space(normal[0], normal[1], normal[2]); plane_space(normal[0], normal[1], normal[2]);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
memnew_placement(&m_jac[i], JacobianEntry3DSW( memnew_placement(&m_jac[i], GodotJacobianEntry3D(
A->get_principal_inertia_axes().transposed(), A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(), B->get_principal_inertia_axes().transposed(),
pivotAInW - A->get_transform().origin - A->get_center_of_mass(), pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
@ -175,19 +175,19 @@ bool HingeJoint3DSW::setup(real_t p_step) {
Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local); Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local);
Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2)); Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
memnew_placement(&m_jacAng[0], JacobianEntry3DSW(jointAxis0, memnew_placement(&m_jacAng[0], GodotJacobianEntry3D(jointAxis0,
A->get_principal_inertia_axes().transposed(), A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(), B->get_principal_inertia_axes().transposed(),
A->get_inv_inertia(), A->get_inv_inertia(),
B->get_inv_inertia())); B->get_inv_inertia()));
memnew_placement(&m_jacAng[1], JacobianEntry3DSW(jointAxis1, memnew_placement(&m_jacAng[1], GodotJacobianEntry3D(jointAxis1,
A->get_principal_inertia_axes().transposed(), A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(), B->get_principal_inertia_axes().transposed(),
A->get_inv_inertia(), A->get_inv_inertia(),
B->get_inv_inertia())); B->get_inv_inertia()));
memnew_placement(&m_jacAng[2], JacobianEntry3DSW(hingeAxisWorld, memnew_placement(&m_jacAng[2], GodotJacobianEntry3D(hingeAxisWorld,
A->get_principal_inertia_axes().transposed(), A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(), B->get_principal_inertia_axes().transposed(),
A->get_inv_inertia(), A->get_inv_inertia(),
@ -226,7 +226,7 @@ bool HingeJoint3DSW::setup(real_t p_step) {
return true; return true;
} }
void HingeJoint3DSW::solve(real_t p_step) { void GodotHingeJoint3D::solve(real_t p_step) {
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin); Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin); Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
@ -377,7 +377,7 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
return (y < 0.0f) ? -angle : angle; return (y < 0.0f) ? -angle : angle;
} }
real_t HingeJoint3DSW::get_hinge_angle() { real_t GodotHingeJoint3D::get_hinge_angle() {
const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0)); const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0));
const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1)); const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1));
const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1)); const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1));
@ -385,7 +385,7 @@ real_t HingeJoint3DSW::get_hinge_angle() {
return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
} }
void HingeJoint3DSW::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) { void GodotHingeJoint3D::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::HINGE_JOINT_BIAS: case PhysicsServer3D::HINGE_JOINT_BIAS:
tau = p_value; tau = p_value;
@ -416,7 +416,7 @@ void HingeJoint3DSW::set_param(PhysicsServer3D::HingeJointParam p_param, real_t
} }
} }
real_t HingeJoint3DSW::get_param(PhysicsServer3D::HingeJointParam p_param) const { real_t GodotHingeJoint3D::get_param(PhysicsServer3D::HingeJointParam p_param) const {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::HINGE_JOINT_BIAS: case PhysicsServer3D::HINGE_JOINT_BIAS:
return tau; return tau;
@ -441,7 +441,7 @@ real_t HingeJoint3DSW::get_param(PhysicsServer3D::HingeJointParam p_param) const
return 0; return 0;
} }
void HingeJoint3DSW::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) { void GodotHingeJoint3D::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) {
switch (p_flag) { switch (p_flag) {
case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
m_useLimit = p_value; m_useLimit = p_value;
@ -454,7 +454,7 @@ void HingeJoint3DSW::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_val
} }
} }
bool HingeJoint3DSW::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const { bool GodotHingeJoint3D::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const {
switch (p_flag) { switch (p_flag) {
case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT: case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
return m_useLimit; return m_useLimit;

28
servers/physics_3d/joints/hinge_joint_3d_sw.h → servers/physics_3d/joints/godot_hinge_joint_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* hinge_joint_3d_sw.h */ /* godot_hinge_joint_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -32,11 +32,11 @@
Adapted to Godot from the Bullet library. Adapted to Godot from the Bullet library.
*/ */
#ifndef HINGE_JOINT_SW_H #ifndef GODOT_HINGE_JOINT_3D_H
#define HINGE_JOINT_SW_H #define GODOT_HINGE_JOINT_3D_H
#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h" #include "servers/physics_3d/godot_joint_3d.h"
#include "servers/physics_3d/joints_3d_sw.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
@ -53,18 +53,18 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
class HingeJoint3DSW : public Joint3DSW { class GodotHingeJoint3D : public GodotJoint3D {
union { union {
struct { struct {
Body3DSW *A; GodotBody3D *A;
Body3DSW *B; GodotBody3D *B;
}; };
Body3DSW *_arr[2] = {}; GodotBody3D *_arr[2] = {};
}; };
JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints GodotJacobianEntry3D m_jac[3]; //3 orthogonal linear constraints
JacobianEntry3DSW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor GodotJacobianEntry3D m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis. Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis.
Transform3D m_rbBFrame; Transform3D m_rbBFrame;
@ -109,8 +109,8 @@ public:
void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value); void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value);
bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const; bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const;
HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameA, const Transform3D &frameB); GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB);
HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB); GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
}; };
#endif // HINGE_JOINT_SW_H #endif // GODOT_HINGE_JOINT_3D_H

28
servers/physics_3d/joints/jacobian_entry_3d_sw.h → servers/physics_3d/joints/godot_jacobian_entry_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* jacobian_entry_3d_sw.h */ /* godot_jacobian_entry_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -32,8 +32,8 @@
Adapted to Godot from the Bullet library. Adapted to Godot from the Bullet library.
*/ */
#ifndef JACOBIAN_ENTRY_SW_H #ifndef GODOT_JACOBIAN_ENTRY_3D_H
#define JACOBIAN_ENTRY_SW_H #define GODOT_JACOBIAN_ENTRY_3D_H
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
@ -52,11 +52,11 @@ subject to the following restrictions:
#include "core/math/transform_3d.h" #include "core/math/transform_3d.h"
class JacobianEntry3DSW { class GodotJacobianEntry3D {
public: public:
JacobianEntry3DSW() {} GodotJacobianEntry3D() {}
//constraint between two different rigidbodies //constraint between two different rigidbodies
JacobianEntry3DSW( GodotJacobianEntry3D(
const Basis &world2A, const Basis &world2A,
const Basis &world2B, const Basis &world2B,
const Vector3 &rel_pos1, const Vector3 &rel_pos2, const Vector3 &rel_pos1, const Vector3 &rel_pos2,
@ -76,7 +76,7 @@ public:
} }
//angular constraint between two different rigidbodies //angular constraint between two different rigidbodies
JacobianEntry3DSW(const Vector3 &jointAxis, GodotJacobianEntry3D(const Vector3 &jointAxis,
const Basis &world2A, const Basis &world2A,
const Basis &world2B, const Basis &world2B,
const Vector3 &inertiaInvA, const Vector3 &inertiaInvA,
@ -92,7 +92,7 @@ public:
} }
//angular constraint between two different rigidbodies //angular constraint between two different rigidbodies
JacobianEntry3DSW(const Vector3 &axisInA, GodotJacobianEntry3D(const Vector3 &axisInA,
const Vector3 &axisInB, const Vector3 &axisInB,
const Vector3 &inertiaInvA, const Vector3 &inertiaInvA,
const Vector3 &inertiaInvB) : const Vector3 &inertiaInvB) :
@ -107,7 +107,7 @@ public:
} }
//constraint on one rigidbody //constraint on one rigidbody
JacobianEntry3DSW( GodotJacobianEntry3D(
const Basis &world2A, const Basis &world2A,
const Vector3 &rel_pos1, const Vector3 &rel_pos2, const Vector3 &rel_pos1, const Vector3 &rel_pos2,
const Vector3 &jointAxis, const Vector3 &jointAxis,
@ -126,16 +126,16 @@ public:
real_t getDiagonal() const { return m_Adiag; } real_t getDiagonal() const { return m_Adiag; }
// for two constraints on the same rigidbody (for example vehicle friction) // for two constraints on the same rigidbody (for example vehicle friction)
real_t getNonDiagonal(const JacobianEntry3DSW &jacB, const real_t massInvA) const { real_t getNonDiagonal(const GodotJacobianEntry3D &jacB, const real_t massInvA) const {
const JacobianEntry3DSW &jacA = *this; const GodotJacobianEntry3D &jacA = *this;
real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ); real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
return lin + ang; return lin + ang;
} }
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
real_t getNonDiagonal(const JacobianEntry3DSW &jacB, const real_t massInvA, const real_t massInvB) const { real_t getNonDiagonal(const GodotJacobianEntry3D &jacB, const real_t massInvA, const real_t massInvB) const {
const JacobianEntry3DSW &jacA = *this; const GodotJacobianEntry3D &jacA = *this;
Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
@ -166,4 +166,4 @@ public:
real_t m_Adiag = 1.0; real_t m_Adiag = 1.0;
}; };
#endif // JACOBIAN_ENTRY_SW_H #endif // GODOT_JACOBIAN_ENTRY_3D_H

20
servers/physics_3d/joints/pin_joint_3d_sw.cpp → servers/physics_3d/joints/godot_pin_joint_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* pin_joint_3d_sw.cpp */ /* godot_pin_joint_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -47,9 +47,9 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "pin_joint_3d_sw.h" #include "godot_pin_joint_3d.h"
bool PinJoint3DSW::setup(real_t p_step) { bool GodotPinJoint3D::setup(real_t p_step) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
@ -63,7 +63,7 @@ bool PinJoint3DSW::setup(real_t p_step) {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
normal[i] = 1; normal[i] = 1;
memnew_placement(&m_jac[i], JacobianEntry3DSW( memnew_placement(&m_jac[i], GodotJacobianEntry3D(
A->get_principal_inertia_axes().transposed(), A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(), B->get_principal_inertia_axes().transposed(),
A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(), A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(),
@ -79,7 +79,7 @@ bool PinJoint3DSW::setup(real_t p_step) {
return true; return true;
} }
void PinJoint3DSW::solve(real_t p_step) { void GodotPinJoint3D::solve(real_t p_step) {
Vector3 pivotAInW = A->get_transform().xform(m_pivotInA); Vector3 pivotAInW = A->get_transform().xform(m_pivotInA);
Vector3 pivotBInW = B->get_transform().xform(m_pivotInB); Vector3 pivotBInW = B->get_transform().xform(m_pivotInB);
@ -137,7 +137,7 @@ void PinJoint3DSW::solve(real_t p_step) {
} }
} }
void PinJoint3DSW::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) { void GodotPinJoint3D::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::PIN_JOINT_BIAS: case PhysicsServer3D::PIN_JOINT_BIAS:
m_tau = p_value; m_tau = p_value;
@ -151,7 +151,7 @@ void PinJoint3DSW::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_va
} }
} }
real_t PinJoint3DSW::get_param(PhysicsServer3D::PinJointParam p_param) const { real_t GodotPinJoint3D::get_param(PhysicsServer3D::PinJointParam p_param) const {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::PIN_JOINT_BIAS: case PhysicsServer3D::PIN_JOINT_BIAS:
return m_tau; return m_tau;
@ -164,8 +164,8 @@ real_t PinJoint3DSW::get_param(PhysicsServer3D::PinJointParam p_param) const {
return 0; return 0;
} }
PinJoint3DSW::PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW *p_body_b, const Vector3 &p_pos_b) : GodotPinJoint3D::GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b) :
Joint3DSW(_arr, 2) { GodotJoint3D(_arr, 2) {
A = p_body_a; A = p_body_a;
B = p_body_b; B = p_body_b;
m_pivotInA = p_pos_a; m_pivotInA = p_pos_a;
@ -175,5 +175,5 @@ PinJoint3DSW::PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW
B->add_constraint(this, 1); B->add_constraint(this, 1);
} }
PinJoint3DSW::~PinJoint3DSW() { GodotPinJoint3D::~GodotPinJoint3D() {
} }

26
servers/physics_3d/joints/pin_joint_3d_sw.h → servers/physics_3d/joints/godot_pin_joint_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* pin_joint_3d_sw.h */ /* godot_pin_joint_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -32,11 +32,11 @@
Adapted to Godot from the Bullet library. Adapted to Godot from the Bullet library.
*/ */
#ifndef PIN_JOINT_SW_H #ifndef GODOT_PIN_JOINT_3D_H
#define PIN_JOINT_SW_H #define GODOT_PIN_JOINT_3D_H
#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h" #include "servers/physics_3d/godot_joint_3d.h"
#include "servers/physics_3d/joints_3d_sw.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
@ -53,14 +53,14 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
class PinJoint3DSW : public Joint3DSW { class GodotPinJoint3D : public GodotJoint3D {
union { union {
struct { struct {
Body3DSW *A; GodotBody3D *A;
Body3DSW *B; GodotBody3D *B;
}; };
Body3DSW *_arr[2] = {}; GodotBody3D *_arr[2] = {};
}; };
real_t m_tau = 0.3; //bias real_t m_tau = 0.3; //bias
@ -68,7 +68,7 @@ class PinJoint3DSW : public Joint3DSW {
real_t m_impulseClamp = 0.0; real_t m_impulseClamp = 0.0;
real_t m_appliedImpulse = 0.0; real_t m_appliedImpulse = 0.0;
JacobianEntry3DSW m_jac[3] = {}; //3 orthogonal linear constraints GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints
Vector3 m_pivotInA; Vector3 m_pivotInA;
Vector3 m_pivotInB; Vector3 m_pivotInB;
@ -88,8 +88,8 @@ public:
Vector3 get_position_a() { return m_pivotInA; } Vector3 get_position_a() { return m_pivotInA; }
Vector3 get_position_b() { return m_pivotInB; } Vector3 get_position_b() { return m_pivotInB; }
PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW *p_body_b, const Vector3 &p_pos_b); GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b);
~PinJoint3DSW(); ~GodotPinJoint3D();
}; };
#endif // PIN_JOINT_SW_H #endif // GODOT_PIN_JOINT_3D_H

48
servers/physics_3d/joints/slider_joint_3d_sw.cpp → servers/physics_3d/joints/godot_slider_joint_3d.cpp
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* slider_joint_3d_sw.cpp */ /* godot_slider_joint_3d.cpp */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -53,7 +53,7 @@ April 04, 2008
*/ */
#include "slider_joint_3d_sw.h" #include "godot_slider_joint_3d.h"
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
@ -76,8 +76,8 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB) : GodotSliderJoint3D::GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB) :
Joint3DSW(_arr, 2), GodotJoint3D(_arr, 2),
m_frameInA(frameInA), m_frameInA(frameInA),
m_frameInB(frameInB) { m_frameInB(frameInB) {
A = rbA; A = rbA;
@ -85,11 +85,11 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D
A->add_constraint(this, 0); A->add_constraint(this, 0);
B->add_constraint(this, 1); B->add_constraint(this, 1);
} // SliderJointSW::SliderJointSW() }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
bool SliderJoint3DSW::setup(real_t p_step) { bool GodotSliderJoint3D::setup(real_t p_step) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC); dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
@ -112,7 +112,7 @@ bool SliderJoint3DSW::setup(real_t p_step) {
//linear part //linear part
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
normalWorld = m_calculatedTransformA.basis.get_axis(i); normalWorld = m_calculatedTransformA.basis.get_axis(i);
memnew_placement(&m_jacLin[i], JacobianEntry3DSW( memnew_placement(&m_jacLin[i], GodotJacobianEntry3D(
A->get_principal_inertia_axes().transposed(), A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(), B->get_principal_inertia_axes().transposed(),
m_relPosA - A->get_center_of_mass(), m_relPosA - A->get_center_of_mass(),
@ -129,7 +129,7 @@ bool SliderJoint3DSW::setup(real_t p_step) {
// angular part // angular part
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
normalWorld = m_calculatedTransformA.basis.get_axis(i); normalWorld = m_calculatedTransformA.basis.get_axis(i);
memnew_placement(&m_jacAng[i], JacobianEntry3DSW( memnew_placement(&m_jacAng[i], GodotJacobianEntry3D(
normalWorld, normalWorld,
A->get_principal_inertia_axes().transposed(), A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(), B->get_principal_inertia_axes().transposed(),
@ -144,11 +144,11 @@ bool SliderJoint3DSW::setup(real_t p_step) {
m_accumulatedAngMotorImpulse = real_t(0.0); m_accumulatedAngMotorImpulse = real_t(0.0);
return true; return true;
} // SliderJointSW::buildJacobianInt() }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void SliderJoint3DSW::solve(real_t p_step) { void GodotSliderJoint3D::solve(real_t p_step) {
int i; int i;
// linear // linear
Vector3 velA = A->get_velocity_in_local_point(m_relPosA); Vector3 velA = A->get_velocity_in_local_point(m_relPosA);
@ -284,13 +284,11 @@ void SliderJoint3DSW::solve(real_t p_step) {
} }
} }
} }
} // SliderJointSW::solveConstraint() }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
//----------------------------------------------------------------------------- void GodotSliderJoint3D::calculateTransforms() {
void SliderJoint3DSW::calculateTransforms() {
m_calculatedTransformA = A->get_transform() * m_frameInA; m_calculatedTransformA = A->get_transform() * m_frameInA;
m_calculatedTransformB = B->get_transform() * m_frameInB; m_calculatedTransformB = B->get_transform() * m_frameInB;
m_realPivotAInW = m_calculatedTransformA.origin; m_realPivotAInW = m_calculatedTransformA.origin;
@ -305,11 +303,11 @@ void SliderJoint3DSW::calculateTransforms() {
normalWorld = m_calculatedTransformA.basis.get_axis(i); normalWorld = m_calculatedTransformA.basis.get_axis(i);
m_depth[i] = m_delta.dot(normalWorld); m_depth[i] = m_delta.dot(normalWorld);
} }
} // SliderJointSW::calculateTransforms() }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void SliderJoint3DSW::testLinLimits() { void GodotSliderJoint3D::testLinLimits() {
m_solveLinLim = false; m_solveLinLim = false;
m_linPos = m_depth[0]; m_linPos = m_depth[0];
if (m_lowerLinLimit <= m_upperLinLimit) { if (m_lowerLinLimit <= m_upperLinLimit) {
@ -325,11 +323,11 @@ void SliderJoint3DSW::testLinLimits() {
} else { } else {
m_depth[0] = real_t(0.); m_depth[0] = real_t(0.);
} }
} // SliderJointSW::testLinLimits() }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void SliderJoint3DSW::testAngLimits() { void GodotSliderJoint3D::testAngLimits() {
m_angDepth = real_t(0.); m_angDepth = real_t(0.);
m_solveAngLim = false; m_solveAngLim = false;
if (m_lowerAngLimit <= m_upperAngLimit) { if (m_lowerAngLimit <= m_upperAngLimit) {
@ -345,26 +343,26 @@ void SliderJoint3DSW::testAngLimits() {
m_solveAngLim = true; m_solveAngLim = true;
} }
} }
} // SliderJointSW::testAngLimits() }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
Vector3 SliderJoint3DSW::getAncorInA() { Vector3 GodotSliderJoint3D::getAncorInA() {
Vector3 ancorInA; Vector3 ancorInA;
ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis; ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis;
ancorInA = A->get_transform().inverse().xform(ancorInA); ancorInA = A->get_transform().inverse().xform(ancorInA);
return ancorInA; return ancorInA;
} // SliderJointSW::getAncorInA() }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
Vector3 SliderJoint3DSW::getAncorInB() { Vector3 GodotSliderJoint3D::getAncorInB() {
Vector3 ancorInB; Vector3 ancorInB;
ancorInB = m_frameInB.origin; ancorInB = m_frameInB.origin;
return ancorInB; return ancorInB;
} // SliderJointSW::getAncorInB(); }
void SliderJoint3DSW::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) { void GodotSliderJoint3D::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
m_upperLinLimit = p_value; m_upperLinLimit = p_value;
@ -439,7 +437,7 @@ void SliderJoint3DSW::set_param(PhysicsServer3D::SliderJointParam p_param, real_
} }
} }
real_t SliderJoint3DSW::get_param(PhysicsServer3D::SliderJointParam p_param) const { real_t GodotSliderJoint3D::get_param(PhysicsServer3D::SliderJointParam p_param) const {
switch (p_param) { switch (p_param) {
case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER: case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
return m_upperLinLimit; return m_upperLinLimit;

30
servers/physics_3d/joints/slider_joint_3d_sw.h → servers/physics_3d/joints/godot_slider_joint_3d.h
View file

@ -1,5 +1,5 @@
/*************************************************************************/ /*************************************************************************/
/* slider_joint_3d_sw.h */ /* godot_slider_joint_3d.h */
/*************************************************************************/ /*************************************************************************/
/* This file is part of: */ /* This file is part of: */
/* GODOT ENGINE */ /* GODOT ENGINE */
@ -32,11 +32,11 @@
Adapted to Godot from the Bullet library. Adapted to Godot from the Bullet library.
*/ */
#ifndef SLIDER_JOINT_SW_H #ifndef GODOT_SLIDER_JOINT_3D_H
#define SLIDER_JOINT_SW_H #define GODOT_SLIDER_JOINT_3D_H
#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h" #include "servers/physics_3d/godot_joint_3d.h"
#include "servers/physics_3d/joints_3d_sw.h" #include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
@ -65,15 +65,15 @@ April 04, 2008
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
class SliderJoint3DSW : public Joint3DSW { class GodotSliderJoint3D : public GodotJoint3D {
protected: protected:
union { union {
struct { struct {
Body3DSW *A; GodotBody3D *A;
Body3DSW *B; GodotBody3D *B;
}; };
Body3DSW *_arr[2] = { nullptr, nullptr }; GodotBody3D *_arr[2] = { nullptr, nullptr };
}; };
Transform3D m_frameInA; Transform3D m_frameInA;
@ -114,10 +114,10 @@ protected:
bool m_solveLinLim = false; bool m_solveLinLim = false;
bool m_solveAngLim = false; bool m_solveAngLim = false;
JacobianEntry3DSW m_jacLin[3] = {}; GodotJacobianEntry3D m_jacLin[3] = {};
real_t m_jacLinDiagABInv[3] = {}; real_t m_jacLinDiagABInv[3] = {};
JacobianEntry3DSW m_jacAng[3] = {}; GodotJacobianEntry3D m_jacAng[3] = {};
real_t m_timeStep = 0.0; real_t m_timeStep = 0.0;
Transform3D m_calculatedTransformA; Transform3D m_calculatedTransformA;
@ -149,13 +149,13 @@ protected:
public: public:
// constructors // constructors
SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB); GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB);
//SliderJointSW(); //SliderJointSW();
// overrides // overrides
// access // access
const Body3DSW *getRigidBodyA() const { return A; } const GodotBody3D *getRigidBodyA() const { return A; }
const Body3DSW *getRigidBodyB() const { return B; } const GodotBody3D *getRigidBodyB() const { return B; }
const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; } const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; }
const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; } const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; }
const Transform3D &getFrameOffsetA() const { return m_frameInA; } const Transform3D &getFrameOffsetA() const { return m_frameInA; }
@ -243,4 +243,4 @@ public:
virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; } virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; }
}; };
#endif // SLIDER_JOINT_SW_H #endif // GODOT_SLIDER_JOINT_3D_H

1748
servers/physics_3d/physics_server_3d_sw.cpp
View file

File diff suppressed because it is too large Load diff

6
servers/physics_server_2d.h
View file

@ -28,8 +28,8 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef PHYSICS_2D_SERVER_H #ifndef PHYSICS_SERVER_2D_H
#define PHYSICS_2D_SERVER_H #define PHYSICS_SERVER_2D_H
#include "core/io/resource.h" #include "core/io/resource.h"
#include "core/object/class_db.h" #include "core/object/class_db.h"
@ -701,4 +701,4 @@ VARIANT_ENUM_CAST(PhysicsServer2D::DampedSpringParam);
VARIANT_ENUM_CAST(PhysicsServer2D::AreaBodyStatus); VARIANT_ENUM_CAST(PhysicsServer2D::AreaBodyStatus);
VARIANT_ENUM_CAST(PhysicsServer2D::ProcessInfo); VARIANT_ENUM_CAST(PhysicsServer2D::ProcessInfo);
#endif #endif // PHYSICS_SERVER_2D_H

0
servers/physics_2d/physics_server_2d_wrap_mt.cpp → servers/physics_server_2d_wrap_mt.cpp
View file

6
servers/physics_2d/physics_server_2d_wrap_mt.h → servers/physics_server_2d_wrap_mt.h
View file

@ -28,8 +28,8 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef PHYSICS2DSERVERWRAPMT_H #ifndef PHYSICS_SERVER_2D_WRAP_MT_H
#define PHYSICS2DSERVERWRAPMT_H #define PHYSICS_SERVER_2D_WRAP_MT_H
#include "core/config/project_settings.h" #include "core/config/project_settings.h"
#include "core/os/thread.h" #include "core/os/thread.h"
@ -330,4 +330,4 @@ public:
#endif #endif
#undef SYNC_DEBUG #undef SYNC_DEBUG
#endif // PHYSICS2DSERVERWRAPMT_H #endif // PHYSICS_SERVER_2D_WRAP_MT_H

6
servers/physics_server_3d.h
View file

@ -28,8 +28,8 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef PHYSICS_SERVER_H #ifndef PHYSICS_SERVER_3D_H
#define PHYSICS_SERVER_H #define PHYSICS_SERVER_3D_H
#include "core/io/resource.h" #include "core/io/resource.h"
#include "core/object/class_db.h" #include "core/object/class_db.h"
@ -903,4 +903,4 @@ VARIANT_ENUM_CAST(PhysicsServer3D::G6DOFJointAxisFlag);
VARIANT_ENUM_CAST(PhysicsServer3D::AreaBodyStatus); VARIANT_ENUM_CAST(PhysicsServer3D::AreaBodyStatus);
VARIANT_ENUM_CAST(PhysicsServer3D::ProcessInfo); VARIANT_ENUM_CAST(PhysicsServer3D::ProcessInfo);
#endif #endif // PHYSICS_SERVER_3D_H

0
servers/physics_3d/physics_server_3d_wrap_mt.cpp → servers/physics_server_3d_wrap_mt.cpp
View file

6
servers/physics_3d/physics_server_3d_wrap_mt.h → servers/physics_server_3d_wrap_mt.h
View file

@ -28,8 +28,8 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef PHYSICS3DSERVERWRAPMT_H #ifndef PHYSICS_SERVER_3D_WRAP_MT_H
#define PHYSICS3DSERVERWRAPMT_H #define PHYSICS_SERVER_3D_WRAP_MT_H
#include "core/config/project_settings.h" #include "core/config/project_settings.h"
#include "core/os/thread.h" #include "core/os/thread.h"
@ -406,4 +406,4 @@ public:
#endif #endif
#undef SYNC_DEBUG #undef SYNC_DEBUG
#endif // PHYSICS3DSERVERWRAPMT_H #endif // PHYSICS_SERVER_3D_WRAP_MT_H

12
servers/register_server_types.cpp
View file

@ -59,12 +59,12 @@
#include "display_server.h" #include "display_server.h"
#include "navigation_server_2d.h" #include "navigation_server_2d.h"
#include "navigation_server_3d.h" #include "navigation_server_3d.h"
#include "physics_2d/physics_server_2d_sw.h" #include "physics_2d/godot_physics_server_2d.h"
#include "physics_2d/physics_server_2d_wrap_mt.h" #include "physics_3d/godot_physics_server_3d.h"
#include "physics_3d/physics_server_3d_sw.h"
#include "physics_3d/physics_server_3d_wrap_mt.h"
#include "physics_server_2d.h" #include "physics_server_2d.h"
#include "physics_server_2d_wrap_mt.h"
#include "physics_server_3d.h" #include "physics_server_3d.h"
#include "physics_server_3d_wrap_mt.h"
#include "rendering/renderer_compositor.h" #include "rendering/renderer_compositor.h"
#include "rendering/rendering_device.h" #include "rendering/rendering_device.h"
#include "rendering/rendering_device_binds.h" #include "rendering/rendering_device_binds.h"
@ -82,7 +82,7 @@ ShaderTypes *shader_types = nullptr;
PhysicsServer3D *_createGodotPhysics3DCallback() { PhysicsServer3D *_createGodotPhysics3DCallback() {
bool using_threads = GLOBAL_GET("physics/3d/run_on_thread"); bool using_threads = GLOBAL_GET("physics/3d/run_on_thread");
PhysicsServer3D *physics_server = memnew(PhysicsServer3DSW(using_threads)); PhysicsServer3D *physics_server = memnew(GodotPhysicsServer3D(using_threads));
return memnew(PhysicsServer3DWrapMT(physics_server, using_threads)); return memnew(PhysicsServer3DWrapMT(physics_server, using_threads));
} }
@ -90,7 +90,7 @@ PhysicsServer3D *_createGodotPhysics3DCallback() {
PhysicsServer2D *_createGodotPhysics2DCallback() { PhysicsServer2D *_createGodotPhysics2DCallback() {
bool using_threads = GLOBAL_GET("physics/2d/run_on_thread"); bool using_threads = GLOBAL_GET("physics/2d/run_on_thread");
PhysicsServer2D *physics_server = memnew(PhysicsServer2DSW(using_threads)); PhysicsServer2D *physics_server = memnew(GodotPhysicsServer2D(using_threads));
return memnew(PhysicsServer2DWrapMT(physics_server, using_threads)); return memnew(PhysicsServer2DWrapMT(physics_server, using_threads));
} }