Develop Reference

Triangle rotation with undesired result

(Rewritten, compressed and partly corrected version)
I try some trivial rotation of a triangle in 3d to point its normal straight up without any distortion. I'm following this post calculate-rotation-matrix-to-align-vector, but for some reasons, my result is undesirable and I can't see the error in my code or my error in reasoning. My Python code as followed:
# defining the triangle with three points in 3d space (xyz)
v1 = [1,1,1]
v2 = [2,2,2]
v3 = [2,1,3]
# calculating the normal and converting to unit-vector
n = np.cross( np.subtract(v2,v1), np.subtract(v3,v1) )
a = n / np.sqrt( n[0]**2 + n[1]**2 + n[2]**2 )
b = [0,1,0]
v = np.cross(a,b)
s = np.sqrt(v.dot(v))
c = np.dot(a,b)
Vx = np.array([
[ 0 , -v[2], v[1] ],
[ v[2], 0 , -v[0]],
[-v[1], v[0], 0 ]
])
Vx2 = Vx.dot(Vx)
I = np.identity(3)
iv = (1/(1+c))
# single rotation matrix
R = np.add(I, np.add(Vx, np.multiply(Vx2,iv)))
Then calculating the centroid, the rotation matrix as described and also as separated matrices as explained here Rotating a Vector in 3D Space
cx = (v1[0]+v2[0]+v3[0]) / 3.0
cy = (v1[1]+v2[1]+v3[1]) / 3.0
cz = (v1[2]+v2[2]+v3[2]) / 3.0
v1t = [v1[0]-cx,v1[1]-cy,v1[2]-cz]
v2t = [v2[0]-cx,v2[1]-cy,v2[2]-cz]
v3t = [v3[0]-cx,v3[1]-cy,v3[2]-cz]
As well as the single rotation matrices:
rz = np.array([
[c,-s, 0],
[s, c, 0],
[0, 0, 1]
])
ry = np.array([
[ c, 0, s],
[ 0, 1, 0],
[-s, 0, c]
])
rx = np.array([
[1, 0, 0],
[0, c,-s],
[0, s, c]
])
rxyz = rx.dot(ry).dot(rz)
Now I apply the rotation, with R and rxyz:
p1 = R.dot(v1t)
p2 = R.dot(v2t)
p3 = R.dot(v3t)
p4 = rxyz.dot(v1t)
p5 = rxyz.dot(v2t)
p6 = rxyz.dot(v3t)
The result is now better than the first approach and the Y of p1 and p3 are the correctly same, but p2 is still wrong. And there still seems to be a distortion.
P1-3 (red)
[-0.20673470096224283, 1.1102230246251565e-16, -1.2299659828522118]
[-0.5865305980755144, -3.885780586188048e-16, 0.45993196570442385]
[0.7932652990377571, 1.1102230246251565e-16, 0.770034017147788]
P4-6 (blue)
[-1.1482080390363765, 0.30059830961195716, -0.38316381732390803]
[0.3040075530555323, -0.6336677067539932, -0.2481938771563051]
[0.8442004859808443, 0.3330693971420361, 0.6313576944802128]
The light gray triangle is the original translated to 0,0,0 and red and blue are the rotated ones.

Aligning two 3D objects in cartesian coordinates

I have two copies of the same molecule as .xyz file. This means that each atom is has X, Y and Z coordinates. However, you can rotate the molecule and obtain different coordinates for each atom, although the relative positions are the same and the molecule remains the same. I want to align the two molecules using three atoms as reference points. However, I am struggling to completely align the two molecules.
Firstly, I align both molecules by translation for a single atom. Then, I am doing two subsequent rotation using rotation matrices as explained elsewhere. For some reason, I need to take the negative of the cross product of both vectors and use a sinus instead of a cosinus to get both structures to be perfectly aligned (I discovered this after a lot of trial and error).
For the second rotation, I project both vectors I want to align on a plane defined by the rotation vector. This is necessary because I don't want to rotate along the cross product of the two vectors to align, since that would disalign the rest of the molecule. Instead, I rotate along the two already aligned vectors. The project allows me to find the angle in the plane between the two vectors, and thus the rotation necessary.
However, this code does not properly align the two molecules.
"group1[0]" contains the XYZ coordinates of the three atoms to align in a list. Likewise for "group2[0]" and the structure 2.
#Point 1: align the functional groups to the origin
O1 = np.array(coords1[group1[0][0]])
O2 = np.array(coords2[group2[0][0]])
mat_2 = np.zeros((len(atoms2), 3))
for ind, c in enumerate(coords1):
coords1[ind] = np.array(c) - O1
for ind, c in enumerate(coords2):
coords2[ind] = np.array(c) - O2
mat_2[ind] = coords2[ind]
#Point 2: align according to a first vector
v1 = np.array(coords1[group1[0][1]])#Since atom 1 is the origin, the coordinates is the vector already
v2 = np.array(coords2[group2[0][1]])#Since atom 1 is the origin, the coordinates is the vector already
v1 = v1/np.linalg.norm(v1)
v2 = v2/np.linalg.norm(v2)
#Let v be the axis of rotation
v = -np.cross(v1, v2)#why do I need a minus here?
if np.linalg.norm(v) != 0:
a = np.arccos(np.dot(v1, v2)/(np.linalg.norm(v1)*np.linalg.norm(v2)))
#c = np.dot(v1, v2)*np.cos(a)
c = np.dot(v1, v2)*np.sin(a)#The internet says cos, but this works perfectly
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
rot_mat = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) + vx + vx.dot(vx)*(1-c)/(1-c**2)
mat_2 = np.array(mat_2)
R_mat_rot = np.matmul(rot_mat, mat_2.T).T
else:
exit(0)
coords3 = R_mat_rot.copy()
#I get exactly what I want up until here
#Point 3: Rotate along atom2-atom1 (v1) to align the third atom
v = -v1.copy()
v2 = np.array(coords3[group2[0][2]]) - np.array(coords3[group2[0][0]]) #Since atom 1 is the origin, the coordinates is the vector already
v2 = v2/np.linalg.norm(v2)
v1 = np.array(coords1[group1[0][2]]) - np.array(coords1[group1[0][0]]) #Since atom 1 is the origin, the coordinates is the vector already
v1 = v1/np.linalg.norm(v1)
if np.linalg.norm(v) != 0:
#consider v to be the vector normal to a plane
#we want the projection of v1 and v2 unto that plane
vp1 = np.cross(v, np.cross(v1, v)) - np.array(coords1[group1[0][0]])
vp1 = vp1/np.linalg.norm(vp1)
vp2 = np.cross(v, np.cross(v2, v)) - np.array(coords3[group2[0][0]])
vp2 = vp2/np.linalg.norm(vp2)
#we find the angle between those vectors on the plane
a = np.arccos(np.dot(vp1, vp2))/(np.linalg.norm(vp1)*np.linalg.norm(vp2))
#rotation of that amount
c = np.dot(v1, v2)*np.cos(a)
vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
rot_mat = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) + vx + np.dot(vx, vx)*(1-c)/(1-c**2)
R_mat_rot = np.matmul(rot_mat, coords3.T).T
coords4 = R_mat_rot.copy()#Final coordinates

Solve C*M = N (C,M and N are matrices) where M is known and the structure of N is given in SymPy

I have come across this matrix multiplication problem where M is some non-singular 3x3 matrix with known values (i.e M = sympy.Matrix([[1, 0, 0],[0, 2, 0],[0, 0, 3]])) C is a 3x3 matrix to be determined and N is of the following form:
1. The 1st and 3rdrow of N are the same as C (e.g N.row(0)[i] = C.row(0)[i] for 0<=i<=2)
2. The elements in the 2nd row of N are the sum of the corresponding column in M (e.g N.row(1)[1] = sum(M.col(1)))
After searching the web for a way to express this problem as a system of equations I've found nothing. I've been trying to solve this using symbolic matrices and by or by solving three different systems of the form Ax=b each one made of a row from C multiplied by M with b as a column from N such that A = M.T, x = (C.row(i)).T and b = N.
Solving it symbolically resulted in a ridiculous expression that cannot be even be comprehended and I was unable to get a numeric solution from it.
My latest attempt follow:
import sympy as sp
def func(mat=matrix([[1, 1, 1], [0, 2, 2], [1, 4, 5]])):
c11, c12, c13, c21, c22, c23, c31, c32, c33 = sp.symbols('c11, c12, c13, c21, c22, c23, c31, c32, c33')
M = mat.T
b1 = sp.Matrix([[x, y, z]]).T
b2 = sp.Matrix([[sum(M.col(0)), sum(M.col(1)), sum(M.col(2))]]).T
b3 = sp.Matrix([[a, b, c]]).T
M1 = M.col_insert(3, b1)
M2 = M.col_insert(3, b2)
M3 = M.col_insert(3, b3)
C1 = sp.linsolve(M1, (x, y, z))
C2 = sp.linsolve(M2, (x, y, z))
C3 = sp.linsolve(M3, (a, b, c))
return C1, C2, C3
Calling this yields the following:
>>> func()
({(x + y - z, -x/2 + 2*y - 3*z/2, -y + z)}, {(-3, -17/2, 6)}, {(a + b - c, -a/2 + 2*b - 3*c/2, -b + c)})

I won't claim I understand your code, but the solution is actually easy to guess: The first and third rows of C and N must either be left eigenvectors of M with eigenvalue 1 which will in the general case not exist or must be zero. The middle row requirement is solved by C being all ones and since M is nonsingular that's the only solution.
Let's use good old numpy to numerically check this:
import numpy as np
M = np.random.random((3, 3))
M
# array([[ 0.39632944, 0.82429087, 0.88705214],
# [ 0.39092656, 0.63228762, 0.54931835],
# [ 0.76935833, 0.40833527, 0.46202912]])
C = np.outer((0,1,0),(1,1,1))
C
# array([[0, 0, 0]
# [1, 1, 1],
# [0, 0, 0]])
N = np.outer((0,1,0),M.sum(0))
N
# array([[ 0. , 0. , 0. ],
# [ 1.55661432, 1.86491377, 1.89839961],
# [ 0. , 0. , 0. ]])
np.allclose(C # M , N)
# True

python recursive vectorization with timeseries

I have a Timeseries (s) which need to be processed recursively to get a timeseries result (res). Here is my sample code:
res=s.copy()*0
res[1]=k # k is a constant
for i in range(2,len(s)):
res[i]=c1*(s[i]+s[i-1])/2 +c2*res[i-1]+c3*res[i-2]
where c1,c2,c3 are constants. It works properly but I'd like to use vectorization and I tried with:
res[2:]=c1*(s[2:]+s[1:-1])/2+c2*res[1:-1]+c3*res[0:-2]
but I get "ValueError: operands could not be broadcast together with shapes (1016) (1018) "
if I try with
res=c1*(s[2:]+s[1:-1])/2+c2*res[1:-1]+c3*res[0:-2]
doesn't give any error, but I don't get a correct result, because res[0] and res[1] have to be initialized before the calculation will take place.
Is there a way to process it with vectorization?
Any help will be appreciated, thanks!

This expression
res[i] = c1*(s[i] + s[i-1])/2 + c2*res[i-1] + c3*res[i-2]
says that res is the output of a linear filter (or ARMA process) with input s. Several libraries have functions for computing this. Here's how you can use the scipy function scipy.signal.lfilter.
From inspection of the recurrence relation, we get the coefficients of the numerator (b) and denominator (a) of the filter's transfer function:
b = c1 * np.array([0.5, 0.5])
a = np.array([1, -c2, -c3])
We'll also need an appropriate initial condition for lfilter to handle res[:2] == [0, k]. For this, we use scipy.signal.lfiltic:
zi = lfiltic(b, a, [k, 0], x=s[1::-1])
In the simplest case, one would call lfilter like this:
y = lfilter(b, a, s)
With an initial condition zi, we use:
y, zo = lfilter(b, a, s, zi=zi)
However, to exactly match the calculation provided in the question, we need the output y to start with [0, k]. So we'll allocate an array y, initialize the first two elements with [0, k], and assign the output of lfilter to y[2:]:
y = np.empty_like(s)
y[:2] = [0, k]
y[2:], zo = lfilter(b, a, s[2:], zi=zi)
Here's a complete script with the original loop and with lfilter:
import numpy as np
from scipy.signal import lfilter, lfiltic
c1 = 0.125
c2 = 0.5
c3 = 0.25
np.random.seed(123)
s = np.random.rand(8)
k = 3.0
# Original version (edited lightly)
res = np.zeros_like(s)
res[1] = k # k is a constant
for i in range(2, len(s)):
res[i] = c1*(s[i] + s[i-1])/2 + c2*res[i-1] + c3*res[i-2]
# Using scipy.signal.lfilter
# Coefficients of the filter's transfer function.
b = c1 * np.array([0.5, 0.5])
a = np.array([1, -c2, -c3])
# Create the initial condition of the filter such that
# y[:2] == [0, k]
zi = lfiltic(b, a, [k, 0], x=s[1::-1])
y = np.empty_like(s)
y[:2] = [0, k]
y[2:], zo = lfilter(b, a, s[2:], zi=zi)
np.set_printoptions(precision=5)
print "res:", res
print "y: ", y
The output is:
res: [ 0. 3. 1.53206 1.56467 1.24477 1.08496 0.94142 0.84605]
y: [ 0. 3. 1.53206 1.56467 1.24477 1.08496 0.94142 0.84605]
lfilter accepts an axis argument, so you can filter an array of signals with a single call. lfiltic does not have an axis argument, so setting up the initial conditions requires a loop. The following script shows an example.
import numpy as np
from scipy.signal import lfilter, lfiltic
import matplotlib.pyplot as plt
# Parameters
c1 = 0.2
c2 = 1.1
c3 = -0.5
k = 1
# Create an array of signals for the demonstration.
np.random.seed(123)
nsamples = 50
nsignals = 4
s = np.random.randn(nsamples, nsignals)
# Coefficients of the filter's transfer function.
b = c1 * np.array([0.5, 0.5])
a = np.array([1, -c2, -c3])
# Create the initial condition of the filter for each signal
# such that
# y[:2] == [0, k]
# We need a loop here, because lfiltic is not vectorized.
zi = np.empty((2, nsignals))
for i in range(nsignals):
zi[:, i] = lfiltic(b, a, [k, 0], x=s[1::-1, i])
# Create the filtered signals.
y = np.empty_like(s)
y[:2, :] = np.array([0, k]).reshape(-1, 1)
y[2:, :], zo = lfilter(b, a, s[2:], zi=zi, axis=0)
# Plot the filtered signals.
plt.plot(y, linewidth=2, alpha=0.6)
ptp = y.ptp()
plt.ylim(y.min() - 0.05*ptp, y.max() + 0.05*ptp)
plt.grid(True)
plt.show()
Plot:

Am I using scipy.linalg.solve_discrete_lyapunov correctly

I'm using scipy.linalg.solve_discrete_lyapunov to compute matrix P from
MT PM - P = -Q where M = A - BK and Q = I
(see below and also see Lyapunov Equation). However, for the computed P I get MT PM - P ≠ - Q.
Here is the code:
import numpy as np
import scipy as sp
A = np.array([[-1.86194971, 3.49237959],[-2.34245904, 3.86194971]])
B = np.array([[ 3000., 2500.5], [ 2000.2, 3000.]])
K = np.array([[ 0.0001367, -0.00016844], [-0.00069637, 0.0009627]])
I = np.array([[1., 0.],[0., 1.]])
# Eigenvalues of A are (0.9, 1.1)
# Eigenvalues of A-BK are (0.29, 0.49) (i.e. A-BK is Schur)
P = sp.linalg.solve_discrete_lyapunov(A-np.dot(B,K), I)
# P= [[ 6.61311138 4.32497891]
# [ 4.32497891 4.36910499]]
# But after checking (A-BK)^TP(A-BK)-P, that is
J = np.dot((A.transpose()-np.dot(K.transpose(),B.transpose())),np.dot(P,A-np.dot(B,K)))-P
# I get the following
# J = (A-BK)^TP(A-BK)-P = [[ -1.11929701 -19.5567893 ]
# [-19.5567893 37.89911723]]
#
# Not equal to -I?

Let M = A - np.dot(B,K). Then solve_discrete_lyapunov(M, I) is solving
np.dot(M, np.dot(P, M.T)) - P = -I
In [64]: M = A - np.dot(B,K)
In [65]: np.dot(M, np.dot(P,M.T)) - P
Out[65]:
array([[ -1.00014927e+00, -9.93418066e-05],
[ -9.93418066e-05, -1.00006419e+00]])
In [66]: np.allclose(np.dot(M, np.dot(P,M.T)) - P, -I, atol=0.001)
Out[66]: True
If you want to solve
np.dot(M.T, np.dot(P, M)) - P + I = 0
then call
P = solve_discrete_lyapunov(M.T, I)