Rotation d'un vecteur 3D ?

Question

J'ai deux vecteurs sous forme de listes Python et un angle. Par exemple :

v = [3,5,0]
axis = [4,4,1]
theta = 1.2 #radian

Quelle est la meilleure façon d'obtenir le vecteur résultant lors de la rotation du vecteur v autour de l'axe ?

La rotation doit apparaître dans le sens inverse des aiguilles d'une montre pour un observateur vers lequel pointe le vecteur de l'axe. C'est ce qu'on appelle le règle de la main droite

Answer 1

L'utilisation de la Formule d'Euler-Rodrigues :

import numpy as np
import math

def rotation_matrix(axis, theta):
    """
    Return the rotation matrix associated with counterclockwise rotation about
    the given axis by theta radians.
    """
    axis = np.asarray(axis)
    axis = axis / math.sqrt(np.dot(axis, axis))
    a = math.cos(theta / 2.0)
    b, c, d = -axis * math.sin(theta / 2.0)
    aa, bb, cc, dd = a * a, b * b, c * c, d * d
    bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
    return np.array([[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)],
                     [2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)],
                     [2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]])

v = [3, 5, 0]
axis = [4, 4, 1]
theta = 1.2 

print(np.dot(rotation_matrix(axis, theta), v)) 
# [ 2.74911638  4.77180932  1.91629719]

Answer 2

Un one-liner, avec des fonctions numpy/scipy.

Nous utilisons les éléments suivants :

laisser a est le vecteur unitaire le long de axe , c'est-à-dire a = axis/norm(axis)
et A = I × a est la matrice asymétrique associée à a c'est-à-dire le produit croisé de la matrice identité et de la matrice a

puis M = exp(θ A) est la matrice de rotation.

from numpy import cross, eye, dot
from scipy.linalg import expm, norm

def M(axis, theta):
    return expm(cross(eye(3), axis/norm(axis)*theta))

v, axis, theta = [3,5,0], [4,4,1], 1.2
M0 = M(axis, theta)

print(dot(M0,v))
# [ 2.74911638  4.77180932  1.91629719]

expm (code ici) calcule la série de Taylor de l'exponentielle :
\sum_{k=0}^{20} \frac{1}{k!} (θ A)^k Il est donc coûteux en temps, mais lisible et sécurisé. Cela peut être un bon moyen si vous avez peu de rotations à faire mais beaucoup de vecteurs.

Answer 3

Je voulais juste mentionner que si la vitesse est nécessaire, envelopper le code de unutbu dans weave.inline de scipy et passer une matrice déjà existante comme paramètre permet de diviser par 20 le temps d'exécution.

Le code (dans rotation_matrix_test.py) :

import numpy as np
import timeit

from math import cos, sin, sqrt
import numpy.random as nr

from scipy import weave

def rotation_matrix_weave(axis, theta, mat = None):
    if mat == None:
        mat = np.eye(3,3)

    support = "#include <math.h>"
    code = """
        double x = sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
        double a = cos(theta / 2.0);
        double b = -(axis[0] / x) * sin(theta / 2.0);
        double c = -(axis[1] / x) * sin(theta / 2.0);
        double d = -(axis[2] / x) * sin(theta / 2.0);

        mat[0] = a*a + b*b - c*c - d*d;
        mat[1] = 2 * (b*c - a*d);
        mat[2] = 2 * (b*d + a*c);

        mat[3*1 + 0] = 2*(b*c+a*d);
        mat[3*1 + 1] = a*a+c*c-b*b-d*d;
        mat[3*1 + 2] = 2*(c*d-a*b);

        mat[3*2 + 0] = 2*(b*d-a*c);
        mat[3*2 + 1] = 2*(c*d+a*b);
        mat[3*2 + 2] = a*a+d*d-b*b-c*c;
    """

    weave.inline(code, ['axis', 'theta', 'mat'], support_code = support, libraries = ['m'])

    return mat

def rotation_matrix_numpy(axis, theta):
    mat = np.eye(3,3)
    axis = axis/sqrt(np.dot(axis, axis))
    a = cos(theta/2.)
    b, c, d = -axis*sin(theta/2.)

    return np.array([[a*a+b*b-c*c-d*d, 2*(b*c-a*d), 2*(b*d+a*c)],
                  [2*(b*c+a*d), a*a+c*c-b*b-d*d, 2*(c*d-a*b)],
                  [2*(b*d-a*c), 2*(c*d+a*b), a*a+d*d-b*b-c*c]])

Le timing :

>>> import timeit
>>> 
>>> setup = """
... import numpy as np
... import numpy.random as nr
... 
... from rotation_matrix_test import rotation_matrix_weave
... from rotation_matrix_test import rotation_matrix_numpy
... 
... mat1 = np.eye(3,3)
... theta = nr.random()
... axis = nr.random(3)
... """
>>> 
>>> timeit.repeat("rotation_matrix_weave(axis, theta, mat1)", setup=setup, number=100000)
[0.36641597747802734, 0.34883809089660645, 0.3459300994873047]
>>> timeit.repeat("rotation_matrix_numpy(axis, theta)", setup=setup, number=100000)
[7.180983066558838, 7.172032117843628, 7.180462837219238]

Answer 4

Voici une méthode élégante utilisant des quaternions qui sont extrêmement rapides ; je peux calculer 10 millions de rotations par seconde avec des tableaux numpy vectorisés de manière appropriée. Elle s'appuie sur l'extension quaternion de numpy que l'on a trouvée aquí .

Théorie des quaternions : Un quaternion est un nombre à une dimension réelle et trois dimensions imaginaires qui s'écrit généralement comme suit q = w + xi + yj + zk où "i", "j", "k" sont des dimensions imaginaires. De même qu'un nombre complexe unitaire "c" peut représenter toutes les rotations 2d par c=exp(i * theta) un quaternion unitaire "q" peut représenter toutes les rotations 3D par q=exp(p) où "p" est un quaternion imaginaire pur défini par votre axe et votre angle.

Nous commençons par convertir votre axe et votre angle en un quaternion dont les dimensions imaginaires sont données par votre axe de rotation, et dont la magnitude est donnée par la moitié de l'angle de rotation en radians. Les vecteurs à 4 éléments (w, x, y, z) sont construits comme suit :

import numpy as np
import quaternion as quat

v = [3,5,0]
axis = [4,4,1]
theta = 1.2 #radian

vector = np.array([0.] + v)
rot_axis = np.array([0.] + axis)
axis_angle = (theta*0.5) * rot_axis/np.linalg.norm(rot_axis)

Tout d'abord, un tableau numpy de 4 éléments est construit avec la composante réelle w=0 pour le vecteur à faire tourner vector et l'axe de rotation rot_axis . La représentation de l'angle de l'axe est ensuite construite en normalisant puis en multipliant par la moitié l'angle souhaité theta . Voir aquí pour expliquer pourquoi la moitié de l'angle est nécessaire.

Créez maintenant les quaternions v y qlog en utilisant la bibliothèque, et obtenir le quaternion de rotation unitaire q en prenant l'exponentielle.

vec = quat.quaternion(*v)
qlog = quat.quaternion(*axis_angle)
q = np.exp(qlog)

Enfin, la rotation du vecteur est calculée par l'opération suivante.

v_prime = q * vec * np.conjugate(q)

print(v_prime) # quaternion(0.0, 2.7491163, 4.7718093, 1.9162971)

Il suffit maintenant d'écarter l'élément réel pour obtenir un vecteur rotatif !

v_prime_vec = v_prime.imag # [2.74911638 4.77180932 1.91629719] as a numpy array

Notez que cette méthode est particulièrement efficace si vous devez faire pivoter un vecteur par de nombreuses rotations séquentielles, car le produit quaternaire peut simplement être calculé comme q = q1 * q2 * q3 * q4 * ... * qn, puis le vecteur n'est tourné que de 'q' à la toute fin en utilisant v' = q * v * conj(q).

Cette méthode permet une transformation transparente entre l'angle de l'axe <---> l'opérateur de rotation 3D simplement par exp y log fonctions (oui log(q) ne renvoie que la représentation de l'angle de l'axe !) Pour plus de précisions sur le fonctionnement de la multiplication des quaternions, etc. aquí

Answer 5

Jetez un coup d'œil à http://vpython.org/contents/docs/visual/VisualIntro.html .

Il fournit un vector qui possède une méthode A.rotate(theta,B) . Il fournit également une fonction d'aide rotate(A,theta,B) si vous ne voulez pas appeler la méthode sur A .

http://vpython.org/contents/docs/visual/vector.html