mirror of
https://github.com/godotengine/godot
synced 2024-09-05 11:28:29 +00:00
Implement collision impulse in Godot Physics 3D
This commit is contained in:
parent
e780dc332a
commit
8ee6264cc9
|
@ -152,10 +152,10 @@
|
|||
</description>
|
||||
</method>
|
||||
<method name="get_contact_impulse" qualifiers="const">
|
||||
<return type="float" />
|
||||
<return type="Vector3" />
|
||||
<param index="0" name="contact_idx" type="int" />
|
||||
<description>
|
||||
Impulse created by the contact. Only implemented for Bullet physics.
|
||||
Impulse created by the contact.
|
||||
</description>
|
||||
</method>
|
||||
<method name="get_contact_local_normal" qualifiers="const">
|
||||
|
|
|
@ -131,7 +131,7 @@
|
|||
</description>
|
||||
</method>
|
||||
<method name="_get_contact_impulse" qualifiers="virtual const">
|
||||
<return type="float" />
|
||||
<return type="Vector3" />
|
||||
<param index="0" name="contact_idx" type="int" />
|
||||
<description>
|
||||
</description>
|
||||
|
|
|
@ -94,7 +94,7 @@ public:
|
|||
|
||||
EXBIND1RC(Vector3, get_contact_local_position, int)
|
||||
EXBIND1RC(Vector3, get_contact_local_normal, int)
|
||||
EXBIND1RC(real_t, get_contact_impulse, int)
|
||||
EXBIND1RC(Vector3, get_contact_impulse, int)
|
||||
EXBIND1RC(int, get_contact_local_shape, int)
|
||||
EXBIND1RC(RID, get_contact_collider, int)
|
||||
EXBIND1RC(Vector3, get_contact_collider_position, int)
|
||||
|
|
|
@ -126,6 +126,7 @@ class GodotBody3D : public GodotCollisionObject3D {
|
|||
ObjectID collider_instance_id;
|
||||
RID collider;
|
||||
Vector3 collider_velocity_at_pos;
|
||||
Vector3 impulse;
|
||||
};
|
||||
|
||||
Vector<Contact> contacts; //no contacts by default
|
||||
|
@ -183,7 +184,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 Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos);
|
||||
_FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos, const Vector3 &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); }
|
||||
|
@ -347,7 +348,7 @@ public:
|
|||
|
||||
//add contact inline
|
||||
|
||||
void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) {
|
||||
void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos, const Vector3 &p_impulse) {
|
||||
int c_max = contacts.size();
|
||||
|
||||
if (c_max == 0) {
|
||||
|
@ -387,6 +388,7 @@ void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &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_3D_H
|
||||
|
|
|
@ -188,8 +188,9 @@ Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_normal(int p_contact_id
|
|||
return body->contacts[p_contact_idx].local_normal;
|
||||
}
|
||||
|
||||
real_t GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const {
|
||||
return 0.0f; // Only implemented for bullet
|
||||
Vector3 GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const {
|
||||
ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
|
||||
return body->contacts[p_contact_idx].impulse;
|
||||
}
|
||||
|
||||
int GodotPhysicsDirectBodyState3D::get_contact_local_shape(int p_contact_idx) const {
|
||||
|
|
|
@ -89,7 +89,7 @@ public:
|
|||
|
||||
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 Vector3 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;
|
||||
|
|
|
@ -364,26 +364,6 @@ bool GodotBodyPair3D::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;
|
||||
|
||||
// contact query reporting...
|
||||
|
||||
if (A->can_report_contacts()) {
|
||||
Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity();
|
||||
A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crA);
|
||||
}
|
||||
|
||||
if (B->can_report_contacts()) {
|
||||
Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity();
|
||||
B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB);
|
||||
}
|
||||
|
||||
if (report_contacts_only) {
|
||||
collided = false;
|
||||
continue;
|
||||
}
|
||||
|
||||
c.active = true;
|
||||
do_process = true;
|
||||
|
||||
// Precompute normal mass, tangent mass, and bias.
|
||||
Vector3 inertia_A = inv_inertia_tensor_A.xform(c.rA.cross(c.normal));
|
||||
Vector3 inertia_B = inv_inertia_tensor_B.xform(c.rB.cross(c.normal));
|
||||
|
@ -395,6 +375,29 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) {
|
|||
c.depth = depth;
|
||||
|
||||
Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
|
||||
|
||||
c.acc_impulse -= j_vec;
|
||||
|
||||
// contact query reporting...
|
||||
|
||||
if (A->can_report_contacts()) {
|
||||
Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity();
|
||||
A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crA, c.acc_impulse);
|
||||
}
|
||||
|
||||
if (B->can_report_contacts()) {
|
||||
Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity();
|
||||
B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB, -c.acc_impulse);
|
||||
}
|
||||
|
||||
if (report_contacts_only) {
|
||||
collided = false;
|
||||
continue;
|
||||
}
|
||||
|
||||
c.active = true;
|
||||
do_process = true;
|
||||
|
||||
if (collide_A) {
|
||||
A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass());
|
||||
}
|
||||
|
@ -504,6 +507,7 @@ void GodotBodyPair3D::solve(real_t p_step) {
|
|||
if (collide_B) {
|
||||
B->apply_impulse(j, c.rB + B->get_center_of_mass());
|
||||
}
|
||||
c.acc_impulse -= j;
|
||||
|
||||
c.active = true;
|
||||
}
|
||||
|
@ -550,6 +554,7 @@ void GodotBodyPair3D::solve(real_t p_step) {
|
|||
if (collide_B) {
|
||||
B->apply_impulse(jt, c.rB + B->get_center_of_mass());
|
||||
}
|
||||
c.acc_impulse -= jt;
|
||||
|
||||
c.active = true;
|
||||
}
|
||||
|
@ -745,23 +750,6 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
|
|||
c.rA = global_A - transform_A.origin - body->get_center_of_mass();
|
||||
c.rB = global_B;
|
||||
|
||||
if (body->can_report_contacts()) {
|
||||
Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity();
|
||||
body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA);
|
||||
}
|
||||
|
||||
if (report_contacts_only) {
|
||||
collided = false;
|
||||
continue;
|
||||
}
|
||||
|
||||
c.active = true;
|
||||
do_process = true;
|
||||
|
||||
if (body_collides) {
|
||||
body->set_active(true);
|
||||
}
|
||||
|
||||
// Precompute normal mass, tangent mass, and bias.
|
||||
Vector3 inertia_A = body_inv_inertia_tensor.xform(c.rA.cross(c.normal));
|
||||
real_t kNormal = body_inv_mass + node_inv_mass;
|
||||
|
@ -778,6 +766,24 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
|
|||
if (soft_body_collides) {
|
||||
soft_body->apply_node_impulse(c.index_B, j_vec);
|
||||
}
|
||||
c.acc_impulse -= j_vec;
|
||||
|
||||
if (body->can_report_contacts()) {
|
||||
Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity();
|
||||
body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA, c.acc_impulse);
|
||||
}
|
||||
|
||||
if (report_contacts_only) {
|
||||
collided = false;
|
||||
continue;
|
||||
}
|
||||
|
||||
c.active = true;
|
||||
do_process = true;
|
||||
|
||||
if (body_collides) {
|
||||
body->set_active(true);
|
||||
}
|
||||
|
||||
c.bounce = body->get_bounce();
|
||||
|
||||
|
@ -880,6 +886,7 @@ void GodotBodySoftBodyPair3D::solve(real_t p_step) {
|
|||
if (soft_body_collides) {
|
||||
soft_body->apply_node_impulse(c.index_B, j);
|
||||
}
|
||||
c.acc_impulse -= j;
|
||||
|
||||
c.active = true;
|
||||
}
|
||||
|
@ -924,6 +931,7 @@ void GodotBodySoftBodyPair3D::solve(real_t p_step) {
|
|||
if (soft_body_collides) {
|
||||
soft_body->apply_node_impulse(c.index_B, jt);
|
||||
}
|
||||
c.acc_impulse -= jt;
|
||||
|
||||
c.active = true;
|
||||
}
|
||||
|
|
|
@ -44,6 +44,7 @@ protected:
|
|||
Vector3 normal;
|
||||
int index_A = 0, index_B = 0;
|
||||
Vector3 local_A, local_B;
|
||||
Vector3 acc_impulse; // accumulated impulse - only one of the object's impulse is needed as impulse_a == -impulse_b
|
||||
real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn)
|
||||
Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt)
|
||||
real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb)
|
||||
|
|
|
@ -95,7 +95,7 @@ public:
|
|||
|
||||
virtual Vector3 get_contact_local_position(int p_contact_idx) const = 0;
|
||||
virtual Vector3 get_contact_local_normal(int p_contact_idx) const = 0;
|
||||
virtual real_t get_contact_impulse(int p_contact_idx) const = 0;
|
||||
virtual Vector3 get_contact_impulse(int p_contact_idx) const = 0;
|
||||
virtual int get_contact_local_shape(int p_contact_idx) const = 0;
|
||||
|
||||
virtual RID get_contact_collider(int p_contact_idx) const = 0;
|
||||
|
|
Loading…
Reference in a new issue