Pythonで３次元の衝突判定を実装したが特定の条件で動作しない

Question

前提・実現したいこと

現在、Python3.7で物理シミュレータを制作しているのですが、平面(三角ポリゴン)と点の衝突を判定しようとして詰まってしまいました。

具体的に、三角ポリゴンを構成する三点の座標と、点の持つ現在の座標から次フレーム後の座標を入力として衝突を計算し、衝突する際には座標を返すという仕組みを作りました。

計算式やアイデアについては全てここから拝借しました。
この参考サイトに掲載されているプログラムをpythonに書き換えて実装してみました。お見苦しい点があるかもしれませんが、以下がソースコードです。

def rayIntersectsTriangle(origin, ray, v0, v1, v2):
    edge1 = v1 - v0
    edge2 = v2 - v0
    denominator = LA.det([edge1.tolist(), 
                          edge2.tolist(), 
                          (ray*-1).tolist()])  ## クラメルの共通分母
    if denominator > 0:
        u = LA.det([(origin - v0).tolist(), edge2.tolist(), (ray*-1).tolist()])  /  denominator
        if (u >= 0) & (u <= 1):
            v = LA.det([edge1.tolist(), (origin - v0).tolist(), (ray*-1).tolist()])  /  denominator
            if (v >= 0) & (u+v <= 1):
                t = LA.det([edge1.tolist(), edge2.tolist(), (origin - v0).tolist()])  /  denominator
                intersection = origin + (ray * t)
                return intersection
            else:
                return None
        else:
            return None
    else:
        return None

計算自体は上手く行ってるようで、条件が合えば期待通りの結果を得ることができました。
例えば、以下の条件でrayIntersectsTriangle関数を呼んだ場合は上手く行きます。

import numpy as np
import numpy.linalg as LA

%matplotlib notebook
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt

## POLIGON(x, z, y)
tri = np.array([[1,1,5.2],
                [1,3,5],
                [3,1,4.8]])

## PARTICLE(x, z, y)
origin      = np.array([2,1.5,5.5])
origin_next = np.array([2,1.5,4.5])
ray = np.abs(origin_next - origin)

fig = plt.figure()
ax = Axes3D(fig)
ax.set_xlabel("x", size = 14)
ax.set_ylabel("z", size = 14)
ax.set_zlabel("y", size = 14)

## Poligon
plt.plot(*zip(tri[0],tri[2], tri[1],tri[0]), 'k-', lw=2)

## RAY
plt.plot(*origin.reshape(3, 1), "o", color='blue')
plt.plot(*origin_next.reshape(3, 1), "o", color='blue')
plt.plot(*zip(origin,origin_next), 'r-', lw=2)

## collision
p = rayIntersectsTriangle(origin, ray, *tri)
if p is not None:
    plt.plot(*p.reshape(3, 1), "o", color='green')
else:
    print("return None")
plt.show()

実行結果です。緑の点が交差している点です。

しかし、originとorigin_nextの関係がorigin < origin_nextの関係になるとうまく動作しません(要検証)。
例えばx成分をorigin < origin_nextの関係にし実行すると

## PARTICLE(x, z, y)
origin      = np.array([2,1.5,5.5])
origin_next = np.array([2.5,1.5,4.5])

このようになる

何かヒントになることや、サイトのURLだけでもいいので反応をくださるとありがたいです。
原因を詳しく追及できてない点については本当に心苦しいですが、皆様のお力をお借りしたいと思ってます。
どうかよろしくお願いいたします。

補足情報（FW/ツールのバージョンなど）

• Python 3.7
• Numpy 1.19.3
• Matplotlib 3.3.3

Comments

[fana]

Python知らんし，超あてずっぽうだけども，文面から，正負を無視した記述がどこかに紛れ込んでいるのだろうと想像．たとえば，これ↓とか，字面的にあやしく見える（絶対値？　なぜ？）
ray = np.abs(origin_next - origin)

[ryoike]

おっしゃる通りです。方向ベクトルを絶対値にしていました。ベストアンサーとしたいので回答の方にも投稿をお願いできないでしょうか？

[fana]

書いてみました．

Answer 1

（私自身は Python コードを正確に読めませんが）

p = rayIntersectsTriangle(origin, ray, *tri)

という形で，線分情報をoriginとrayで示す形であるならば，
当然，ray は origin_nextのorignから見た相対座標を示すものであろう，という点と，

座標値の前後関係により動作の成否が変わるという現象の話とに基づき，
原因は

ray = np.abs(origin_next - origin)

であろうと推測されます．
（相対座標をベクトルで示すときに絶対値を取るのはおかしい）