Определить основные направления в эллипсоидах относительно исходного кадра python - PullRequest
Новая
       6

Определить основные направления в эллипсоидах относительно исходного кадра python

0 голосов
/ 04 марта 2020

Моя конечная цель - использовать библиотеку FCL для обнаружения столкновения между эллипсоидом и коробкой в ​​3D. Для эллипсоида я начну с матрицы $ S $ из определения $ (x- c) 'S (x- c) \ leq1 $ и его центроида $ c $. Чтобы использовать библиотеку, мне нужно инициализировать объект эллипсоида с его радиусами, а затем перевести и повернуть его. Я пытаюсь использовать np.linalg.svd, чтобы получить $ (x- c) 'U \ Lambda U' (x- c) \ leq1 $, чтобы извлечь матрицу вращения $ U '$ и радиусы из $ \ Lambda $. Вот MWE: 


import numpy as np
from scipy.spatial.transform import Rotation as R
from scipy.linalg import svd
import fcl


# this is just dummy code to create the S matrix, assume it's given to me
r = R.from_euler('z', 45, degrees=True)
try:
    #depends on the version of scipy
    dcm = r.as_matrix()
except:
    dcm = r.as_dcm()
D = np.array([1./2.0**2, 1./1.0**2, 1.0/0.8**2]) # the radii
S = dcm.dot(np.diag(D)).dot(dcm.transpose())
e_center = np.array([-0.99-1./np.sqrt(2), 1+1./np.sqrt(2), 0.0])

# here the action starts
Rot, radii, RotT = svd(S)
# this is meaningless because the order changes
e1 = fcl.Ellipsoid(1./np.sqrt(radii[0]), 1./np.sqrt(radii[1]), 1./np.sqrt(radii[2]))
# meaningless because RotT and e_center (and radii) don't match
tf_e1 = fcl.Transform(RotT, e_center)
obj_e1 = fcl.CollisionObject(e1, tf_e1)

# what would've happened if I new the rotation order
e2 = fcl.Ellipsoid(1./np.sqrt(D[0]), 1./np.sqrt(D[1]), 1./np.sqrt(D[2]))
tf_e2 = fcl.Transform(dcm, e_center)
obj_e2 = fcl.CollisionObject(e2, tf_e2)

# box with center in origin and side lengths of 2, so it reaches +1/-1 in every dimension
b = fcl.Box(2,2,2)
tf_b = fcl.Transform()
obj_b = fcl.CollisionObject(b, tf_b)

# detect collision
request = fcl.CollisionRequest()
result = fcl.CollisionResult()

if( fcl.collide(obj_b, obj_e1, request, result) > 0):
    print('using svd: collision')
else:
    print('using svd: no collision')
if( fcl.collide(obj_b, obj_e2, request, result) > 0):
    print('using the original order: collision')
else:
    print('using the original order: no collision')

Где вывод: 

using svd: no collision
using the original order: collision

Где ясно, что он должен был сталкиваться, как видно на рисунке. Коробка с эллипсом, вид сверху

Итак, вот где моя проблема. Алгоритм SVD изменяет порядок координат (от наивысшего к собственному значению), и он больше не основан на исходной задаче и на векторе переноса. Это похоже на алгоритм eig.

Как я могу справиться с этим, чтобы получить первоначальное значение? Или что я делаю не так, чтобы вращать эллипс?

Спасибо

...