Pythonで非一様な密度の重心位置 (CoG)から慣性モーメントテンソルを求めて物体を回す

#0. 概要
まずは、こんな感じのものを作る。
![ezgif.com-crop (1).gif](https://qiita-image-store.s3.amazonaws.com/0/95636/f1ac2c83-2670-786a-91f3-673d07f2e3c3.gif)
黒い点が各物質の密度(重さ)で赤色になるにつれて重くなる。四隅の赤い丸が矩形の頂点。黄色い丸がジャイロセンサ。その隣の緑色がCG（重心位置）。青色の丸が力点で、フェース法線の大きさが力の大きさF。

ってことで、非一様な密度から重心位置 (CoG)の慣性モーメントテンソルを求めて、物体を回す！！
時間がたつにつれて角加速度が増加していき、回転が速くなるのは摩擦や空気抵抗が一切なく、減衰しないからである。

# 0.1 実装コード

``` python
# -*- coding: UTF-8 -*-
import matplotlib.animation as anm
import matplotlib.pyplot as plt
import matplotlib.transforms as trm
import numpy as np

# -- Constant --
AX1_XLIM_SIZE_0 = 0
AX1_XLIM_SIZE_1 = 150

AX1_YLIM_SIZE_0 = 0
AX1_YLIM_SIZE_1 = 150

AX2_XLIM_SIZE_0 = 0
AX2_XLIM_SIZE_1 = 150

AX2_YLIM_SIZE_0 = 0
AX2_YLIM_SIZE_1 = 150

NV_SIZE = 20


class C_Thruster:
    """Thruster Object"""

    def __init__(self, _x, _y):  # コンストラクタ
        self.posxy = np.matrix([_x, _y]).A1
        self.nvxy  = np.matrix([_x, _y]).A1
        self.flag = 0


class C_MassObject:
    """Mass Object"""

    def __init__(self):                  # コンストラクタ
        self.width = 100
        self.height = 100
        self.x_m = 100
        self.y_m = 100
        # Center of gravity
        self.cxy = np.matrix([AX2_XLIM_SIZE_1 / 2, AX2_YLIM_SIZE_1 / 2]).A1

        # Origin center
        self.origxy  = np.matrix([AX2_XLIM_SIZE_1 / 2, AX2_YLIM_SIZE_1 / 2]).A1

        # Position
        self.posxy = [
            np.matrix([-self.x_m / 2, self.y_m / 2]).A1,
            np.matrix([self.x_m / 2, self.y_m / 2]).A1,
            np.matrix([-self.x_m / 2, -self.y_m / 2]).A1,
            np.matrix([self.x_m / 2, -self.y_m/ 2]).A1
        ]

        for _posxy in self.posxy:
            _posxy += self.origxy

        self.orig_left_up_xy = self.posxy[0]

        self.rotate = 0
        self.rotate_rad = 0
        self.rotate_acc_rad_s = 0
        self.rotate_total_rad = 0
        self.pyro = [C_Thruster(self.posxy[0][0], self.posxy[0][1] - 20), C_Thruster(self.posxy[0][0] + 20, self.posxy[0][1]),
                     C_Thruster(self.posxy[1][0], self.posxy[1][1] - 20), C_Thruster(self.posxy[1][0] - 20, self.posxy[1][1]),
                     C_Thruster(self.posxy[2][0], self.posxy[2][1] + 20), C_Thruster(self.posxy[2][0] + 20, self.posxy[2][1]),
                     C_Thruster(self.posxy[3][0], self.posxy[3][1] + 20), C_Thruster(self.posxy[3][0] - 20, self.posxy[3][1])]

        # cg 1.5, 2.5 max 10kg
        self.max_mass = 100

        self.mass_kg = np.matrix([
            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
            [1, 1, 77, 1, 1, 1, 1, 1, 1, 1],
            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
            [99, 1, 1, 1, 1, 1, 1, 1, 1, 1]
        ]).A

        self.rho = np.matrix([
            [.0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [.0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        ]).A



        self.x_grid = 10
        self.y_grid = 10
        self.x_grid_m = 100 / 10
        self.y_grid_m = 100 / 10
        self.z_grid_m = 1
        self.cg_xy_m = np.matrix([.0, .0]).A1

        #
        self.acc_1 = np.matrix([
            [.0, .0],
            [.0, .0]
        ]).A

        self.accSensor1xy = np.matrix([.0, .0]).A1
        self.accSensor2xy = np.matrix([.0, .0]).A1

        self.f_kg = 10

        self.inertia = 0
        self.inertia_mat = np.matrix([
            [.0, .0, .0],
            [.0, .0, .0],
            [.0, .0, .0]
        ]).A

        # z axis
        self.paxisInertia  = np.matrix([
            [.0, .0, .1],
            [.0, .0, .1],
            [.0, .0, .1]
        ]).A

        # convert
        self.grid_x = np.matrix([
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0]
        ]).A
        self.grid_y = np.matrix([
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0],
            [.0, .0, .0, .0, .0, .0, .0, .0, .0, .0]
        ]).A
        for i in range(self.x_grid):
            for j in range(self.y_grid):
                tmpx = j * self.x_grid_m
                tmpy = i * -self.y_grid_m

                # Centre of position on grid
                tmpx += (self.x_grid_m / 2)
                tmpy -= (self.y_grid_m / 2)
                self.grid_x[i][j] = tmpx + self.orig_left_up_xy[0]
                self.grid_y[i][j] = tmpy + self.orig_left_up_xy[1]

                # kg -> rho
                self.rho[i][j] = self.mass_kg[i][j] / (self.x_grid_m * self.y_grid_m * self.z_grid_m)

        self.cg_xy_m[0]  += self.orig_left_up_xy[0]
        self.cg_xy_m[1]  = self.orig_left_up_xy[1] - self.cg_xy_m[1]

        self.accSensor1xy[0]  = self.grid_x[7][2]
        self.accSensor1xy[1]  = self.grid_y[7][2]


        # X方向の重心
        tmprho = .0
        tmprhob = .0
        tmpcgx = .0
        tmpcgxb = .0

        for i in range(self.y_grid):
            for j in range(self.x_grid):
                if j == 0:
                    tmpcgx = self.grid_x[i][j]
                    tmprho = self.rho[i][j]
                    continue
                else:
                    l = tmpcgx - self.grid_x[i][j]
                    x = (tmprho / (tmprho + self.rho[i][j])) * l
                    tmpcgx = self.grid_x[i][j] + x
                    tmprho += self.rho[i][j]

            if i != 0:
                l = tmpcgxb - tmpcgx
                x = (tmprhob / (tmprhob + tmprho)) * l
                tmprhob += tmprho
                tmpcgxb = tmpcgx + x
            else:
                tmprhob = tmprho
                tmpcgxb = tmpcgx

        self.cg_xy_m[0] = tmpcgxb



        # Y方向の重心
        tmprho = .0
        tmprhob = .0
        tmpcgx = .0
        tmpcgxb = .0
        for i in range(self.x_grid):
            for j in range(self.y_grid):
                if j == 0:
                    tmpcgx = self.grid_y[j][i]
                    tmprho = self.rho[j][i]
                    continue
                else:
                    l = tmpcgx - self.grid_y[j][i]
                    x = (tmprho / (tmprho + self.rho[j][i])) * l
                    tmpcgx = self.grid_y[j][i] + x
                    tmprho += self.rho[j][i]

            if i != 0:
                l = tmpcgxb - tmpcgx
                x = (tmprhob / (tmprhob + tmprho)) * l
                tmprhob += tmprho
                tmpcgxb = tmpcgx + x
            else:
                tmprhob = tmprho
                tmpcgxb = tmpcgx

        self.cg_xy_m[1] = tmpcgxb

        #self.accSensor1xy[0] = self.cg_xy_m[0] - 0.01
        #self.accSensor1xy[1] = self.cg_xy_m[1] - 0.01


def calcRotate(_posxy, _orgxy, _rad):
    ADBD = _posxy - _orgxy
    ADBD = np.matrix([[ADBD[0]], [ADBD[1]]])
    rotateMat = np.matrix([
        [np.cos(_rad), -np.sin(_rad)],
        [np.sin(_rad), np.cos(_rad)],
    ])
    cxy = (rotateMat * ADBD).A1 + _orgxy

    return  cxy

def calcNorm(_posxy, _orgxy):
    ADBD = _posxy - _orgxy
    ADBD = np.matrix([[ADBD[0]], [ADBD[1]]])
    theta = np.deg2rad(90)
    rotateMat = np.matrix([
        [np.cos(theta), -np.sin(theta)],
        [np.sin(theta), np.cos(theta)],
    ])
    cxy = (rotateMat * ADBD)
    vv = cxy.A1
    nv = vv / np.linalg.norm(vv)
    return nv

def calcInertia(obj):
    tmp_inertia_mat = np.matrix([
        [.0, .0, .0],
        [.0, .0, .0],
        [.0, .0, .0]
    ]).A

    for i in range(obj.x_grid):
        for j in range(obj.y_grid):
            # xyz
            # xx = y^2 + z^2
            tmpr = ( ((obj.cg_xy_m[1] - obj.grid_y[i][j]) ** 2) + (0))
            tmp_inertia_mat[0][0] += obj.rho[i][j] * obj.x_grid_m * obj.y_grid_m * obj.z_grid_m * tmpr

            # xy = -xy
            tmpr = ( (obj.cg_xy_m[0] - obj.grid_x[i][j]) * (obj.cg_xy_m[1] - obj.grid_y[i][j]) )
            tmp_inertia_mat[0][1] -= obj.rho[i][j] * obj.x_grid_m * obj.y_grid_m * obj.z_grid_m * tmpr

            # xz = -xz
            tmpr = ((obj.cg_xy_m[0] - obj.grid_x[i][j])  *  ( 0 ))
            tmp_inertia_mat[0][2] -= obj.rho[i][j] * obj.x_grid_m * obj.y_grid_m * obj.z_grid_m * tmpr

            # yx = -xy
            tmpr = ( (obj.cg_xy_m[0] - obj.grid_x[i][j]) * (obj.cg_xy_m[1] - obj.grid_y[i][j]) )
            tmp_inertia_mat[1][0] -= obj.rho[i][j] * obj.x_grid_m * obj.y_grid_m * obj.z_grid_m * tmpr

            # yy = z^2 + x^2
            tmpr = (0) + ((obj.cg_xy_m[0] - obj.grid_x[i][j]) ** 2)
            tmp_inertia_mat[1][1] += obj.rho[i][j] * obj.x_grid_m * obj.y_grid_m * obj.z_grid_m * tmpr

            # yz = -yz
            tmpr = (obj.cg_xy_m[1] - obj.grid_y[i][j])  *  ( 0 )
            tmp_inertia_mat[1][2] -= obj.rho[i][j] * obj.x_grid_m * obj.y_grid_m * obj.z_grid_m * tmpr

            # zx = -xz
            tmpr = ((obj.cg_xy_m[0] - obj.grid_x[i][j])  *  ( 0 ))
            tmp_inertia_mat[2][0] -= obj.rho[i][j] * obj.x_grid_m * obj.y_grid_m * obj.z_grid_m * tmpr

            # zy = -yz
            tmpr = ((obj.cg_xy_m[1] - obj.grid_y[i][j])  *  ( 0 ))
            tmp_inertia_mat[2][1] -= obj.rho[i][j] * obj.x_grid_m * obj.y_grid_m * obj.z_grid_m * tmpr

            # zz = x^2 + y^2
            tmpr = (((obj.cg_xy_m[0] - obj.grid_x[i][j]) ** 2) + ((obj.cg_xy_m[1] - obj.grid_y[i][j]) ** 2))
            tmp_inertia_mat[2][2] += obj.rho[i][j] * obj.x_grid_m * obj.y_grid_m * obj.z_grid_m * tmpr


    return tmp_inertia_mat

def calcAngular(_obj, _t = 1):
    w_rad_s = 0.0

    # Izz (X, Y 回転なし)
    tmpInertia = _obj.inertia_mat[:][2].sum()

    for i in range(len(_obj.pyro)):
        if _obj.pyro[i].flag == 1:
            ADBD = _obj.pyro[i].posxy - _obj.cg_xy_m
            L = np.sqrt(ADBD[0] ** 2 + ADBD[1] ** 2)
            f_kg = _obj.f_kg * L * 1
            w_rad_s = f_kg / tmpInertia

    return w_rad_s

def drawGridPoint(obj, ax):
    for i in range(obj.x_grid):
        for j in range(obj.y_grid):
            ax.scatter((obj.grid_x[i][j]), (obj.grid_y[i][j]),  color=[obj.mass_kg[i][j]/obj.max_mass, 0, 0.2], zorder=2)

    ax.scatter((obj.cg_xy_m[0]), (obj.cg_xy_m[1]), c='green', zorder=2)
    return 0


def update(count, fig_title, A, _obj1):
    if count != 0:
        plt.clf()                      # 現在描写されているグラフを消去

#    y = A * np.sin(x - count)
#    plt.subplot(1, 2, 1)
#    ax1 = plt.gca()
#    ax1.set_aspect('equal', adjustable='box')
#    ax1.set_xlim([AX1_XLIM_SIZE_0, AX1_XLIM_SIZE_1])
#    ax1.set_ylim([AX1_YLIM_SIZE_0, AX1_YLIM_SIZE_1])
#    plt.plot(x, y, "r")
#    plt.title("left" + '[' + str(count) + ']')

    # ---
    plt.subplot(1, 1, 1)
    ax2 = plt.gca()
    ax2.set_aspect('equal', adjustable='box')
    ax2.set_xlim([AX2_XLIM_SIZE_0, AX2_XLIM_SIZE_1])
    ax2.set_ylim([AX2_YLIM_SIZE_0, AX2_YLIM_SIZE_1])


    #############################################################
    # Inertia
    #############################################################
    # Inertia (質量が変更しない限りは再計算の必要なし)
    # _obj1.inertia_mat = calcInertia(_obj1)

    # acceleration
    _obj1.rotate_acc_rad_s  = calcAngular(_obj1, 1)
    # speed
    _obj1.rotate_rad += _obj1.rotate_acc_rad_s
    # past rotate + speed
    _obj1.rotate = _obj1.rotate_rad
    #
    _obj1.rotate_total_rad += _obj1.rotate_rad
    print("角加速度: " + str(np.rad2deg(_obj1.rotate_rad)))
    print("角度: " + str(np.rad2deg(_obj1.rotate_total_rad) % 360))

    #############################################################
    # Rotation
    #############################################################
    # Sensor Rotation
    if count != 0:
      tmp_Acc = _obj1.acc_1[1] - _obj1.acc_1[0]
      tmp_Deg = np.rad2deg(np.arctan(tmp_Acc[1] / tmp_Acc[0]))

    _obj1.acc_1[0] = _obj1.accSensor1xy
    _obj1.accSensor1xy = calcRotate(_obj1.accSensor1xy, _obj1.cg_xy_m, _obj1.rotate_rad)
    _obj1.acc_1[1] = _obj1.accSensor1xy

    if count != 0:
      tmp_Acc2 = _obj1.acc_1[1] - _obj1.acc_1[0]
      tmp_Deg2 = np.rad2deg(np.arctan(tmp_Acc2[1] / tmp_Acc2[0]))
      print(np.abs(tmp_Deg  - tmp_Deg2))

    #print (np.rad2deg( np.arctan(1 / np.sqrt((tmp_Acc[0] ** 2) + (tmp_Acc[1] ** 2))) ))
    #print (np.rad2deg( np.arccos(tmp_Acc[1])))


    # Pyro Rotation
    for i in range(len(_obj1.pyro)):
        _obj1.pyro[i].posxy = calcRotate(_obj1.pyro[i].posxy, _obj1.cg_xy_m, _obj1.rotate)

    # Rect Rotation
    for i in range(len(_obj1.posxy)):
        _obj1.posxy[i] = calcRotate(_obj1.posxy[i], _obj1.cg_xy_m, _obj1.rotate)

    # Grid Rotation
    for i in range(_obj1.x_grid):
        for j in range(_obj1.y_grid):
            tmpxy = np.matrix([_obj1.grid_x[i][j], _obj1.grid_y[i][j]])
            tmpxy = calcRotate(tmpxy.A1, _obj1.cg_xy_m, _obj1.rotate)
            _obj1.grid_x[i][j] = tmpxy[0]
            _obj1.grid_y[i][j] = tmpxy[1]

    # Pyro NV - 1
    nv = calcNorm(_obj1.posxy[1], _obj1.posxy[0])
    _obj1.pyro[1].nvxy = _obj1.pyro[1].posxy + (nv * NV_SIZE)
    _obj1.pyro[3].nvxy = _obj1.pyro[3].posxy + (nv * NV_SIZE)

    # Pyro NV - 2
    nv = calcNorm(_obj1.posxy[3], _obj1.posxy[1])
    _obj1.pyro[2].nvxy = _obj1.pyro[2].posxy + (nv * NV_SIZE)
    _obj1.pyro[6].nvxy = _obj1.pyro[6].posxy + (nv * NV_SIZE)

    # Pyro NV - 3
    nv = calcNorm(_obj1.posxy[0], _obj1.posxy[2])
    _obj1.pyro[0].nvxy = _obj1.pyro[0].posxy + (nv * NV_SIZE)
    _obj1.pyro[4].nvxy = _obj1.pyro[4].posxy + (nv * NV_SIZE)

    # Pyro NV - 4
    nv = calcNorm(_obj1.posxy[2], _obj1.posxy[3])
    _obj1.pyro[5].nvxy = _obj1.pyro[5].posxy + (nv * NV_SIZE)
    _obj1.pyro[7].nvxy = _obj1.pyro[7].posxy + (nv * NV_SIZE)

    #############################################################
    # Draw
    #############################################################
    # Sensor
    ax2.scatter((_obj1.accSensor1xy[0]), (_obj1.accSensor1xy[1]), c='Yellow', zorder=3)

    # Rect Dot
    for i in range(len(_obj1.posxy)):
        ax2.scatter((_obj1.posxy[i][0]), (_obj1.posxy[i][1]), c='red', zorder=2)

    # Rect Line
    plt.plot((_obj1.posxy[0][0], _obj1.posxy[1][0]), (_obj1.posxy[0][1], _obj1.posxy[1][1]), 'k-', lw=2)
    plt.plot((_obj1.posxy[3][0], _obj1.posxy[1][0]), (_obj1.posxy[3][1], _obj1.posxy[1][1]), 'k-', lw=2)
    plt.plot((_obj1.posxy[0][0], _obj1.posxy[2][0]), (_obj1.posxy[0][1], _obj1.posxy[2][1]), 'k-', lw=2)
    plt.plot((_obj1.posxy[3][0], _obj1.posxy[2][0]), (_obj1.posxy[3][1], _obj1.posxy[2][1]), 'k-', lw=2)

    # Draw Grid
    drawGridPoint(_obj1, ax2)

    # Pyro dot and Norm vector
    for i in range(len(_obj1.pyro)):
        if _obj1.pyro[i].flag == 1:
            ax2.scatter((_obj1.pyro[i].posxy[0]), (_obj1.pyro[i].posxy[1]), c='blue', zorder=2)
            plt.plot([_obj1.pyro[i].posxy[0], _obj1.pyro[i].nvxy[0]], [_obj1.pyro[i].posxy[1], _obj1.pyro[i].nvxy[1]], 'k-', lw=1)
        else:
            ax2.scatter((_obj1.pyro[i].posxy[0]), (_obj1.pyro[i].posxy[1]), c='green', zorder=2)


    plt.title("right" + '[' + str(count)+ ']')

obj1 = C_MassObject()
obj1.pyro[2].flag = 1
obj1.inertia_mat = calcInertia(obj1)

fig = plt.figure(figsize = (10, 5))
x = np.arange(0, 10, 0.1)
ani = anm.FuncAnimation(fig, update, fargs = ('Initial Animation! ', 2.0, obj1), interval = 1,
                        repeat=False, blit=False)
plt.show()

```

# 1. 非一様な物体の重心位置の求め方
Pythonで非一様な密度を持つ物体の重心位置 (CoG)を求めてみます。
非一様なので多角形でもなんでも良いですが、近似するのはグリッドで行います。図にするとこんな感じ。
![image.png](https://qiita-image-store.s3.amazonaws.com/0/95636/c79d33bb-a8eb-830a-a8d9-82c584b6c9ad.png)

# 1.1. 支配方程式
まずは2点間の重心位置を求める方法
![image.png](https://qiita-image-store.s3.amazonaws.com/0/95636/f61cfe0e-9501-4f8b-4547-217ddf9bade4.png)

これを拡張して、各セル間の重心位置を求めていく。
手順としては、2点のセルの重心位置を求めた後、その2点を結合(密度を積算)し、1つのセルとする。
その後、隣接セルとの重心位置を求める。これを全セル繰り返す。

# 1.2. 実装


# 2. 物体の回転

# 2.1. 支配方程式

# 2.2. 実装


# 3. 慣性モーメントテンソルの求め方

# 3.1. 支配方程式

# 3.2. 実装

# 4. アニメーション
# 4.1. 実装