diff options
Diffstat (limited to 'servers/physics_2d/godot_joints_2d.cpp')
| -rw-r--r-- | servers/physics_2d/godot_joints_2d.cpp | 24 |
1 files changed, 14 insertions, 10 deletions
diff --git a/servers/physics_2d/godot_joints_2d.cpp b/servers/physics_2d/godot_joints_2d.cpp index 7c08c2f4b4..0c21b08ea9 100644 --- a/servers/physics_2d/godot_joints_2d.cpp +++ b/servers/physics_2d/godot_joints_2d.cpp @@ -5,8 +5,8 @@ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2022 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 */ @@ -118,22 +118,26 @@ bool GodotPinJoint2D::setup(real_t p_step) { K1[0].y = 0.0f; K1[1].y = A->get_inv_mass() + B_inv_mass; + Vector2 r1 = rA - A->get_center_of_mass(); + Transform2D K2; - K2[0].x = A->get_inv_inertia() * rA.y * rA.y; - K2[1].x = -A->get_inv_inertia() * rA.x * rA.y; - K2[0].y = -A->get_inv_inertia() * rA.x * rA.y; - K2[1].y = A->get_inv_inertia() * rA.x * rA.x; + K2[0].x = A->get_inv_inertia() * r1.y * r1.y; + K2[1].x = -A->get_inv_inertia() * r1.x * r1.y; + K2[0].y = -A->get_inv_inertia() * r1.x * r1.y; + K2[1].y = A->get_inv_inertia() * r1.x * r1.x; Transform2D K; K[0] = K1[0] + K2[0]; K[1] = K1[1] + K2[1]; if (B) { + Vector2 r2 = rB - B->get_center_of_mass(); + Transform2D K3; - K3[0].x = B->get_inv_inertia() * rB.y * rB.y; - K3[1].x = -B->get_inv_inertia() * rB.x * rB.y; - K3[0].y = -B->get_inv_inertia() * rB.x * rB.y; - K3[1].y = B->get_inv_inertia() * rB.x * rB.x; + K3[0].x = B->get_inv_inertia() * r2.y * r2.y; + K3[1].x = -B->get_inv_inertia() * r2.x * r2.y; + K3[0].y = -B->get_inv_inertia() * r2.x * r2.y; + K3[1].y = B->get_inv_inertia() * r2.x * r2.x; K[0] += K3[0]; K[1] += K3[1]; |