I have two vectors as Python lists and an angle. E.g.:
v = [3,5,0]
axis = [4,4,1]
theta = 1.2 #radian
What is the best/easiest way to get the resulting vector when rotating the v vector around the axis?
The rotation should appear to be counter clockwise for an observer to whom the axis vector is pointing. This is called the right hand rule
Using the Euler-Rodrigues formula:
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]
A one-liner, with numpy/scipy functions.
We use the following:
let a be the unit vector along axis, i.e. a = axis/norm(axis)
and A = I × a be the skew-symmetric matrix associated to a, i.e. the cross product of the identity matrix with a
then M = exp(θ A) is the rotation matrix.
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 here) computes the taylor series of the exponential:
\sum_{k=0}^{20} \frac{1}{k!} (θ A)^k
, so it's time expensive, but readable and secure.
It can be a good way if you have few rotations to do but a lot of vectors.
I just wanted to mention that if speed is required, wrapping unutbu's code in scipy's weave.inline and passing an already existing matrix as a parameter yields a 20-fold decrease in the running time.
The code (in 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]])
The 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]
Here is an elegant method using quaternions that are blazingly fast; I can calculate 10 million rotations per second with appropriately vectorised numpy arrays. It relies on the quaternion extension to numpy found here.
Quaternion Theory:
A quaternion is a number with one real and 3 imaginary dimensions usually written as q = w + xi + yj + zk where 'i', 'j', 'k' are imaginary dimensions. Just as a unit complex number 'c' can represent all 2d rotations by c=exp(i * theta), a unit quaternion 'q' can represent all 3d rotations by q=exp(p), where 'p' is a pure imaginary quaternion set by your axis and angle.
We start by converting your axis and angle to a quaternion whose imaginary dimensions are given by your axis of rotation, and whose magnitude is given by half the angle of rotation in radians. The 4 element vectors (w, x, y, z) are constructed as follows:
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)
First, a numpy array of 4 elements is constructed with the real component w=0 for both the vector to be rotated vector and the rotation axis rot_axis. The axis angle representation is then constructed by normalizing then multiplying by half the desired angle theta. See here for why half the angle is required.
Now create the quaternions v and qlog using the library, and get the unit rotation quaternion q by taking the exponential.
vec = quat.quaternion(*v)
qlog = quat.quaternion(*axis_angle)
q = np.exp(qlog)
Finally, the rotation of the vector is calculated by the following operation.
v_prime = q * vec * np.conjugate(q)
print(v_prime) # quaternion(0.0, 2.7491163, 4.7718093, 1.9162971)
Now just discard the real element and you have your rotated vector!
v_prime_vec = v_prime.imag # [2.74911638 4.77180932 1.91629719] as a numpy array
Note that this method is particularly efficient if you have to rotate a vector through many sequential rotations, as the quaternion product can just be calculated as q = q1 * q2 * q3 * q4 * ... * qn and then the vector is only rotated by 'q' at the very end using v' = q * v * conj(q).
This method gives you a seamless transformation between axis angle <---> 3d rotation operator simply by exp and log functions (yes log(q) just returns the axis-angle representation!). For further clarification of how quaternion multiplication etc. work, see here
Take a look at http://vpython.org/contents/docs/visual/VisualIntro.html.
It provides a vector class which has a method A.rotate(theta,B). It also provides a helper function rotate(A,theta,B) if you don't want to call the method on A.
http://vpython.org/contents/docs/visual/vector.html
Use scipy's Rotation.from_rotvec(). The argument is the rotation vector (a unit vector) multiplied by the rotation angle in rads.
from scipy.spatial.transform import Rotation
from numpy.linalg import norm
v = [3, 5, 0]
axis = [4, 4, 1]
theta = 1.2
axis = axis / norm(axis) # normalize the rotation vector first
rot = Rotation.from_rotvec(theta * axis)
new_v = rot.apply(v)
print(new_v) # results in [2.74911638 4.77180932 1.91629719]
There are several more ways to use Rotation based on what data you have about the rotation:
from_quat Initialized from quaternions.
from_dcm Initialized from direction cosine matrices.
from_euler Initialized from Euler angles.
Off-topic note: One line code is not necessarily better code as implied by some users.
I made a fairly complete library of 3D mathematics for Python{2,3}. It still does not use Cython, but relies heavily on the efficiency of numpy. You can find it here with pip:
python[3] -m pip install math3d
Or have a look at my gitweb http://git.automatics.dyndns.dk/?p=pymath3d.git and now also on github: https://github.com/mortlind/pymath3d .
Once installed, in python you may create the orientation object which can rotate vectors, or be part of transform objects. E.g. the following code snippet composes an orientation that represents a rotation of 1 rad around the axis [1,2,3], applies it to the vector [4,5,6], and prints the result:
import math3d as m3d
r = m3d.Orientation.new_axis_angle([1,2,3], 1)
v = m3d.Vector(4,5,6)
print(r * v)
The output would be
<Vector: (2.53727, 6.15234, 5.71935)>
This is more efficient, by a factor of approximately four, as far as I can time it, than the oneliner using scipy posted by B. M. above. However, it requires installation of my math3d package.
It can also be solved using quaternion theory:
def angle_axis_quat(theta, axis):
"""
Given an angle and an axis, it returns a quaternion.
"""
axis = np.array(axis) / np.linalg.norm(axis)
return np.append([np.cos(theta/2)],np.sin(theta/2) * axis)
def mult_quat(q1, q2):
"""
Quaternion multiplication.
"""
q3 = np.copy(q1)
q3[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3]
q3[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2]
q3[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1]
q3[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0]
return q3
def rotate_quat(quat, vect):
"""
Rotate a vector with the rotation defined by a quaternion.
"""
# Transfrom vect into an quaternion
vect = np.append([0],vect)
# Normalize it
norm_vect = np.linalg.norm(vect)
vect = vect/norm_vect
# Computes the conjugate of quat
quat_ = np.append(quat[0],-quat[1:])
# The result is given by: quat * vect * quat_
res = mult_quat(quat, mult_quat(vect,quat_)) * norm_vect
return res[1:]
v = [3, 5, 0]
axis = [4, 4, 1]
theta = 1.2
print(rotate_quat(angle_axis_quat(theta, axis), v))
# [2.74911638 4.77180932 1.91629719]
Disclaimer: I am the author of this package
While special classes for rotations can be convenient, in some cases one needs rotation matrices (e.g. for working with other libraries like the affine_transform functions in scipy). To avoid everyone implementing their own little matrix generating functions, there exists a tiny pure python package which does nothing more than providing convenient rotation matrix generating functions. The package is on github (mgen) and can be installed via pip:
pip install mgen
Example usage copied from the readme:
import numpy as np
np.set_printoptions(suppress=True)
from mgen import rotation_around_axis
from mgen import rotation_from_angles
from mgen import rotation_around_x
matrix = rotation_from_angles([np.pi/2, 0, 0], 'XYX')
matrix.dot([0, 1, 0])
# array([0., 0., 1.])
matrix = rotation_around_axis([1, 0, 0], np.pi/2)
matrix.dot([0, 1, 0])
# array([0., 0., 1.])
matrix = rotation_around_x(np.pi/2)
matrix.dot([0, 1, 0])
# array([0., 0., 1.])
Note that the matrices are just regular numpy arrays, so no new data-structures are introduced when using this package.
Using pyquaternion is extremely simple; to install it (while still in python), run in your console:
import pip;
pip.main(['install','pyquaternion'])
Once installed:
from pyquaternion import Quaternion
v = [3,5,0]
axis = [4,4,1]
theta = 1.2 #radian
rotated_v = Quaternion(axis=axis,angle=theta).rotate(v)
I needed to rotate a 3D model around one of the three axes {x, y, z} in which that model was embedded and this was the top result for a search of how to do this in numpy. I used the following simple function:
def rotate(X, theta, axis='x'):
'''Rotate multidimensional array `X` `theta` degrees around axis `axis`'''
c, s = np.cos(theta), np.sin(theta)
if axis == 'x': return np.dot(X, np.array([
[1., 0, 0],
[0 , c, -s],
[0 , s, c]
]))
elif axis == 'y': return np.dot(X, np.array([
[c, 0, -s],
[0, 1, 0],
[s, 0, c]
]))
elif axis == 'z': return np.dot(X, np.array([
[c, -s, 0 ],
[s, c, 0 ],
[0, 0, 1.],
]))
Related
For the 1D cosine transform the documentation is clear in here, and I can reproduce it easily:
The formula is:
and here it is reproduced manually, for example for the third harmonic:
import numpy as np
from scipy.fft import fft, dct
y = np.array([10,3,5])
# dct() call:
print(dct(y, 2))
# manual reproduction for k = 2:
N = 3
k = 2
n = np.array([0,1,2])
2 * (
y[0] * np.cos((np.pi*k*(2*n[0]+1))/(2*N)) +
y[1] * np.cos((np.pi*k*(2*n[1]+1))/(2*N)) +
y[2] * np.cos((np.pi*k*(2*n[2]+1))/(2*N)))
in both cases I get 9.
But there is no documentation for the 2D DCT, and my attempts at hacking the formula with a toy matrix are not working out:
Compare:
z = np.array([[ 2, 3 ],
[ 10, 15]])
dct(z, axis=0) # dct() call
to for instance:
N = 2
M = 2
k = 0
l = 0
n = np.array([0,1])
m = np.array([0,1])
M*N * (
z[0,0] * np.cos((np.pi*k*(2*n[0]+1))/(2*N)) * np.cos((np.pi*l*(2*m[0]+1))/(2*M)) +
z[0,1] * np.cos((np.pi*k*(2*n[0]+1))/(2*N)) * np.cos((np.pi*l*(2*m[1]+1))/(2*M)) +
z[1,0] * np.cos((np.pi*k*(2*n[1]+1))/(2*N)) * np.cos((np.pi*l*(2*m[0]+1))/(2*M)) +
z[1,1] * np.cos((np.pi*k*(2*n[1]+1))/(2*N)) * np.cos((np.pi*l*(2*m[1]+1))/(2*M))
)
for the first coefficient.
Can anyone help me reconcile the output of dct() with my attempted manual calculation?
I guess the formula is not...
but it would be really easy to correct if I could get the same output manually for one of the coefficients on the example matrix above.
You can't find the formula for the multidimensional mode because the function doesn't do multidimensional cosine transforms. The axis keyword should be suspicious: in NumPy, SciPy it typically determines the direction along which lower-dimensional operations should be performed.
In other words, dct(z, axis=0) is just a columnwise 1d cosine transform:
import numpy as np
from scipy.fft import dct
z = np.array([[ 2, 3],
[ 10, 15]])
print(dct(z, axis=0)) # dct() call
print(np.array([dct(column) for column in z.T]).T)
# both outputs
# [[ 24. 36. ]
# [-11.3137085 -16.97056275]]
Note the two transposes on the last line: all that it did was first loop over the array to slice it according to columns, then join them again columnwise. The latter would probably be better spelled out as
res = np.stack([dct(column) for column in z.T], axis=1)
I have written code that calculates the angle between two vectors. However the way in which is does this is to start with two vectors, rotate each according to some euler angles calculated in a separate program, then calculate the angle between the vectors.
Up until now I have been working with a use case that means both starting vectors are (0,0,1) that makes life super easy. I could just take one set of euler angles away from the other and then calculate the angle between 0,0,1 and the vector that had been rotated by the difference. It meant I could plot nice distribution plots and vector diagrams because everything was normalised to 0,0,1. (I have 1000s of these vectors for the record).
No I am trying to write in a function that would allow for a use case where the two starting vectors are not on 0,0,1. I figured the easiest way to do this would be to calculate direction of the vector relative to 0,0,1 and after calculating the position of the vector just rotate by the precalculated offsets. (this might be a stupid way to do it, if it is please tell me).
MY current code works for a case where a vector is 0,1,0 but then breaks down if i start entering random numbers.
import numpy as np
import math
def RotationMatrix(axis, rotang):
"""
This uses Euler-Rodrigues formula.
"""
#Input taken in degrees, here we change it to radians
theta = rotang * 0.0174532925
axis = np.asarray(axis)
#Ensure axis is a unit vector
axis = axis/math.sqrt(np.dot(axis, axis))
#calclating a, b, c and d according to euler-rodrigues forumla requirments
a = math.cos(theta/2)
b, c, d = axis*math.sin(theta/2)
a2, b2, c2, d2 = 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 the rotation matrix
return np.array([[a2+b2-c2-d2, 2*(bc-ad), 2*(bd+ac)],
[2*(bc+ad), a2+c2-b2-d2, 2*(cd-ab)],
[2*(bd-ac), 2*(cd+ab), a2+d2-b2-c2]])
def ApplyRotationMatrix(vector, rotationmatrix):
"""
This function take the output from the RotationMatrix function and
uses that to apply the rotation to an input vector
"""
a1 = (vector[0] * rotationmatrix[0, 0]) + (vector[1] * rotationmatrix[0, 1]) + (vector[2] * rotationmatrix[0, 2])
b1 = (vector[0] * rotationmatrix[1, 0]) + (vector[1] * rotationmatrix[1, 1]) + (vector[2] * rotationmatrix[1, 2])
c1 = (vector[0] * rotationmatrix[2, 0]) + (vector[1] * rotationmatrix[2, 1]) + (vector[2] * rotationmatrix[2, 2])
return np.array((a1, b1, c1)
'''
Functions for Calculating the angles of 3D vectors relative to one another
'''
def CalculateAngleBetweenVector(vector, vector2):
"""
Does what it says on the tin, outputs an angle in degrees between two input vectors.
"""
dp = np.dot(vector, vector2)
maga = math.sqrt((vector[0] ** 2) + (vector[1] ** 2) + (vector[2] ** 2))
magb = math.sqrt((vector2[0] ** 2) + (vector2[1] ** 2) + (vector2[2] ** 2))
magc = maga * magb
dpmag = dp / magc
#These if statements deal with rounding errors of floating point operations
if dpmag > 1:
error = dpmag - 1
print('error = {}, do not worry if this number is very small'.format(error))
dpmag = 1
elif dpmag < -1:
error = 1 + dpmag
print('error = {}, do not worry if this number is very small'.format(error))
dpmag = -1
angleindeg = ((math.acos(dpmag)) * 180) / math.pi
return angleindeg
def CalculateAngleAroundZ(Vector):
X,Y,Z = Vector[0], Vector[1], Vector[2]
AngleAroundZ = math.atan2(Y, X)
AngleAroundZdeg = (AngleAroundZ*180)/math.pi
return AngleAroundZdeg
def CalculateAngleAroundX(Vector):
X,Y,Z = Vector[0], Vector[1], Vector[2]
AngleAroundZ = math.atan2(Y, Z)
AngleAroundZdeg = (AngleAroundZ*180)/math.pi
return AngleAroundZdeg
def CalculateAngleAroundY(Vector):
X,Y,Z = Vector[0], Vector[1], Vector[2]
AngleAroundZ = math.atan2(X, Z)
AngleAroundZdeg = (AngleAroundZ*180)/math.pi
return AngleAroundZdeg
V1 = (0,0,1)
V2 = (3,5,4)
Xoffset = (CalculateAngleAroundX(V2))
Yoffset = (CalculateAngleAroundY(V2))
Zoffset = (CalculateAngleAroundZ(V2))
XRM = RotationMatrix((1,0,0), (Xoffset * 1))
YRM = RotationMatrix((0,1,0), (Yoffset * 1))
ZRM = RotationMatrix((0,0,1), (Zoffset * 1))
V2 = V2 / np.linalg.norm(V2)
V2X = ApplyRotationMatrix(V2, XRM)
V2XY = ApplyRotationMatrix(V2X, YRM)
V2XYZ = ApplyRotationMatrix(V2XY, ZRM)
print(V2XYZ)
print(CalculateAngleBetweenVector(V1, V2XYZ))
Any advice to fix this problem will be much appreciated.
I'm not sure to fully understand what you need but if it is to compute the angle between two vectors in space you can use the formula:
where a.b is the scalar product and theta is the angle between vectors.
thus your function CalculateAngleBetweenVector becomes:
def CalculateAngleBetweenVector(vector, vector2):
return math.acos(np.dot(vector,vector2)/(np.linalg.norm(vector)* np.linalg.norm(vector2))) * 180 /math.pi
You can also simplify your ApplyRotationMatrix function:
def ApplyRotationMatrix(vector, rotationmatrix):
"""
This function take the output from the RotationMatrix function and
uses that to apply the rotation to an input vector
"""
return rotationmatrix # vector
the # symbol is the matrix product
Hope this will help you. Feel free to precise your request if this is not helpfull.
Im an idiot I just needed to do the cross product and the dot product and rotate by the dot product *-1 around the cross product.
I'm trying to do a particle in a box simulation with no potential field. Took me some time to find out that simple explicit and implicit methods break unitary time evolution so I resorted to crank-nicolson, which is supposed to be unitary. But when I try it I find that it still is not so. I'm not sure what I'm missing.. The formulation I used is this:
where T is the tridiagonal Toeplitz matrix for the second derivative wrt x and
The system simplifies to
The A and B matrices are:
I just solve this linear system for using the sparse module. The math makes sense and I found the same numeric scheme in some papers so that led me to believe my code is where the problem is.
Here's my code so far:
import numpy as np
import matplotlib.pyplot as plt
from scipy.linalg import toeplitz
from scipy.sparse.linalg import spsolve
from scipy import sparse
# Spatial discretisation
N = 100
x = np.linspace(0, 1, N)
dx = x[1] - x[0]
# Time discretisation
K = 10000
t = np.linspace(0, 10, K)
dt = t[1] - t[0]
alpha = (1j * dt) / (2 * (dx ** 2))
A = sparse.csc_matrix(toeplitz([1 + 2 * alpha, -alpha, *np.zeros(N-4)]), dtype=np.cfloat) # 2 less for both boundaries
B = sparse.csc_matrix(toeplitz([1 - 2 * alpha, alpha, *np.zeros(N-4)]), dtype=np.cfloat)
# Initial and boundary conditions (localized gaussian)
psi = np.exp((1j * 50 * x) - (200 * (x - .5) ** 2))
b = B.dot(psi[1:-1])
psi[0], psi[-1] = 0, 0
for index, step in enumerate(t):
# Within the domain
psi[1:-1] = spsolve(A, b)
# Enforce boundaries
# psi[0], psi[N - 1] = 0, 0
b = B.dot(psi[1:-1])
# Square integration to show if it's unitary
print(np.trapz(np.abs(psi) ** 2, dx))
You are relying on the Toeplitz constructor to produce a symmetric matrix, so that the entries below the diagonal are the same as above the diagonal. However, the documentation for scipy.linalg.toeplitz(c, r=None) says not "transpose", but
*"If r is not given, r == conjugate(c) is assumed."
so that the resulting matrix is self-adjoint. In this case this means that the entries above the diagonal have their sign switched.
It makes no sense to first construct a dense matrix and then extract a sparse representation. Construct it as sparse tridiagonal matrix from the start, using scipy.sparse.diags
A = sparse.diags([ (N-3)*[-alpha], (N-2)*[1+2*alpha], (N-3)*[-alpha]], [-1,0,1], format="csc");
B = sparse.diags([ (N-3)*[ alpha], (N-2)*[1-2*alpha], (N-3)*[ alpha]], [-1,0,1], format="csc");
I have two separate vectors of 3D data points that represent curves and I'm plotting these as scatter data in a 3D plot with matplotlib.
Both the vectors start at the origin, and both are of unit length. The curves are similar to each other, however, there is typically a rotation between the two curves (for test purposes, I've actually being using one curve and applying a rotation matrix to it to create the second curve).
I want to align the two curves so that they line up in 3D e.g. rotate curve b, so that its start and end points line up with curve a. I've been trying to do this by subtracting the final point from the first, to get a direction vector representing the straight line from the start to the end of each curve, converting these to unit vectors and then calculating the cross and dot products and using the methodology outlined in this answer (https://math.stackexchange.com/a/476311/357495) to calculate a rotation matrix.
However, when I do this, the calculated rotation matrix is wrong and I'm not sure why?
My code is below (I'm using Python 2.7):
# curve_1, curve_2 are arrays of 3D points, of the same length (both start at the origin)
curve_vec_1 = (curve_1[0] - curve_1[-1]).reshape(3,1)
curve_vec_2 = (curve_2[index][0] - curve_2[index][-1]).reshape(3,1)
a,b = (curve_vec_1/ np.linalg.norm(curve_vec_1)).reshape(3), (curve_vec_2/ np.linalg.norm(curve_vec_2)).reshape(3)
v = np.cross(a,b)
c = np.dot(a,b)
s = np.linalg.norm(v)
I = np.identity(3)
vXStr = '{} {} {}; {} {} {}; {} {} {}'.format(0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0)
k = np.matrix(vXStr)
r = I + k + np.square(k) * ((1 -c)/(s**2))
for i in xrange(item.shape[0]):
item[i] = (np.dot(r, item[i]).reshape(3,1)).reshape(3)
In my test case, curve 2 is simply curve 1 with the following rotation matrix applied:
[[1 0 0 ]
[ 0 0.5 0.866]
[ 0 -0.866 0.5 ]]
(just a 60 degree rotation around the x axis).
The rotation matrix computed by my code to align the two vectors again is:
[[ 1. -0.32264329 0.27572962]
[ 0.53984249 1. -0.35320293]
[-0.20753816 0.64292975 1. ]]
The plot of the direction vectors for the two original curves (a and b in blue and green respectively) and the result of b transformed with the computed rotation matrix (red) is below. I'm trying to compute the rotation matrix to align the green vector to the blue.
Based on Daniel F's correction, here is a function that does what you want:
import numpy as np
def rotation_matrix_from_vectors(vec1, vec2):
""" Find the rotation matrix that aligns vec1 to vec2
:param vec1: A 3d "source" vector
:param vec2: A 3d "destination" vector
:return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2.
"""
a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3)
v = np.cross(a, b)
c = np.dot(a, b)
s = np.linalg.norm(v)
kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
rotation_matrix = np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2))
return rotation_matrix
Test:
vec1 = [2, 3, 2.5]
vec2 = [-3, 1, -3.4]
mat = rotation_matrix_from_vectors(vec1, vec2)
vec1_rot = mat.dot(vec1)
assert np.allclose(vec1_rot/np.linalg.norm(vec1_rot), vec2/np.linalg.norm(vec2))
Problem is here:
r = I + k + np.square(k) * ((1 -c)/(s**2))
np.square(k) squares each element of the matrix. You want np.matmul(k,k) or k # k which is the matrix multiplied by itself.
I'd also implement the side cases (especially s=0) mentioned in the comments of that answer or you will end up with errors for quite a few cases.
Based off of #Peter and #Daniel F's work. The above function worked for me, except for in cases of the same direction vector, where v would be a zero vector. I catch this here, and return the identity vector instead.
def rotation_matrix_from_vectors(vec1, vec2):
""" Find the rotation matrix that aligns vec1 to vec2
:param vec1: A 3d "source" vector
:param vec2: A 3d "destination" vector
:return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2.
"""
a, b = (vec1 / numpy.linalg.norm(vec1)).reshape(3), (vec2 / numpy.linalg.norm(vec2)).reshape(3)
v = numpy.cross(a, b)
if any(v): #if not all zeros then
c = numpy.dot(a, b)
s = numpy.linalg.norm(v)
kmat = numpy.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
return numpy.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2))
else:
return numpy.eye(3) #cross of all zeros only occurs on identical directions
One can use scipy for this, reproducing here #Peter answer with scipy Rotation see:
https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html?highlight=scipy%20spatial%20transform%20rotation#scipy.spatial.transform.Rotation
from scipy.spatial.transform import Rotation as R
import numpy as np
def get_rotation_matrix(vec2, vec1=np.array([1, 0, 0])):
"""get rotation matrix between two vectors using scipy"""
vec1 = np.reshape(vec1, (1, -1))
vec2 = np.reshape(vec2, (1, -1))
r = R.align_vectors(vec2, vec1)
return r[0].as_matrix()
vec1 = np.array([2, 3, 2.5])
vec2 = np.array([-3, 1, -3.4])
mat = get_rotation_matrix(vec1=vec1, vec2=vec2)
print(mat)
vec1_rot = mat.dot(vec1)
assert np.allclose(vec1_rot / np.linalg.norm(vec1_rot), vec2 / np.linalg.norm(vec2))
terveisin, Markus
I think if you do not have rotation axis, the rotation matrix is not unique.
I have built a small code that I want to use for solving eigenvalue problems involving large sparse matrices. It's working fine, all I want to do now is to set some elements in the sparse matrix to zero, i.e. the ones in the very top row (which corresponds to implementing boundary conditions). I can just adjust the column vectors (C0, C1, and C2) below to achieve that. However, I wondered if there is a more direct way. Evidently, NumPy indexing does not work with SciPy's sparse package.
import scipy.sparse as sp
import scipy.sparse.linalg as la
import numpy as np
import matplotlib.pyplot as plt
#discretize x-axis
N = 11
x = np.linspace(-5,5,N)
print(x)
V = x * x / 2
h = len(x)/(N)
hi2 = 1./(h**2)
#discretize Schroedinger Equation, i.e. build
#banded matrix from difference equation
C0 = np.ones(N)*30. + V
C1 = np.ones(N) * -16.
C2 = np.ones(N) * 1.
diagonals = np.array([-2,-1,0,1,2])
H = sp.spdiags([C2, C1, C0,C1,C2],[-2,-1,0,1,2], N, N)
H *= hi2 * (- 1./12.) * (- 1. / 2.)
#solve for eigenvalues
EV = la.eigsh(H,return_eigenvectors = False)
#check structure of H
plt.figure()
plt.spy(H)
plt.show()
This is a visualisation of the matrix that is build by the code above. I want so set the elements in the first row zero.
As suggested in the comments, I'll post the answer that I found to my own question. There are several matrix classes in in SciPy's sparse package, they are listed here. One can convert sparse matrices from one class to another. So for what I need to do, I choose to convert my sparse matrix to the class csr_matrix, simply by
H = sp.csr_matrix(H)
Then I can set the elements in the first row to 0 by using the regular NumPy notation:
H[0,0] = 0
H[0,1] = 0
H[0,2] = 0
For completeness, I post the full modified code snippet below.
#SciPy Sparse linear algebra takes care of sparse matrix computations
#http://docs.scipy.org/doc/scipy/reference/sparse.linalg.html
import scipy.sparse as sp
import scipy.sparse.linalg as la
import numpy as np
import matplotlib.pyplot as plt
#discretize x-axis
N = 1100
x = np.linspace(-100,100,N)
V = x * x / 2.
h = len(x)/(N)
hi2 = 1./(h**2)
#discretize Schroedinger Equation, i.e. build
#banded matrix from difference equation
C0 = np.ones(N)*30. + V
C1 = np.ones(N) * -16.
C2 = np.ones(N) * 1.
H = sp.spdiags([C2, C1, C0, C1, C2],[-2,-1,0,1,2], N, N)
H *= hi2 * (- 1./12.) * (- 1. / 2.)
H = sp.csr_matrix(H)
H[0,0] = 0
H[0,1] = 0
H[0,2] = 0
#check structure of H
plt.figure()
plt.spy(H)
plt.show()
EV = la.eigsh(H,return_eigenvectors = False)
Using lil_matrix is much more efficient in scipy to change elements than simple numpy method.
H = sp.csr_matrix(H)
HL = H.tolil()
HL[1,1] = 5 # same as the numpy indexing notation
print HL
print HL.todense() # if numpy style matrix is required
H = HL.tocsr() # if csr is required