Vật lý: tính gia tốc góc trong thế giới so với không gian cục bộ

Tôi có một mô phỏng cơ thể cứng nhắc khá đơn giản được viết bằng Python. Trong mã của tôi, tôi tính toán gia tốc góc bằng công thức:

angAcc = mô-men xoắn * quán tính ^ -1 (lưu ý thứ tự ma trận hàng chính)

Bây giờ, mô-men xoắn là trong không gian thế giới, do đó quán tính của tôi cũng sẽ ở trong không gian thế giới. Mã Python của tôi thực hiện tính toán gia tốc góc trông như thế này:

oriMat = quaternion.ToMatrix(self.ori)
inertia_world = self.inertia_local * oriMat
matrix.Invert(inertia_world)
angAcc = vector.Transform(self.totalTorque, inertia_world)

oriMat có hiệu quả chuyển đổi từ địa phương sang thế giới. self.totalTorque là mô-men xoắn trong không gian thế giới. Vì vậy, mã này biến đổi quán tính cục bộ thành quán tính thế giới và sử dụng nghịch đảo của nó cùng với mô-men thế giới để tính gia tốc góc của thế giới. Mã này hoạt động hoàn toàn tốt.

Tôi nghĩ để kiểm tra xem liệu tôi có thể tiến hành tính toán gia tốc góc trong không gian cục bộ và cuối cùng chuyển đổi angAcc cục bộ sang angAcc thế giới. Đây là mã:

oriMat = quaternion.ToMatrix(self.ori)
self.totalTorque = vector.Transform(self.totalTorque, matrix.Inverted(oriMat))
angAcc = vector.Transform(self.totalTorque, matrix.Inverted(self.inertia_local))
angAcc = vector.Transform(angAcc, oriMat)

Ở đây tôi muốn thực hiện biến đổi mô-men xoắn trong không gian cục bộ. Để làm như vậy, tôi chuyển đổi self.totalTorque sang không gian cục bộ của cơ thể cứng bằng cách sử dụng nghịch đảo của oriMat. Sau đó, tôi mul rằng bằng cách đảo ngược quán tính địa phương để có được gia tốc góc trong không gian địa phương. Cuối cùng, tôi biến đổi gia tốc góc với không gian thế giới bằng cách biến đổi nó bằng oriMat.

Vì một số lý do, mã này không mang lại hành vi đúng đắn cho cơ thể cứng nhắc của tôi và tôi thực sự không thể biết tại sao. Bất cứ ai cũng có một ý tưởng?

Bạn nói :

Sau đó, tôi mul rằng bằng cách đảo ngược quán tính địa phương để có được gia tốc góc trong không gian địa phương.

Điều này có vẻ sai, vì "quán tính cục bộ đảo ngược" là "quán tính thế giới", vì vậy bạn đang đặt "cái đó" vào không gian thế giới trong khi bạn muốn nó trong không gian địa phương ...

Vì vậy, mã phải là:

// oriMat: local -> world
oriMat = quaternion.ToMatrix(self.ori)

// put total torque in local space
self.totalTorque = vector.Transform(self.totalTorque, matrix.Inverted(oriMat))

// multiply local torque with local inertia to get angAcc in local space
angAcc = vector.Transform(self.totalTorque, self.inertia_local))

// put angAcc in world space
angAcc = vector.Transform(angAcc, oriMat)