maxmustermann/godot
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Clean physics direct body state usage in 2D and 3D physics

Browse source 
Use a C++ callback instead of Callable for synchronizing physics nodes' state with physics servers.

Remove usage of PhysicsDirectBodyState in physics nodes when not
necessary.

Store PhysicsDirectBodyState for bodies individually instead of a
singleton to avoid issues when accessing direct body state for multiple
bodies.

PhysicsDirectBodyState is initialized only when needed, so it doesn't
have to be created when using the physics server directly.

Move PhysicsDirectBodyState2D and PhysicsDirectBodyState3D to separate
cpp files.
This commit is contained in:
PouleyKetchoupp 2021-06-04 16:09:41 -07:00
parent 353bb45e21
commit 6a9ed72185
29 changed files with 818 additions and 436 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
doc/classes/PhysicsServer2D.xml
View file

@ -489,6 +489,9 @@
<argument index="2" name="userdata" type="Variant" default="null" />
<description>
Sets the function used to calculate physics for an object, if that object allows it (see [method body_set_omit_force_integration]).
The force integration function takes 2 arguments:
[code]state:[/code] [PhysicsDirectBodyState2D] used to retrieve and modify the body's state.
[code]userdata:[/code] Optional user data, if it was passed when calling [code]body_set_force_integration_callback[/code].
</description>
</method>
<method name="body_set_max_contacts_reported">

3
doc/classes/PhysicsServer3D.xml
View file

@ -478,6 +478,9 @@
<argument index="2" name="userdata" type="Variant" default="null" />
<description>
Sets the function used to calculate physics for an object, if that object allows it (see [method body_set_omit_force_integration]).
The force integration function takes 2 arguments:
[code]state:[/code] [PhysicsDirectBodyState3D] used to retrieve and modify the body's state.
[code]userdata:[/code] Optional user data, if it was passed when calling [code]body_set_force_integration_callback[/code].
</description>
</method>
<method name="body_set_max_contacts_reported">

86
scene/2d/physics_body_2d.cpp
View file

@ -259,15 +259,17 @@ bool StaticBody2D::is_sync_to_physics_enabled() const {
return sync_to_physics;
}
void StaticBody2D::_direct_state_changed(Object *p_state) {
void StaticBody2D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state) {
StaticBody2D *body = (StaticBody2D *)p_instance;
body->_body_state_changed(p_state);
}
void StaticBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state) {
if (!sync_to_physics) {
return;
}
PhysicsDirectBodyState2D *state = Object::cast_to<PhysicsDirectBodyState2D>(p_state);
ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState2D object as argument");
last_valid_transform = state->get_transform();
last_valid_transform = p_state->get_transform();
set_notify_local_transform(false);
set_global_transform(last_valid_transform);
set_notify_local_transform(true);
@ -379,11 +381,11 @@ void StaticBody2D::_update_kinematic_motion() {
#endif
if (kinematic_motion && sync_to_physics) {
PhysicsServer2D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &StaticBody2D::_direct_state_changed));
PhysicsServer2D::get_singleton()->body_set_state_sync_callback(get_rid(), this, _body_state_changed_callback);
set_only_update_transform_changes(true);
set_notify_local_transform(true);
} else {
PhysicsServer2D::get_singleton()->body_set_force_integration_callback(get_rid(), Callable());
PhysicsServer2D::get_singleton()->body_set_state_sync_callback(get_rid(), nullptr, nullptr);
set_only_update_transform_changes(false);
set_notify_local_transform(false);
}
@ -511,26 +513,26 @@ struct _RigidBody2DInOut {
int local_shape = 0;
};
void RigidBody2D::_direct_state_changed(Object *p_state) {
#ifdef DEBUG_ENABLED
state = Object::cast_to<PhysicsDirectBodyState2D>(p_state);
ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState2D object as argument");
#else
state = (PhysicsDirectBodyState2D *)p_state; //trust it
#endif
void RigidBody2D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state) {
RigidBody2D *body = (RigidBody2D *)p_instance;
body->_body_state_changed(p_state);
}
void RigidBody2D::_body_state_changed(PhysicsDirectBodyState2D *p_state) {
set_block_transform_notify(true); // don't want notify (would feedback loop)
if (mode != MODE_KINEMATIC) {
set_global_transform(state->get_transform());
set_global_transform(p_state->get_transform());
}
linear_velocity = state->get_linear_velocity();
angular_velocity = state->get_angular_velocity();
if (sleeping != state->is_sleeping()) {
sleeping = state->is_sleeping();
linear_velocity = p_state->get_linear_velocity();
angular_velocity = p_state->get_angular_velocity();
if (sleeping != p_state->is_sleeping()) {
sleeping = p_state->is_sleeping();
emit_signal(SceneStringNames::get_singleton()->sleeping_state_changed);
}
GDVIRTUAL_CALL(_integrate_forces, state);
GDVIRTUAL_CALL(_integrate_forces, p_state);
set_block_transform_notify(false); // want it back
@ -546,20 +548,18 @@ void RigidBody2D::_direct_state_changed(Object *p_state) {
}
}
_RigidBody2DInOut *toadd = (_RigidBody2DInOut *)alloca(state->get_contact_count() * sizeof(_RigidBody2DInOut));
_RigidBody2DInOut *toadd = (_RigidBody2DInOut *)alloca(p_state->get_contact_count() * sizeof(_RigidBody2DInOut));
int toadd_count = 0; //state->get_contact_count();
RigidBody2D_RemoveAction *toremove = (RigidBody2D_RemoveAction *)alloca(rc * sizeof(RigidBody2D_RemoveAction));
int toremove_count = 0;
//put the ones to add
for (int i = 0; i < state->get_contact_count(); i++) {
RID rid = state->get_contact_collider(i);
ObjectID obj = state->get_contact_collider_id(i);
int local_shape = state->get_contact_local_shape(i);
int shape = state->get_contact_collider_shape(i);
//bool found=false;
for (int i = 0; i < p_state->get_contact_count(); i++) {
RID rid = p_state->get_contact_collider(i);
ObjectID obj = p_state->get_contact_collider_id(i);
int local_shape = p_state->get_contact_local_shape(i);
int shape = p_state->get_contact_collider_shape(i);
Map<ObjectID, BodyState>::Element *E = contact_monitor->body_map.find(obj);
if (!E) {
@ -612,8 +612,6 @@ void RigidBody2D::_direct_state_changed(Object *p_state) {
contact_monitor->locked = false;
}
state = nullptr;
}
void RigidBody2D::set_mode(Mode p_mode) {
@ -709,25 +707,15 @@ real_t RigidBody2D::get_angular_damp() const {
}
void RigidBody2D::set_axis_velocity(const Vector2 &p_axis) {
Vector2 v = state ? state->get_linear_velocity() : linear_velocity;
Vector2 axis = p_axis.normalized();
v -= axis * axis.dot(v);
v += p_axis;
if (state) {
set_linear_velocity(v);
} else {
PhysicsServer2D::get_singleton()->body_set_axis_velocity(get_rid(), p_axis);
linear_velocity = v;
}
linear_velocity -= axis * axis.dot(linear_velocity);
linear_velocity += p_axis;
PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
}
void RigidBody2D::set_linear_velocity(const Vector2 &p_velocity) {
linear_velocity = p_velocity;
if (state) {
state->set_linear_velocity(linear_velocity);
} else {
PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
}
PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
}
Vector2 RigidBody2D::get_linear_velocity() const {
@ -736,11 +724,7 @@ Vector2 RigidBody2D::get_linear_velocity() const {
void RigidBody2D::set_angular_velocity(real_t p_velocity) {
angular_velocity = p_velocity;
if (state) {
state->set_angular_velocity(angular_velocity);
} else {
PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity);
}
PhysicsServer2D::get_singleton()->body_set_state(get_rid(), PhysicsServer2D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity);
}
real_t RigidBody2D::get_angular_velocity() const {
@ -1019,7 +1003,7 @@ void RigidBody2D::_bind_methods() {
RigidBody2D::RigidBody2D() :
PhysicsBody2D(PhysicsServer2D::BODY_MODE_DYNAMIC) {
PhysicsServer2D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &RigidBody2D::_direct_state_changed));
PhysicsServer2D::get_singleton()->body_set_state_sync_callback(get_rid(), this, _body_state_changed_callback);
}
RigidBody2D::~RigidBody2D() {
@ -1052,7 +1036,7 @@ bool CharacterBody2D::move_and_slide() {
if ((on_floor || on_wall) && platform_rid.is_valid()) {
bool excluded = (moving_platform_ignore_layers & platform_layer) != 0;
if (!excluded) {
// This approach makes sure there is less delay between the actual body velocity and the one we saved.
//this approach makes sure there is less delay between the actual body velocity and the one we saved
PhysicsDirectBodyState2D *bs = PhysicsServer2D::get_singleton()->body_get_direct_state(platform_rid);
if (bs) {
Transform2D gt = get_global_transform();

8
scene/2d/physics_body_2d.h
View file

@ -73,7 +73,8 @@ class StaticBody2D : public PhysicsBody2D {
Transform2D last_valid_transform;
void _direct_state_changed(Object *p_state);
static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state);
void _body_state_changed(PhysicsDirectBodyState2D *p_state);
protected:
void _notification(int p_what);
@ -124,7 +125,6 @@ public:
private:
bool can_sleep = true;
PhysicsDirectBodyState2D *state = nullptr;
Mode mode = MODE_DYNAMIC;
real_t mass = 1.0;
@ -183,7 +183,9 @@ private:
void _body_exit_tree(ObjectID p_id);
void _body_inout(int p_status, const RID &p_body, ObjectID p_instance, int p_body_shape, int p_local_shape);
void _direct_state_changed(Object *p_state);
static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState2D *p_state);
void _body_state_changed(PhysicsDirectBodyState2D *p_state);
protected:
void _notification(int p_what);

111
scene/3d/physics_body_3d.cpp
View file

@ -283,18 +283,20 @@ bool StaticBody3D::is_sync_to_physics_enabled() const {
return sync_to_physics;
}
void StaticBody3D::_direct_state_changed(Object *p_state) {
PhysicsDirectBodyState3D *state = Object::cast_to<PhysicsDirectBodyState3D>(p_state);
ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument");
void StaticBody3D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state) {
StaticBody3D *body = (StaticBody3D *)p_instance;
body->_body_state_changed(p_state);
}
linear_velocity = state->get_linear_velocity();
angular_velocity = state->get_angular_velocity();
void StaticBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
linear_velocity = p_state->get_linear_velocity();
angular_velocity = p_state->get_angular_velocity();
if (!sync_to_physics) {
return;
}
last_valid_transform = state->get_transform();
last_valid_transform = p_state->get_transform();
set_notify_local_transform(false);
set_global_transform(last_valid_transform);
set_notify_local_transform(true);
@ -458,13 +460,13 @@ void StaticBody3D::_update_kinematic_motion() {
bool needs_physics_process = false;
if (kinematic_motion) {
PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &StaticBody3D::_direct_state_changed));
PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, &StaticBody3D::_body_state_changed_callback);
if (!constant_angular_velocity.is_equal_approx(Vector3()) || !constant_linear_velocity.is_equal_approx(Vector3())) {
needs_physics_process = true;
}
} else {
PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), Callable());
PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), nullptr, nullptr);
}
set_physics_process_internal(needs_physics_process);
@ -582,25 +584,26 @@ struct _RigidBodyInOut {
int local_shape = 0;
};
void RigidBody3D::_direct_state_changed(Object *p_state) {
#ifdef DEBUG_ENABLED
state = Object::cast_to<PhysicsDirectBodyState3D>(p_state);
ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument");
#else
state = (PhysicsDirectBodyState3D *)p_state; //trust it
#endif
void RigidBody3D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state) {
RigidBody3D *body = (RigidBody3D *)p_instance;
body->_body_state_changed(p_state);
}
void RigidBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
set_ignore_transform_notification(true);
set_global_transform(state->get_transform());
linear_velocity = state->get_linear_velocity();
angular_velocity = state->get_angular_velocity();
inverse_inertia_tensor = state->get_inverse_inertia_tensor();
if (sleeping != state->is_sleeping()) {
sleeping = state->is_sleeping();
set_global_transform(p_state->get_transform());
linear_velocity = p_state->get_linear_velocity();
angular_velocity = p_state->get_angular_velocity();
inverse_inertia_tensor = p_state->get_inverse_inertia_tensor();
if (sleeping != p_state->is_sleeping()) {
sleeping = p_state->is_sleeping();
emit_signal(SceneStringNames::get_singleton()->sleeping_state_changed);
}
GDVIRTUAL_CALL(_integrate_forces, state);
GDVIRTUAL_CALL(_integrate_forces, p_state);
set_ignore_transform_notification(false);
_on_transform_changed();
@ -617,18 +620,18 @@ void RigidBody3D::_direct_state_changed(Object *p_state) {
}
}
_RigidBodyInOut *toadd = (_RigidBodyInOut *)alloca(state->get_contact_count() * sizeof(_RigidBodyInOut));
_RigidBodyInOut *toadd = (_RigidBodyInOut *)alloca(p_state->get_contact_count() * sizeof(_RigidBodyInOut));
int toadd_count = 0; //state->get_contact_count();
RigidBody3D_RemoveAction *toremove = (RigidBody3D_RemoveAction *)alloca(rc * sizeof(RigidBody3D_RemoveAction));
int toremove_count = 0;
//put the ones to add
for (int i = 0; i < state->get_contact_count(); i++) {
RID rid = state->get_contact_collider(i);
ObjectID obj = state->get_contact_collider_id(i);
int local_shape = state->get_contact_local_shape(i);
int shape = state->get_contact_collider_shape(i);
for (int i = 0; i < p_state->get_contact_count(); i++) {
RID rid = p_state->get_contact_collider(i);
ObjectID obj = p_state->get_contact_collider_id(i);
int local_shape = p_state->get_contact_local_shape(i);
int shape = p_state->get_contact_collider_shape(i);
//bool found=false;
@ -683,8 +686,6 @@ void RigidBody3D::_direct_state_changed(Object *p_state) {
contact_monitor->locked = false;
}
state = nullptr;
}
void RigidBody3D::_notification(int p_what) {
@ -787,25 +788,15 @@ real_t RigidBody3D::get_angular_damp() const {
}
void RigidBody3D::set_axis_velocity(const Vector3 &p_axis) {
Vector3 v = state ? state->get_linear_velocity() : linear_velocity;
Vector3 axis = p_axis.normalized();
v -= axis * axis.dot(v);
v += p_axis;
if (state) {
set_linear_velocity(v);
} else {
PhysicsServer3D::get_singleton()->body_set_axis_velocity(get_rid(), p_axis);
linear_velocity = v;
}
linear_velocity -= axis * axis.dot(linear_velocity);
linear_velocity += p_axis;
PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
}
void RigidBody3D::set_linear_velocity(const Vector3 &p_velocity) {
linear_velocity = p_velocity;
if (state) {
state->set_linear_velocity(linear_velocity);
} else {
PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
}
PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_LINEAR_VELOCITY, linear_velocity);
}
Vector3 RigidBody3D::get_linear_velocity() const {
@ -814,11 +805,7 @@ Vector3 RigidBody3D::get_linear_velocity() const {
void RigidBody3D::set_angular_velocity(const Vector3 &p_velocity) {
angular_velocity = p_velocity;
if (state) {
state->set_angular_velocity(angular_velocity);
} else {
PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity);
}
PhysicsServer3D::get_singleton()->body_set_state(get_rid(), PhysicsServer3D::BODY_STATE_ANGULAR_VELOCITY, angular_velocity);
}
Vector3 RigidBody3D::get_angular_velocity() const {
@ -1055,7 +1042,7 @@ void RigidBody3D::_bind_methods() {
RigidBody3D::RigidBody3D() :
PhysicsBody3D(PhysicsServer3D::BODY_MODE_DYNAMIC) {
PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &RigidBody3D::_direct_state_changed));
PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, &RigidBody3D::_body_state_changed_callback);
}
RigidBody3D::~RigidBody3D() {
@ -2249,23 +2236,19 @@ void PhysicalBone3D::_notification(int p_what) {
}
}
void PhysicalBone3D::_direct_state_changed(Object *p_state) {
void PhysicalBone3D::_body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state) {
PhysicalBone3D *bone = (PhysicalBone3D *)p_instance;
bone->_body_state_changed(p_state);
}
void PhysicalBone3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
if (!simulate_physics || !_internal_simulate_physics) {
return;
}
/// Update bone transform
/// Update bone transform.
PhysicsDirectBodyState3D *state;
#ifdef DEBUG_ENABLED
state = Object::cast_to<PhysicsDirectBodyState3D>(p_state);
ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument");
#else
state = (PhysicsDirectBodyState3D *)p_state; //trust it
#endif
Transform3D global_transform(state->get_transform());
Transform3D global_transform(p_state->get_transform());
set_ignore_transform_notification(true);
set_global_transform(global_transform);
@ -2727,7 +2710,7 @@ void PhysicalBone3D::_start_physics_simulation() {
set_body_mode(PhysicsServer3D::BODY_MODE_DYNAMIC);
PhysicsServer3D::get_singleton()->body_set_collision_layer(get_rid(), get_collision_layer());
PhysicsServer3D::get_singleton()->body_set_collision_mask(get_rid(), get_collision_mask());
PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &PhysicalBone3D::_direct_state_changed));
PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), this, &PhysicalBone3D::_body_state_changed_callback);
set_as_top_level(true);
_internal_simulate_physics = true;
}
@ -2746,7 +2729,7 @@ void PhysicalBone3D::_stop_physics_simulation() {
PhysicsServer3D::get_singleton()->body_set_collision_mask(get_rid(), 0);
}
if (_internal_simulate_physics) {
PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), Callable());
PhysicsServer3D::get_singleton()->body_set_state_sync_callback(get_rid(), nullptr, nullptr);
parent_skeleton->set_bone_global_pose_override(bone_id, Transform3D(), 0.0, false);
set_as_top_level(false);
_internal_simulate_physics = false;

10
scene/3d/physics_body_3d.h
View file

@ -86,7 +86,8 @@ class StaticBody3D : public PhysicsBody3D {
Transform3D last_valid_transform;
void _direct_state_changed(Object *p_state);
static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state);
void _body_state_changed(PhysicsDirectBodyState3D *p_state);
protected:
void _notification(int p_what);
@ -136,7 +137,6 @@ public:
protected:
bool can_sleep = true;
PhysicsDirectBodyState3D *state = nullptr;
Mode mode = MODE_DYNAMIC;
real_t mass = 1.0;
@ -197,7 +197,8 @@ protected:
void _body_exit_tree(ObjectID p_id);
void _body_inout(int p_status, const RID &p_body, ObjectID p_instance, int p_body_shape, int p_local_shape);
virtual void _direct_state_changed(Object *p_state);
static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state);
virtual void _body_state_changed(PhysicsDirectBodyState3D *p_state);
void _notification(int p_what);
static void _bind_methods();
@ -525,7 +526,8 @@ protected:
bool _get(const StringName &p_name, Variant &r_ret) const;
void _get_property_list(List<PropertyInfo> *p_list) const;
void _notification(int p_what);
void _direct_state_changed(Object *p_state);
static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state);
void _body_state_changed(PhysicsDirectBodyState3D *p_state);
static void _bind_methods();

33
scene/3d/vehicle_body_3d.cpp
View file

@ -802,24 +802,21 @@ void VehicleBody3D::_update_friction(PhysicsDirectBodyState3D *s) {
}
}
void VehicleBody3D::_direct_state_changed(Object *p_state) {
RigidBody3D::_direct_state_changed(p_state);
void VehicleBody3D::_body_state_changed(PhysicsDirectBodyState3D *p_state) {
RigidBody3D::_body_state_changed(p_state);
state = Object::cast_to<PhysicsDirectBodyState3D>(p_state);
ERR_FAIL_NULL_MSG(state, "Method '_direct_state_changed' must receive a valid PhysicsDirectBodyState3D object as argument");
real_t step = state->get_step();
real_t step = p_state->get_step();
for (int i = 0; i < wheels.size(); i++) {
_update_wheel(i, state);
_update_wheel(i, p_state);
}
for (int i = 0; i < wheels.size(); i++) {
_ray_cast(i, state);
wheels[i]->set_transform(state->get_transform().inverse() * wheels[i]->m_worldTransform);
_ray_cast(i, p_state);
wheels[i]->set_transform(p_state->get_transform().inverse() * wheels[i]->m_worldTransform);
}
_update_suspension(state);
_update_suspension(p_state);
for (int i = 0; i < wheels.size(); i++) {
//apply suspension force
@ -831,20 +828,20 @@ void VehicleBody3D::_direct_state_changed(Object *p_state) {
suspensionForce = wheel.m_maxSuspensionForce;
}
Vector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
Vector3 relative_position = wheel.m_raycastInfo.m_contactPointWS - state->get_transform().origin;
Vector3 relative_position = wheel.m_raycastInfo.m_contactPointWS - p_state->get_transform().origin;
state->apply_impulse(impulse, relative_position);
p_state->apply_impulse(impulse, relative_position);
}
_update_friction(state);
_update_friction(p_state);
for (int i = 0; i < wheels.size(); i++) {
VehicleWheel3D &wheel = *wheels[i];
Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - state->get_transform().origin;
Vector3 vel = state->get_linear_velocity() + (state->get_angular_velocity()).cross(relpos); // * mPos);
Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - p_state->get_transform().origin;
Vector3 vel = p_state->get_linear_velocity() + (p_state->get_angular_velocity()).cross(relpos); // * mPos);
if (wheel.m_raycastInfo.m_isInContact) {
const Transform3D &chassisWorldTransform = state->get_transform();
const Transform3D &chassisWorldTransform = p_state->get_transform();
Vector3 fwd(
chassisWorldTransform.basis[0][Vector3::AXIS_Z],
@ -864,8 +861,6 @@ void VehicleBody3D::_direct_state_changed(Object *p_state) {
wheel.m_deltaRotation *= real_t(0.99); //damping of rotation when not in contact
}
state = nullptr;
}
void VehicleBody3D::set_engine_force(real_t p_engine_force) {
@ -926,7 +921,5 @@ void VehicleBody3D::_bind_methods() {
VehicleBody3D::VehicleBody3D() {
exclude.insert(get_rid());
//PhysicsServer3D::get_singleton()->body_set_force_integration_callback(get_rid(), callable_mp(this, &VehicleBody3D::_direct_state_changed));
set_mass(40);
}

3
scene/3d/vehicle_body_3d.h
View file

@ -192,7 +192,8 @@ class VehicleBody3D : public RigidBody3D {
static void _bind_methods();
void _direct_state_changed(Object *p_state) override;
static void _body_state_changed_callback(void *p_instance, PhysicsDirectBodyState3D *p_state);
virtual void _body_state_changed(PhysicsDirectBodyState3D *p_state) override;
public:
void set_engine_force(real_t p_engine_force);

88
servers/physics_2d/body_2d_sw.cpp
View file

@ -29,8 +29,9 @@
/*************************************************************************/
#include "body_2d_sw.h"
#include "area_2d_sw.h"
#include "physics_server_2d_sw.h"
#include "body_direct_state_2d_sw.h"
#include "space_2d_sw.h"
void Body2DSW::_update_inertia() {
@ -502,7 +503,7 @@ void Body2DSW::integrate_velocities(real_t p_step) {
return;
}
if (fi_callback) {
if (fi_callback_data || body_state_callback) {
get_space()->body_add_to_state_query_list(&direct_state_query_list);
}
@ -554,27 +555,27 @@ void Body2DSW::wakeup_neighbours() {
}
void Body2DSW::call_queries() {
if (fi_callback) {
PhysicsDirectBodyState2DSW *dbs = PhysicsDirectBodyState2DSW::singleton;
dbs->body = this;
Variant v = dbs;
const Variant *vp[2] = { &v, &fi_callback->callback_udata };
Object *obj = fi_callback->callable.get_object();
if (!obj) {
if (fi_callback_data) {
if (!fi_callback_data->callable.get_object()) {
set_force_integration_callback(Callable());
} else {
Variant direct_state_variant = get_direct_state();
const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata };
Callable::CallError ce;
Variant rv;
if (fi_callback->callback_udata.get_type() != Variant::NIL) {
fi_callback->callable.call(vp, 2, rv, ce);
if (fi_callback_data->udata.get_type() != Variant::NIL) {
fi_callback_data->callable.call(vp, 2, rv, ce);
} else {
fi_callback->callable.call(vp, 1, rv, ce);
fi_callback_data->callable.call(vp, 1, rv, ce);
}
}
}
if (body_state_callback) {
(body_state_callback)(body_state_callback_instance, get_direct_state());
}
}
bool Body2DSW::sleep_test(real_t p_step) {
@ -594,19 +595,32 @@ bool Body2DSW::sleep_test(real_t p_step) {
}
}
void Body2DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
if (fi_callback) {
memdelete(fi_callback);
fi_callback = nullptr;
}
void Body2DSW::set_state_sync_callback(void *p_instance, PhysicsServer2D::BodyStateCallback p_callback) {
body_state_callback_instance = p_instance;
body_state_callback = p_callback;
}
void Body2DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
if (p_callable.get_object()) {
fi_callback = memnew(ForceIntegrationCallback);
fi_callback->callable = p_callable;
fi_callback->callback_udata = p_udata;
if (!fi_callback_data) {
fi_callback_data = memnew(ForceIntegrationCallbackData);
}
fi_callback_data->callable = p_callable;
fi_callback_data->udata = p_udata;
} else if (fi_callback_data) {
memdelete(fi_callback_data);
fi_callback_data = nullptr;
}
}
PhysicsDirectBodyState2DSW *Body2DSW::get_direct_state() {
if (!direct_state) {
direct_state = memnew(PhysicsDirectBodyState2DSW);
direct_state->body = this;
}
return direct_state;
}
Body2DSW::Body2DSW() :
CollisionObject2DSW(TYPE_BODY),
active_list(this),
@ -639,33 +653,13 @@ Body2DSW::Body2DSW() :
still_time = 0;
continuous_cd_mode = PhysicsServer2D::CCD_MODE_DISABLED;
can_sleep = true;
fi_callback = nullptr;
}
Body2DSW::~Body2DSW() {
if (fi_callback) {
memdelete(fi_callback);
if (fi_callback_data) {
memdelete(fi_callback_data);
}
if (direct_state) {
memdelete(direct_state);
}
}
PhysicsDirectBodyState2DSW *PhysicsDirectBodyState2DSW::singleton = nullptr;
PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() {
return body->get_space()->get_direct_state();
}
Variant PhysicsDirectBodyState2DSW::get_contact_collider_shape_metadata(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant());
if (!PhysicsServer2DSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) {
return Variant();
}
Body2DSW *other = PhysicsServer2DSW::singletonsw->body_owner.getornull(body->contacts[p_contact_idx].collider);
int sidx = body->contacts[p_contact_idx].collider_shape;
if (sidx < 0 || sidx >= other->get_shape_count()) {
return Variant();
}
return other->get_shape_metadata(sidx);
}

98
servers/physics_2d/body_2d_sw.h
View file

@ -38,6 +38,7 @@
#include "core/templates/vset.h"
class Constraint2DSW;
class PhysicsDirectBodyState2DSW;
class Body2DSW : public CollisionObject2DSW {
PhysicsServer2D::BodyMode mode;
@ -116,12 +117,17 @@ class Body2DSW : public CollisionObject2DSW {
Vector<Contact> contacts; //no contacts by default
int contact_count;
struct ForceIntegrationCallback {
void *body_state_callback_instance = nullptr;
PhysicsServer2D::BodyStateCallback body_state_callback = nullptr;
struct ForceIntegrationCallbackData {
Callable callable;
Variant callback_udata;
Variant udata;
};
ForceIntegrationCallback *fi_callback;
ForceIntegrationCallbackData *fi_callback_data = nullptr;
PhysicsDirectBodyState2DSW *direct_state = nullptr;
uint64_t island_step;
@ -130,8 +136,11 @@ class Body2DSW : public CollisionObject2DSW {
friend class PhysicsDirectBodyState2DSW; // i give up, too many functions to expose
public:
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());
PhysicsDirectBodyState2DSW *get_direct_state();
_FORCE_INLINE_ void add_area(Area2DSW *p_area) {
int index = areas.find(AreaCMP(p_area));
if (index > -1) {
@ -332,87 +341,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;
}
class PhysicsDirectBodyState2DSW : public PhysicsDirectBodyState2D {
GDCLASS(PhysicsDirectBodyState2DSW, PhysicsDirectBodyState2D);
public:
static PhysicsDirectBodyState2DSW *singleton;
Body2DSW *body;
real_t step;
virtual Vector2 get_total_gravity() const override { return body->gravity; } // get gravity vector working on this body space/area
virtual real_t get_total_angular_damp() const override { return body->area_angular_damp; } // get density of this body space/area
virtual real_t get_total_linear_damp() const override { return body->area_linear_damp; } // get density of this body space/area
virtual real_t get_inverse_mass() const override { return body->get_inv_mass(); } // get the mass
virtual real_t get_inverse_inertia() const override { return body->get_inv_inertia(); } // get density of this body space
virtual void set_linear_velocity(const Vector2 &p_velocity) override { body->set_linear_velocity(p_velocity); }
virtual Vector2 get_linear_velocity() const override { return body->get_linear_velocity(); }
virtual void set_angular_velocity(real_t p_velocity) override { body->set_angular_velocity(p_velocity); }
virtual real_t get_angular_velocity() const override { return body->get_angular_velocity(); }
virtual void set_transform(const Transform2D &p_transform) override { body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform); }
virtual Transform2D get_transform() const override { return body->get_transform(); }
virtual Vector2 get_velocity_at_local_position(const Vector2 &p_position) const override { return body->get_velocity_in_local_point(p_position); }
virtual void add_central_force(const Vector2 &p_force) override { body->add_central_force(p_force); }
virtual void add_force(const Vector2 &p_force, const Vector2 &p_position = Vector2()) override { body->add_force(p_force, p_position); }
virtual void add_torque(real_t p_torque) override { body->add_torque(p_torque); }
virtual void apply_central_impulse(const Vector2 &p_impulse) override { body->apply_central_impulse(p_impulse); }
virtual void apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position = Vector2()) override { body->apply_impulse(p_impulse, p_position); }
virtual void apply_torque_impulse(real_t p_torque) override { body->apply_torque_impulse(p_torque); }
virtual void set_sleep_state(bool p_enable) override { body->set_active(!p_enable); }
virtual bool is_sleeping() const override { return !body->is_active(); }
virtual int get_contact_count() const override { return body->contact_count; }
virtual Vector2 get_contact_local_position(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].local_pos;
}
virtual Vector2 get_contact_local_normal(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].local_normal;
}
virtual int get_contact_local_shape(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
return body->contacts[p_contact_idx].local_shape;
}
virtual RID get_contact_collider(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
return body->contacts[p_contact_idx].collider;
}
virtual Vector2 get_contact_collider_position(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].collider_pos;
}
virtual ObjectID get_contact_collider_id(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID());
return body->contacts[p_contact_idx].collider_instance_id;
}
virtual int get_contact_collider_shape(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
return body->contacts[p_contact_idx].collider_shape;
}
virtual Variant get_contact_collider_shape_metadata(int p_contact_idx) const override;
virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].collider_velocity_at_pos;
}
virtual PhysicsDirectSpaceState2D *get_space_state() override;
virtual real_t get_step() const override { return step; }
PhysicsDirectBodyState2DSW() {
singleton = this;
body = nullptr;
}
};
#endif // BODY_2D_SW_H

182
servers/physics_2d/body_direct_state_2d_sw.cpp Normal file
View file

@ -0,0 +1,182 @@
/*************************************************************************/
/* body_direct_state_2d_sw.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#include "body_direct_state_2d_sw.h"
#include "body_2d_sw.h"
#include "physics_server_2d_sw.h"
#include "space_2d_sw.h"
Vector2 PhysicsDirectBodyState2DSW::get_total_gravity() const {
return body->gravity;
}
real_t PhysicsDirectBodyState2DSW::get_total_angular_damp() const {
return body->area_angular_damp;
}
real_t PhysicsDirectBodyState2DSW::get_total_linear_damp() const {
return body->area_linear_damp;
}
real_t PhysicsDirectBodyState2DSW::get_inverse_mass() const {
return body->get_inv_mass();
}
real_t PhysicsDirectBodyState2DSW::get_inverse_inertia() const {
return body->get_inv_inertia();
}
void PhysicsDirectBodyState2DSW::set_linear_velocity(const Vector2 &p_velocity) {
body->set_linear_velocity(p_velocity);
}
Vector2 PhysicsDirectBodyState2DSW::get_linear_velocity() const {
return body->get_linear_velocity();
}
void PhysicsDirectBodyState2DSW::set_angular_velocity(real_t p_velocity) {
body->set_angular_velocity(p_velocity);
}
real_t PhysicsDirectBodyState2DSW::get_angular_velocity() const {
return body->get_angular_velocity();
}
void PhysicsDirectBodyState2DSW::set_transform(const Transform2D &p_transform) {
body->set_state(PhysicsServer2D::BODY_STATE_TRANSFORM, p_transform);
}
Transform2D PhysicsDirectBodyState2DSW::get_transform() const {
return body->get_transform();
}
Vector2 PhysicsDirectBodyState2DSW::get_velocity_at_local_position(const Vector2 &p_position) const {
return body->get_velocity_in_local_point(p_position);
}
void PhysicsDirectBodyState2DSW::add_central_force(const Vector2 &p_force) {
body->add_central_force(p_force);
}
void PhysicsDirectBodyState2DSW::add_force(const Vector2 &p_force, const Vector2 &p_position) {
body->add_force(p_force, p_position);
}
void PhysicsDirectBodyState2DSW::add_torque(real_t p_torque) {
body->add_torque(p_torque);
}
void PhysicsDirectBodyState2DSW::apply_central_impulse(const Vector2 &p_impulse) {
body->apply_central_impulse(p_impulse);
}
void PhysicsDirectBodyState2DSW::apply_impulse(const Vector2 &p_impulse, const Vector2 &p_position) {
body->apply_impulse(p_impulse, p_position);
}
void PhysicsDirectBodyState2DSW::apply_torque_impulse(real_t p_torque) {
body->apply_torque_impulse(p_torque);
}
void PhysicsDirectBodyState2DSW::set_sleep_state(bool p_enable) {
body->set_active(!p_enable);
}
bool PhysicsDirectBodyState2DSW::is_sleeping() const {
return !body->is_active();
}
int PhysicsDirectBodyState2DSW::get_contact_count() const {
return body->contact_count;
}
Vector2 PhysicsDirectBodyState2DSW::get_contact_local_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].local_pos;
}
Vector2 PhysicsDirectBodyState2DSW::get_contact_local_normal(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].local_normal;
}
int PhysicsDirectBodyState2DSW::get_contact_local_shape(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
return body->contacts[p_contact_idx].local_shape;
}
RID PhysicsDirectBodyState2DSW::get_contact_collider(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
return body->contacts[p_contact_idx].collider;
}
Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].collider_pos;
}
ObjectID PhysicsDirectBodyState2DSW::get_contact_collider_id(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID());
return body->contacts[p_contact_idx].collider_instance_id;
}
int PhysicsDirectBodyState2DSW::get_contact_collider_shape(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
return body->contacts[p_contact_idx].collider_shape;
}
Vector2 PhysicsDirectBodyState2DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].collider_velocity_at_pos;
}
Variant PhysicsDirectBodyState2DSW::get_contact_collider_shape_metadata(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Variant());
if (!PhysicsServer2DSW::singletonsw->body_owner.owns(body->contacts[p_contact_idx].collider)) {
return Variant();
}
Body2DSW *other = PhysicsServer2DSW::singletonsw->body_owner.getornull(body->contacts[p_contact_idx].collider);
int sidx = body->contacts[p_contact_idx].collider_shape;
if (sidx < 0 || sidx >= other->get_shape_count()) {
return Variant();
}
return other->get_shape_metadata(sidx);
}
PhysicsDirectSpaceState2D *PhysicsDirectBodyState2DSW::get_space_state() {
return body->get_space()->get_direct_state();
}
real_t PhysicsDirectBodyState2DSW::get_step() const {
return body->get_space()->get_last_step();
}

91
servers/physics_2d/body_direct_state_2d_sw.h Normal file
View file

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

20
servers/physics_2d/physics_server_2d_sw.cpp
View file

@ -30,6 +30,7 @@
#include "physics_server_2d_sw.h"
#include "body_direct_state_2d_sw.h"
#include "broad_phase_2d_bvh.h"
#include "collision_solver_2d_sw.h"
#include "core/config/project_settings.h"
@ -919,6 +920,12 @@ int PhysicsServer2DSW::body_get_max_contacts_reported(RID p_body) const {
return body->get_max_contacts_reported();
}
void PhysicsServer2DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) {
Body2DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
body->set_state_sync_callback(p_instance, p_callback);
}
void PhysicsServer2DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) {
Body2DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
@ -953,17 +960,13 @@ bool PhysicsServer2DSW::body_test_motion(RID p_body, const Transform2D &p_from,
PhysicsDirectBodyState2D *PhysicsServer2DSW::body_get_direct_state(RID p_body) {
ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
if (!body_owner.owns(p_body)) {
return nullptr;
}
Body2DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, nullptr);
ERR_FAIL_COND_V(!body->get_space(), nullptr);
ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
direct_state->body = body;
return direct_state;
return body->get_direct_state();
}
/* JOINT API */
@ -1227,10 +1230,8 @@ void PhysicsServer2DSW::set_collision_iterations(int p_iterations) {
void PhysicsServer2DSW::init() {
doing_sync = false;
last_step = 0.001;
iterations = 8; // 8?
stepper = memnew(Step2DSW);
direct_state = memnew(PhysicsDirectBodyState2DSW);
};
void PhysicsServer2DSW::step(real_t p_step) {
@ -1240,8 +1241,6 @@ void PhysicsServer2DSW::step(real_t p_step) {
_update_shapes();
last_step = p_step;
PhysicsDirectBodyState2DSW::singleton->step = p_step;
island_count = 0;
active_objects = 0;
collision_pairs = 0;
@ -1313,7 +1312,6 @@ void PhysicsServer2DSW::end_sync() {
void PhysicsServer2DSW::finish() {
memdelete(stepper);
memdelete(direct_state);
};
void PhysicsServer2DSW::_update_shapes() {

5
servers/physics_2d/physics_server_2d_sw.h
View file

@ -46,7 +46,6 @@ class PhysicsServer2DSW : public PhysicsServer2D {
bool active;
int iterations;
bool doing_sync;
real_t last_step;
int island_count;
int active_objects;
@ -59,8 +58,6 @@ class PhysicsServer2DSW : public PhysicsServer2D {
Step2DSW *stepper;
Set<const Space2DSW *> active_spaces;
PhysicsDirectBodyState2DSW *direct_state;
mutable RID_PtrOwner<Shape2DSW, true> shape_owner;
mutable RID_PtrOwner<Space2DSW, true> space_owner;
mutable RID_PtrOwner<Area2DSW, true> area_owner;
@ -241,7 +238,9 @@ public:
virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override;
virtual int body_get_max_contacts_reported(RID p_body) const override;
virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override;
virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override;
virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override;
virtual void body_set_pickable(RID p_body, bool p_pickable) override;

1
servers/physics_2d/physics_server_2d_wrap_mt.h
View file

@ -244,6 +244,7 @@ public:
FUNC2(body_set_omit_force_integration, RID, bool);
FUNC1RC(bool, body_is_omitting_force_integration, RID);
FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback);
FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &);
bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) override {

4
servers/physics_2d/space_2d_sw.cpp
View file

@ -634,7 +634,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
//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();
//compute displacement from linear velocity
Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step;
Vector2 motion = lv * last_step;
real_t motion_len = motion.length();
motion.normalize();
cbk.valid_depth += motion_len * MAX(motion.dot(-cbk.valid_dir), 0.0);
@ -916,7 +916,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
//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();
//compute displacement from linear velocity
Vector2 motion = lv * PhysicsDirectBodyState2DSW::singleton->step;
Vector2 motion = lv * last_step;
real_t motion_len = motion.length();
motion.normalize();
rcd.valid_depth += motion_len * MAX(motion.dot(-rcd.valid_dir), 0.0);

5
servers/physics_2d/space_2d_sw.h
View file

@ -117,6 +117,8 @@ private:
bool locked;
real_t last_step = 0.001;
int island_count;
int active_objects;
int collision_pairs;
@ -172,6 +174,9 @@ public:
void lock();
void unlock();
real_t get_last_step() const { return last_step; }
void set_last_step(real_t p_step) { last_step = p_step; }
void set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_value);
real_t get_param(PhysicsServer2D::SpaceParameter p_param) const;

2
servers/physics_2d/step_2d_sw.cpp
View file

@ -129,6 +129,8 @@ void Step2DSW::step(Space2DSW *p_space, real_t p_delta, int p_iterations) {
p_space->setup(); //update inertias, etc
p_space->set_last_step(p_delta);
iterations = p_iterations;
delta = p_delta;

74
servers/physics_3d/body_3d_sw.cpp
View file

@ -29,7 +29,9 @@
/*************************************************************************/
#include "body_3d_sw.h"
#include "area_3d_sw.h"
#include "body_direct_state_3d_sw.h"
#include "space_3d_sw.h"
void Body3DSW::_update_inertia() {
@ -543,7 +545,7 @@ void Body3DSW::integrate_velocities(real_t p_step) {
return;
}
if (fi_callback) {
if (fi_callback_data || body_state_callback) {
get_space()->body_add_to_state_query_list(&direct_state_query_list);
}
@ -600,11 +602,6 @@ void Body3DSW::integrate_velocities(real_t p_step) {
_set_inv_transform(get_transform().inverse());
_update_transform_dependant();
/*
if (fi_callback) {
get_space()->body_add_to_state_query_list(&direct_state_query_list);
*/
}
/*
@ -662,24 +659,23 @@ void Body3DSW::wakeup_neighbours() {
}
void Body3DSW::call_queries() {
if (fi_callback) {
PhysicsDirectBodyState3DSW *dbs = PhysicsDirectBodyState3DSW::singleton;
dbs->body = this;
Variant v = dbs;
Object *obj = fi_callback->callable.get_object();
if (!obj) {
if (fi_callback_data) {
if (!fi_callback_data->callable.get_object()) {
set_force_integration_callback(Callable());
} else {
const Variant *vp[2] = { &v, &fi_callback->udata };
Variant direct_state_variant = get_direct_state();
const Variant *vp[2] = { &direct_state_variant, &fi_callback_data->udata };
Callable::CallError ce;
int argc = (fi_callback->udata.get_type() == Variant::NIL) ? 1 : 2;
int argc = (fi_callback_data->udata.get_type() == Variant::NIL) ? 1 : 2;
Variant rv;
fi_callback->callable.call(vp, argc, rv, ce);
fi_callback_data->callable.call(vp, argc, rv, ce);
}
}
if (body_state_callback_instance) {
(body_state_callback)(body_state_callback_instance, get_direct_state());
}
}
bool Body3DSW::sleep_test(real_t p_step) {
@ -699,22 +695,34 @@ bool Body3DSW::sleep_test(real_t p_step) {
}
}
void Body3DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
if (fi_callback) {
memdelete(fi_callback);
fi_callback = nullptr;
}
void Body3DSW::set_state_sync_callback(void *p_instance, PhysicsServer3D::BodyStateCallback p_callback) {
body_state_callback_instance = p_instance;
body_state_callback = p_callback;
}
void Body3DSW::set_force_integration_callback(const Callable &p_callable, const Variant &p_udata) {
if (p_callable.get_object()) {
fi_callback = memnew(ForceIntegrationCallback);
fi_callback->callable = p_callable;
fi_callback->udata = p_udata;
if (!fi_callback_data) {
fi_callback_data = memnew(ForceIntegrationCallbackData);
}
fi_callback_data->callable = p_callable;
fi_callback_data->udata = p_udata;
} else if (fi_callback_data) {
memdelete(fi_callback_data);
fi_callback_data = nullptr;
}
}
PhysicsDirectBodyState3DSW *Body3DSW::get_direct_state() {
if (!direct_state) {
direct_state = memnew(PhysicsDirectBodyState3DSW);
direct_state->body = this;
}
return direct_state;
}
Body3DSW::Body3DSW() :
CollisionObject3DSW(TYPE_BODY),
active_list(this),
inertia_update_list(this),
direct_state_query_list(this) {
@ -742,17 +750,13 @@ Body3DSW::Body3DSW() :
still_time = 0;
continuous_cd = false;
can_sleep = true;
fi_callback = nullptr;
}
Body3DSW::~Body3DSW() {
if (fi_callback) {
memdelete(fi_callback);
if (fi_callback_data) {
memdelete(fi_callback_data);
}
if (direct_state) {
memdelete(direct_state);
}
}
PhysicsDirectBodyState3DSW *PhysicsDirectBodyState3DSW::singleton = nullptr;
PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() {
return body->get_space()->get_direct_state();
}

111
servers/physics_3d/body_3d_sw.h
View file

@ -28,14 +28,15 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef BODY_SW_H
#define BODY_SW_H
#ifndef BODY_3D_SW_H
#define BODY_3D_SW_H
#include "area_3d_sw.h"
#include "collision_object_3d_sw.h"
#include "core/templates/vset.h"
class Constraint3DSW;
class PhysicsDirectBodyState3DSW;
class Body3DSW : public CollisionObject3DSW {
PhysicsServer3D::BodyMode mode;
@ -113,12 +114,17 @@ class Body3DSW : public CollisionObject3DSW {
Vector<Contact> contacts; //no contacts by default
int contact_count;
struct ForceIntegrationCallback {
void *body_state_callback_instance = nullptr;
PhysicsServer3D::BodyStateCallback body_state_callback = nullptr;
struct ForceIntegrationCallbackData {
Callable callable;
Variant udata;
};
ForceIntegrationCallback *fi_callback;
ForceIntegrationCallbackData *fi_callback_data = nullptr;
PhysicsDirectBodyState3DSW *direct_state = nullptr;
uint64_t island_step;
@ -129,8 +135,11 @@ class Body3DSW : public CollisionObject3DSW {
friend class PhysicsDirectBodyState3DSW; // i give up, too many functions to expose
public:
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());
PhysicsDirectBodyState3DSW *get_direct_state();
_FORCE_INLINE_ void add_area(Area3DSW *p_area) {
int index = areas.find(AreaCMP(p_area));
if (index > -1) {
@ -349,96 +358,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;
}
class PhysicsDirectBodyState3DSW : public PhysicsDirectBodyState3D {
GDCLASS(PhysicsDirectBodyState3DSW, PhysicsDirectBodyState3D);
public:
static PhysicsDirectBodyState3DSW *singleton;
Body3DSW *body;
real_t step;
virtual Vector3 get_total_gravity() const override { return body->gravity; } // get gravity vector working on this body space/area
virtual real_t get_total_angular_damp() const override { return body->area_angular_damp; } // get density of this body space/area
virtual real_t get_total_linear_damp() const override { return body->area_linear_damp; } // get density of this body space/area
virtual Vector3 get_center_of_mass() const override { return body->get_center_of_mass(); }
virtual Basis get_principal_inertia_axes() const override { return body->get_principal_inertia_axes(); }
virtual real_t get_inverse_mass() const override { return body->get_inv_mass(); } // get the mass
virtual Vector3 get_inverse_inertia() const override { return body->get_inv_inertia(); } // get density of this body space
virtual Basis get_inverse_inertia_tensor() const override { return body->get_inv_inertia_tensor(); } // get density of this body space
virtual void set_linear_velocity(const Vector3 &p_velocity) override { body->set_linear_velocity(p_velocity); }
virtual Vector3 get_linear_velocity() const override { return body->get_linear_velocity(); }
virtual void set_angular_velocity(const Vector3 &p_velocity) override { body->set_angular_velocity(p_velocity); }
virtual Vector3 get_angular_velocity() const override { return body->get_angular_velocity(); }
virtual void set_transform(const Transform3D &p_transform) override { body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform); }
virtual Transform3D get_transform() const override { return body->get_transform(); }
virtual Vector3 get_velocity_at_local_position(const Vector3 &p_position) const override { return body->get_velocity_in_local_point(p_position); }
virtual void add_central_force(const Vector3 &p_force) override { body->add_central_force(p_force); }
virtual void add_force(const Vector3 &p_force, const Vector3 &p_position = Vector3()) override {
body->add_force(p_force, p_position);
}
virtual void add_torque(const Vector3 &p_torque) override { body->add_torque(p_torque); }
virtual void apply_central_impulse(const Vector3 &p_impulse) override { body->apply_central_impulse(p_impulse); }
virtual void apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position = Vector3()) override {
body->apply_impulse(p_impulse, p_position);
}
virtual void apply_torque_impulse(const Vector3 &p_impulse) override { body->apply_torque_impulse(p_impulse); }
virtual void set_sleep_state(bool p_sleep) override { body->set_active(!p_sleep); }
virtual bool is_sleeping() const override { return !body->is_active(); }
virtual int get_contact_count() const override { return body->contact_count; }
virtual Vector3 get_contact_local_position(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
return body->contacts[p_contact_idx].local_pos;
}
virtual Vector3 get_contact_local_normal(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
return body->contacts[p_contact_idx].local_normal;
}
virtual real_t get_contact_impulse(int p_contact_idx) const override {
return 0.0f; // Only implemented for bullet
}
virtual int get_contact_local_shape(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
return body->contacts[p_contact_idx].local_shape;
}
virtual RID get_contact_collider(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
return body->contacts[p_contact_idx].collider;
}
virtual Vector3 get_contact_collider_position(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
return body->contacts[p_contact_idx].collider_pos;
}
virtual ObjectID get_contact_collider_id(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID());
return body->contacts[p_contact_idx].collider_instance_id;
}
virtual int get_contact_collider_shape(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
return body->contacts[p_contact_idx].collider_shape;
}
virtual Vector3 get_contact_collider_velocity_at_position(int p_contact_idx) const override {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
return body->contacts[p_contact_idx].collider_velocity_at_pos;
}
virtual PhysicsDirectSpaceState3D *get_space_state() override;
virtual real_t get_step() const override { return step; }
PhysicsDirectBodyState3DSW() {
singleton = this;
body = nullptr;
}
};
#endif // BODY__SW_H
#endif // BODY_3D_SW_H

182
servers/physics_3d/body_direct_state_3d_sw.cpp Normal file
View file

@ -0,0 +1,182 @@
/*************************************************************************/
/* body_direct_state_3d_sw.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#include "body_direct_state_3d_sw.h"
#include "body_3d_sw.h"
#include "space_3d_sw.h"
Vector3 PhysicsDirectBodyState3DSW::get_total_gravity() const {
return body->gravity;
}
real_t PhysicsDirectBodyState3DSW::get_total_angular_damp() const {
return body->area_angular_damp;
}
real_t PhysicsDirectBodyState3DSW::get_total_linear_damp() const {
return body->area_linear_damp;
}
Vector3 PhysicsDirectBodyState3DSW::get_center_of_mass() const {
return body->get_center_of_mass();
}
Basis PhysicsDirectBodyState3DSW::get_principal_inertia_axes() const {
return body->get_principal_inertia_axes();
}
real_t PhysicsDirectBodyState3DSW::get_inverse_mass() const {
return body->get_inv_mass();
}
Vector3 PhysicsDirectBodyState3DSW::get_inverse_inertia() const {
return body->get_inv_inertia();
}
Basis PhysicsDirectBodyState3DSW::get_inverse_inertia_tensor() const {
return body->get_inv_inertia_tensor();
}
void PhysicsDirectBodyState3DSW::set_linear_velocity(const Vector3 &p_velocity) {
body->set_linear_velocity(p_velocity);
}
Vector3 PhysicsDirectBodyState3DSW::get_linear_velocity() const {
return body->get_linear_velocity();
}
void PhysicsDirectBodyState3DSW::set_angular_velocity(const Vector3 &p_velocity) {
body->set_angular_velocity(p_velocity);
}
Vector3 PhysicsDirectBodyState3DSW::get_angular_velocity() const {
return body->get_angular_velocity();
}
void PhysicsDirectBodyState3DSW::set_transform(const Transform3D &p_transform) {
body->set_state(PhysicsServer3D::BODY_STATE_TRANSFORM, p_transform);
}
Transform3D PhysicsDirectBodyState3DSW::get_transform() const {
return body->get_transform();
}
Vector3 PhysicsDirectBodyState3DSW::get_velocity_at_local_position(const Vector3 &p_position) const {
return body->get_velocity_in_local_point(p_position);
}
void PhysicsDirectBodyState3DSW::add_central_force(const Vector3 &p_force) {
body->add_central_force(p_force);
}
void PhysicsDirectBodyState3DSW::add_force(const Vector3 &p_force, const Vector3 &p_position) {
body->add_force(p_force, p_position);
}
void PhysicsDirectBodyState3DSW::add_torque(const Vector3 &p_torque) {
body->add_torque(p_torque);
}
void PhysicsDirectBodyState3DSW::apply_central_impulse(const Vector3 &p_impulse) {
body->apply_central_impulse(p_impulse);
}
void PhysicsDirectBodyState3DSW::apply_impulse(const Vector3 &p_impulse, const Vector3 &p_position) {
body->apply_impulse(p_impulse, p_position);
}
void PhysicsDirectBodyState3DSW::apply_torque_impulse(const Vector3 &p_impulse) {
body->apply_torque_impulse(p_impulse);
}
void PhysicsDirectBodyState3DSW::set_sleep_state(bool p_sleep) {
body->set_active(!p_sleep);
}
bool PhysicsDirectBodyState3DSW::is_sleeping() const {
return !body->is_active();
}
int PhysicsDirectBodyState3DSW::get_contact_count() const {
return body->contact_count;
}
Vector3 PhysicsDirectBodyState3DSW::get_contact_local_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
return body->contacts[p_contact_idx].local_pos;
}
Vector3 PhysicsDirectBodyState3DSW::get_contact_local_normal(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
return body->contacts[p_contact_idx].local_normal;
}
real_t PhysicsDirectBodyState3DSW::get_contact_impulse(int p_contact_idx) const {
return 0.0f; // Only implemented for bullet
}
int PhysicsDirectBodyState3DSW::get_contact_local_shape(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
return body->contacts[p_contact_idx].local_shape;
}
RID PhysicsDirectBodyState3DSW::get_contact_collider(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, RID());
return body->contacts[p_contact_idx].collider;
}
Vector3 PhysicsDirectBodyState3DSW::get_contact_collider_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
return body->contacts[p_contact_idx].collider_pos;
}
ObjectID PhysicsDirectBodyState3DSW::get_contact_collider_id(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, ObjectID());
return body->contacts[p_contact_idx].collider_instance_id;
}
int PhysicsDirectBodyState3DSW::get_contact_collider_shape(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, 0);
return body->contacts[p_contact_idx].collider_shape;
}
Vector3 PhysicsDirectBodyState3DSW::get_contact_collider_velocity_at_position(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
return body->contacts[p_contact_idx].collider_velocity_at_pos;
}
PhysicsDirectSpaceState3D *PhysicsDirectBodyState3DSW::get_space_state() {
return body->get_space()->get_direct_state();
}
real_t PhysicsDirectBodyState3DSW::get_step() const {
return body->get_space()->get_last_step();
}

94
servers/physics_3d/body_direct_state_3d_sw.h Normal file
View file

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

20
servers/physics_3d/physics_server_3d_sw.cpp
View file

@ -30,6 +30,7 @@
#include "physics_server_3d_sw.h"
#include "body_direct_state_3d_sw.h"
#include "broad_phase_3d_bvh.h"
#include "core/debugger/engine_debugger.h"
#include "core/os/os.h"
@ -836,6 +837,12 @@ int PhysicsServer3DSW::body_get_max_contacts_reported(RID p_body) const {
return body->get_max_contacts_reported();
}
void PhysicsServer3DSW::body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) {
Body3DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
body->set_state_sync_callback(p_instance, p_callback);
}
void PhysicsServer3DSW::body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata) {
Body3DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND(!body);
@ -863,11 +870,12 @@ PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) {
ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
Body3DSW *body = body_owner.getornull(p_body);
ERR_FAIL_COND_V(!body, nullptr);
ERR_FAIL_NULL_V(body, nullptr);
ERR_FAIL_NULL_V(body->get_space(), nullptr);
ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification.");
direct_state->body = body;
return direct_state;
return body->get_direct_state();
}
/* SOFT BODY */
@ -1572,10 +1580,8 @@ void PhysicsServer3DSW::set_collision_iterations(int p_iterations) {
};
void PhysicsServer3DSW::init() {
last_step = 0.001;
iterations = 8; // 8?
stepper = memnew(Step3DSW);
direct_state = memnew(PhysicsDirectBodyState3DSW);
};
void PhysicsServer3DSW::step(real_t p_step) {
@ -1587,9 +1593,6 @@ void PhysicsServer3DSW::step(real_t p_step) {
_update_shapes();
last_step = p_step;
PhysicsDirectBodyState3DSW::singleton->step = p_step;
island_count = 0;
active_objects = 0;
collision_pairs = 0;
@ -1665,7 +1668,6 @@ void PhysicsServer3DSW::end_sync() {
void PhysicsServer3DSW::finish() {
memdelete(stepper);
memdelete(direct_state);
};
int PhysicsServer3DSW::get_process_info(ProcessInfo p_info) {

4
servers/physics_3d/physics_server_3d_sw.h
View file

@ -44,7 +44,6 @@ class PhysicsServer3DSW : public PhysicsServer3D {
friend class PhysicsDirectSpaceState3DSW;
bool active;
int iterations;
real_t last_step;
int island_count;
int active_objects;
@ -57,8 +56,6 @@ class PhysicsServer3DSW : public PhysicsServer3D {
Step3DSW *stepper;
Set<const Space3DSW *> active_spaces;
PhysicsDirectBodyState3DSW *direct_state;
mutable RID_PtrOwner<Shape3DSW, true> shape_owner;
mutable RID_PtrOwner<Space3DSW, true> space_owner;
mutable RID_PtrOwner<Area3DSW, true> area_owner;
@ -237,6 +234,7 @@ public:
virtual void body_set_max_contacts_reported(RID p_body, int p_contacts) override;
virtual int body_get_max_contacts_reported(RID p_body) const override;
virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) override;
virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) override;
virtual void body_set_ray_pickable(RID p_body, bool p_enable) override;

1
servers/physics_3d/physics_server_3d_wrap_mt.h
View file

@ -245,6 +245,7 @@ public:
FUNC2(body_set_omit_force_integration, RID, bool);
FUNC1RC(bool, body_is_omitting_force_integration, RID);
FUNC3(body_set_state_sync_callback, RID, void *, BodyStateCallback);
FUNC3(body_set_force_integration_callback, RID, const Callable &, const Variant &);
FUNC2(body_set_ray_pickable, RID, bool);

5
servers/physics_3d/space_3d_sw.h
View file

@ -112,6 +112,8 @@ private:
bool locked;
real_t last_step = 0.001;
int island_count;
int active_objects;
int collision_pairs;
@ -174,6 +176,9 @@ public:
void lock();
void unlock();
real_t get_last_step() const { return last_step; }
void set_last_step(real_t p_step) { last_step = p_step; }
void set_param(PhysicsServer3D::SpaceParameter p_param, real_t p_value);
real_t get_param(PhysicsServer3D::SpaceParameter p_param) const;

2
servers/physics_3d/step_3d_sw.cpp
View file

@ -185,6 +185,8 @@ void Step3DSW::step(Space3DSW *p_space, real_t p_delta, int p_iterations) {
p_space->setup(); //update inertias, etc
p_space->set_last_step(p_delta);
iterations = p_iterations;
delta = p_delta;

4
servers/physics_server_2d.h
View file

@ -455,6 +455,10 @@ public:
virtual void body_set_omit_force_integration(RID p_body, bool p_omit) = 0;
virtual bool body_is_omitting_force_integration(RID p_body) const = 0;
// Callback for C++ use only.
typedef void (*BodyStateCallback)(void *p_instance, PhysicsDirectBodyState2D *p_state);
virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) = 0;
virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) = 0;
virtual bool body_collide_shape(RID p_body, int p_body_shape, RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, Vector2 *r_results, int p_result_max, int &r_result_count) = 0;

4
servers/physics_server_3d.h
View file

@ -469,6 +469,10 @@ public:
virtual void body_set_omit_force_integration(RID p_body, bool p_omit) = 0;
virtual bool body_is_omitting_force_integration(RID p_body) const = 0;
// Callback for C++ use only.
typedef void (*BodyStateCallback)(void *p_instance, PhysicsDirectBodyState3D *p_state);
virtual void body_set_state_sync_callback(RID p_body, void *p_instance, BodyStateCallback p_callback) = 0;
virtual void body_set_force_integration_callback(RID p_body, const Callable &p_callable, const Variant &p_udata = Variant()) = 0;
virtual void body_set_ray_pickable(RID p_body, bool p_enable) = 0;