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

PinJoint2D: fix inertia tensor, taking center of mass into account

This commit is contained in:
Ricardo Buring 2022-03-16 19:55:20 +01:00
parent 5c7971d856
commit 7ce4a1d9f3
1 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
servers/physics_2d/godot_joints_2d.cpp
View File

@ -118,22 +118,26 @@ bool GodotPinJoint2D::setup(real_t p_step) {
K1[0].y = 0.0f; K1[0].y = 0.0f;
K1[1].y = A->get_inv_mass() + B_inv_mass; K1[1].y = A->get_inv_mass() + B_inv_mass;
Vector2 r1 = rA - A->get_center_of_mass();
Transform2D K2; Transform2D K2;
K2[0].x = A->get_inv_inertia() * rA.y * rA.y; K2[0].x = A->get_inv_inertia() * r1.y * r1.y;
K2[1].x = -A->get_inv_inertia() * rA.x * rA.y; K2[1].x = -A->get_inv_inertia() * r1.x * r1.y;
K2[0].y = -A->get_inv_inertia() * rA.x * rA.y; K2[0].y = -A->get_inv_inertia() * r1.x * r1.y;
K2[1].y = A->get_inv_inertia() * rA.x * rA.x; K2[1].y = A->get_inv_inertia() * r1.x * r1.x;
Transform2D K; Transform2D K;
K[0] = K1[0] + K2[0]; K[0] = K1[0] + K2[0];
K[1] = K1[1] + K2[1]; K[1] = K1[1] + K2[1];
if (B) { if (B) {
Vector2 r2 = rB - B->get_center_of_mass();
Transform2D K3; Transform2D K3;
K3[0].x = B->get_inv_inertia() * rB.y * rB.y; K3[0].x = B->get_inv_inertia() * r2.y * r2.y;
K3[1].x = -B->get_inv_inertia() * rB.x * rB.y; K3[1].x = -B->get_inv_inertia() * r2.x * r2.y;
K3[0].y = -B->get_inv_inertia() * rB.x * rB.y; K3[0].y = -B->get_inv_inertia() * r2.x * r2.y;
K3[1].y = B->get_inv_inertia() * rB.x * rB.x; K3[1].y = B->get_inv_inertia() * r2.x * r2.x;
K[0] += K3[0]; K[0] += K3[0];
K[1] += K3[1]; K[1] += K3[1];