asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Merge pull request #49471 from nekomatata/body-state-sync-callback

Browse source 
Clean physics direct body state usage in 2D and 3D physics
This commit is contained in:
Juan Linietsky 2021-08-31 14:30:17 -03:00 committed by GitHub
commit 7946066577
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
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() {
@ -1057,7 +1041,7 @@ bool CharacterBody2D::move_and_slide() {
excluded = (moving_platform_wall_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() {
@ -2252,23 +2239,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);
@ -2730,7 +2713,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;
}
@ -2749,7 +2732,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() {
@ -495,7 +496,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);
}
@ -547,27 +548,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) {
@ -587,19 +588,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),
@ -632,33 +646,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"
@ -926,6 +927,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);
@ -960,17 +967,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 */
@ -1234,10 +1237,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) {
@ -1247,8 +1248,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;
@ -1320,7 +1319,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;
@ -242,7 +239,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

@ -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 &);
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);
@ -926,7 +926,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() {
@ -536,7 +538,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);
}
@ -593,11 +595,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);
*/
}
/*
@ -655,24 +652,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) {
@ -692,22 +688,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) {
@ -735,17 +743,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"
@ -842,6 +843,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);
@ -869,11 +876,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 */
@ -1578,10 +1586,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) {
@ -1593,9 +1599,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;
@ -1671,7 +1674,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;
@ -238,6 +235,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

@ -246,6 +246,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

@ -457,6 +457,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

@ -471,6 +471,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;