位置座標から3次元グリッドのインデックスを検索する (Python & NumPy)

はじめに

定義：

グリッド

3つのインデックス i, j, k で指定できる3次元位置座標の集合。

セル

グリッドに含まれる座標のうち、
(i, j, k), (i+1, j, k), (i+1, j+1, k), (i, j+1, k), (i, j, k+1), (i+1, j, k+1), (i+1, j+1, k+1), (i, j+1, k+1)
の8つで構成される平面に囲まれた空間。グリッドの最大インデックスが (I, J, K) であるとき、セルの最大インデックスは (I-1, J-1, K-1).

目標：

• 3次元空間内の任意の位置座標 p を含むセルのインデックス (i, j, k) を求める。

方針

• p からセルの構成面 s へのベクトル
• セル構成面の外向き法線ベクトル

の内積を計算します。計算結果がすべて0以上であれば、p はセル内もしくはセル構成面上に存在するはずです。

言語は Python3.3, 数値計算用ライブラリ NumPy を利用します。NumPy の練習も兼ねて。

書いてみた

座標からインデックスを求めるよりも、グリッドと法線ベクトル生成に多く時間が取られるスクリプトになってしまいました…。はたしてこれで正しいのかどうか。

import numpy
import time


def get_normals(face_triangles):
    return numpy.cross(face_triangles[:,:,1] - face_triangles[:,:,0],
                        face_triangles[:,:,2] - face_triangles[:,:,0])


def get_cells(points, grid_shape):
    cells_shape = numpy.array(grid_shape[:3]) - 1
    num_of_cells = numpy.multiply.reduce(cells_shape)
    face_triangles = numpy.zeros([num_of_cells, 12, 3, 3], dtype=numpy.float)
    points_on_face = numpy.zeros([num_of_cells, 12, 3], dtype=numpy.float)
    i, j, k = (0, 0, 0)
    dj, dk = (grid_shape[0], grid_shape[0] * grid_shape[1])
    for index in range(num_of_cells):
        if i == cells_shape[0]:
            i = 0
            j += 1
        if j == cells_shape[1]:
            j = 0
            k += 1
        base_i = i
        base_j = dj * j
        base_k = dk * k
        next_i = i + 1
        next_j = dj * (j + 1)
        next_k = dk * (k + 1)
        face_triangles[index] = ((points[i + base_j + base_k],
                                  points[next_i + next_j + base_k],
                                  points[next_i + base_j + base_k]),
                                 (points[i + base_j + base_k],
                                  points[i + next_j + base_k],
                                  points[next_i + next_j + base_k]),
                                 (points[i + base_j + next_k],
                                  points[next_i + base_j + next_k],
                                  points[next_i + next_j + next_k]),
                                 (points[i + base_j + next_k],
                                  points[next_i + next_j + next_k],
                                  points[i + next_j + next_k]),
                                 # y-z
                                 (points[i + base_j + base_k],
                                  points[i + next_j + next_k],
                                  points[i + next_j + base_k]),
                                 (points[i + base_j + base_k],
                                  points[i + base_j + next_k],
                                  points[i + next_j + next_k]),
                                 (points[next_i + base_j + base_k],
                                  points[next_i + next_j + base_k],
                                  points[next_i + next_j + next_k]),
                                 (points[next_i + base_j + base_k],
                                  points[next_i + next_j + next_k],
                                  points[next_i + base_j + next_k]),
                                 # z-x
                                 (points[i + base_j + base_k],
                                  points[next_i + base_j + next_k],
                                  points[i + base_j + next_k]),
                                 (points[i + base_j + base_k],
                                  points[next_i + base_j + base_k],
                                  points[next_i + base_j + next_k]),
                                 (points[i + next_j + base_k],
                                  points[i + next_j + next_k],
                                  points[next_i + next_j + next_k]),
                                 (points[i + next_j + base_k],
                                  points[next_i + next_j + next_k],
                                  points[next_i + next_j + base_k]))
        points_on_face[index, 0::4] = points[i + base_j + base_k]
        points_on_face[index, 1::4] = points[i + base_j + base_k]
        points_on_face[index, 2::4] = points[next_i + next_j + next_k]
        points_on_face[index, 3::4] = points[next_i + next_j + next_k]
        i += 1
    normals = get_normals(face_triangles)
    return (normals, points_on_face)


def get_indices_from_coordinate(coordinate, normals, points_on_face):
    vectors = points_on_face - coordinate
    dotprods = numpy.einsum('ijk, ijk->ij', vectors, normals)
    is_coord_back_of_face = numpy.greater_equal(dotprods, [0])
    return numpy.where(numpy.all(is_coord_back_of_face, 1))[0]


if __name__ == "__main__":
    grid_shape = [37, 25, 54]
    num_of_points = numpy.multiply.reduce(grid_shape)
    delta = [1.0, 1.0, 1.0]
    points = numpy.zeros([num_of_points, 3])
    i, j, k = (0, 0, 0)
    start = time.clock()
    for index in range(num_of_points):
        if i == grid_shape[0]:
            i = 0
            j += 1
        if j == grid_shape[1]:
            j = 0
            k += 1
        points[index] = (delta[0] * i, delta[1] * j, delta[2] * k)
        i += 1
    lap1 = time.clock()
    normals, points_on_face = get_cells(points, grid_shape)
    testpoint = numpy.random.rand(3) * 100
    lap2 = time.clock()
    indices = get_indices_from_coordinate(testpoint, normals, points_on_face)
    lap3 = time.clock()
    if len(indices) is 0:
        print(" ".join(["({0[0]}, {0[1]}, {0[2]})".format(testpoint),
                         "is not included in the cells."]))
    for index in indices:
        print(" ".join(["({0[0]}, {0[1]}, {0[2]})".format(testpoint),
                         "is included in cell {}".format(index)]))
    print("lap1: {}\nlap2: {}\nlap3: {}".format(lap1 - start,
                                                 lap2 - lap1,
                                                 lap3 - lap2))

次回、壁面衝突判定の予定