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

Add get_contact_impulse method to PhysicsDirectBodyState2D

Browse Source 
This makes it consistent with 3D.
This commit is contained in:
Ricardo Buring
2023-01-10 23:28:02 +01:00
parent 91713ced81
commit 3efa105548
11 changed files with 48 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
servers/physics_2d/godot_body_2d.h
View File

@ -132,6 +132,7 @@ class GodotBody2D : public GodotCollisionObject2D {
ObjectID collider_instance_id;
RID collider;
Vector2 collider_velocity_at_pos;
Vector2 impulse;
};
Vector<Contact> contacts; //no contacts by default
@ -190,7 +191,7 @@ public:
_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); }
_FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos);
_FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos, const Vector2 &p_impulse);
_FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
_FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); }
@ -340,7 +341,7 @@ public:
//add contact inline
void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos) {
void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos, const Vector2 &p_impulse) {
int c_max = contacts.size();
if (c_max == 0) {
@ -380,6 +381,7 @@ void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local
c[idx].collider_instance_id = p_collider_instance_id;
c[idx].collider = p_collider;
c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
c[idx].impulse = p_impulse;
}
#endif // GODOT_BODY_2D_H

5
servers/physics_2d/godot_body_direct_state_2d.cpp
View File

@ -210,6 +210,11 @@ Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_velocity_at_position
return body->contacts[p_contact_idx].collider_velocity_at_pos;
}
Vector2 GodotPhysicsDirectBodyState2D::get_contact_impulse(int p_contact_idx) const {
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
return body->contacts[p_contact_idx].impulse;
}
PhysicsDirectSpaceState2D *GodotPhysicsDirectBodyState2D::get_space_state() {
return body->get_space()->get_direct_state();
}

2
servers/physics_2d/godot_body_direct_state_2d.h
View File

@ -92,8 +92,8 @@ public:
virtual Vector2 get_contact_collider_position(int p_contact_idx) const override;
virtual ObjectID get_contact_collider_id(int p_contact_idx) const override;
virtual int get_contact_collider_shape(int p_contact_idx) const override;
virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override;
virtual Vector2 get_contact_impulse(int p_contact_idx) const override;
virtual PhysicsDirectSpaceState2D *get_space_state() override;

37
servers/physics_2d/godot_body_pair_2d.cpp
View File

@ -434,21 +434,6 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
c.rA = global_A - A->get_center_of_mass();
c.rB = global_B - B->get_center_of_mass() - offset_B;
if (A->can_report_contacts()) {
Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity());
}
if (B->can_report_contacts()) {
Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity());
}
if (report_contacts_only) {
collided = false;
continue;
}
// Precompute normal mass, tangent mass, and bias.
real_t rnA = c.rA.dot(c.normal);
real_t rnB = c.rB.dot(c.normal);
@ -466,11 +451,28 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
c.depth = depth;
Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
c.acc_impulse -= P;
if (A->can_report_contacts()) {
Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity(), c.acc_impulse);
}
if (B->can_report_contacts()) {
Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity(), c.acc_impulse);
}
if (report_contacts_only) {
collided = false;
continue;
}
#ifdef ACCUMULATE_IMPULSES
{
// Apply normal + friction impulse
Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
if (collide_A) {
A->apply_impulse(-P, c.rA + A->get_center_of_mass());
}
@ -581,6 +583,7 @@ void GodotBodyPair2D::solve(real_t p_step) {
if (collide_B) {
B->apply_impulse(j, c.rB + B->get_center_of_mass());
}
c.acc_impulse -= j;
}
}

1
servers/physics_2d/godot_body_pair_2d.h
View File

@ -59,6 +59,7 @@ class GodotBodyPair2D : public GodotConstraint2D {
Vector2 position;
Vector2 normal;
Vector2 local_A, local_B;
Vector2 acc_impulse; // accumulated impulse
real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn)
real_t acc_tangent_impulse = 0.0; // accumulated tangent impulse (Pt)
real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb)