I would like to perform transformation for this example data set.
There are four known points with coordinates x, y, z in one coordinate[primary_system] system and next four known points with coordinates x, y, h that belong to another coordinate system[secondary_system].
Those points correspond; for example primary_system1 point and secondary_system1 point is exactly the same point but we have it's coordinates in two different coordinate systems.
So I have here four pairs of adjustment points and want to transform another point coordinates from primary system to secondary system according to adjustment.
primary_system1 = (3531820.440, 1174966.736, 5162268.086)
primary_system2 = (3531746.800, 1175275.159, 5162241.325)
primary_system3 = (3532510.182, 1174373.785, 5161954.920)
primary_system4 = (3532495.968, 1175507.195, 5161685.049)
secondary_system1 = (6089665.610, 3591595.470, 148.810)
secondary_system2 = (6089633.900, 3591912.090, 143.120)
secondary_system3 = (6089088.170, 3590826.470, 166.350)
secondary_system4 = (6088672.490, 3591914.630, 147.440)
#transform this point
x = 3532412.323
y = 1175511.432
z = 5161677.111<br>
at the moment I try to average translation for x, y and z axis using each of the four pairs of points like:
#x axis
xt1 = secondary_system1[0] - primary_system1[0]
xt2 = secondary_system2[0] - primary_system2[0]
xt3 = secondary_system3[0] - primary_system3[0]
xt4 = secondary_system4[0] - primary_system4[0]
xt = (xt1+xt2+xt3+xt4)/4 #averaging
...and so on for y and z axis
#y axis
yt1 = secondary_system1[1] - primary_system1[1]
yt2 = secondary_system2[1] - primary_system2[1]
yt3 = secondary_system3[1] - primary_system3[1]
yt4 = secondary_system4[1] - primary_system4[1]
yt = (yt1+yt2+yt3+yt4)/4 #averaging
#z axis
zt1 = secondary_system1[2] - primary_system1[2]
zt2 = secondary_system2[2] - primary_system2[2]
zt3 = secondary_system3[2] - primary_system3[2]
zt4 = secondary_system4[2] - primary_system4[2]
zt = (zt1+zt2+zt3+zt4)/4 #averaging
So above I attempted to calculate average translation vector for every axis
If it is just a translation and rotation, then this is a transformation known as an affine transformation.
It basically takes the form:
secondary_system = A * primary_system + b
where A is a 3x3 matrix (since you're in 3D), and b is a 3x1 translation.
This can equivalently be written
secondary_system_coords2 = A2 * primary_system2,
where
secondary_system_coords2 is the vector [secondary_system,1],
primary_system2 is the vector [primary_system,1], and
A2 is the 4x4 matrix:
[ A b ]
[ 0,0,0,1 ]
(See the wiki page for more info).
So basically, you want to solve the equation:
y = A2 x
for A2, where y consist of points from secondary_system with 1 stuck on the end, and x is points from primary_system with 1 stuck on the end, and A2 is a 4x4 matrix.
Now if x was a square matrix we could solve it like:
A2 = y*x^(-1)
But x is 4x1. However, you are lucky and have 4 sets of x with 4 corresponding sets of y, so you can construct an x that is 4x4 like so:
x = [ primary_system1 | primary_system2 | primary_system3 | primary_system4 ]
where each of primary_systemi is a 4x1 column vector. Same with y.
Once you have A2, to transform a point from system1 to system 2 you just do:
transformed = A2 * point_to_transform
You can set this up (e.g. in numpy) like this:
import numpy as np
def solve_affine( p1, p2, p3, p4, s1, s2, s3, s4 ):
x = np.transpose(np.matrix([p1,p2,p3,p4]))
y = np.transpose(np.matrix([s1,s2,s3,s4]))
# add ones on the bottom of x and y
x = np.vstack((x,[1,1,1,1]))
y = np.vstack((y,[1,1,1,1]))
# solve for A2
A2 = y * x.I
# return function that takes input x and transforms it
# don't need to return the 4th row as it is
return lambda x: (A2*np.vstack((np.matrix(x).reshape(3,1),1)))[0:3,:]
Then use it like this:
transformFn = solve_affine( primary_system1, primary_system2,
primary_system3, primary_system4,
secondary_system1, secondary_system2,
secondary_system3, secondary_system4 )
# test: transform primary_system1 and we should get secondary_system1
np.matrix(secondary_system1).T - transformFn( primary_system1 )
# np.linalg.norm of above is 0.02555
# transform another point (x,y,z).
transformed = transformFn((x,y,z))
Note: There is of course numerical error here, and this may not be the best way to solve for the transform (you might be able to do some sort of least squares thing).
Also, the error for converting primary_systemx to secondary_systemx is (for this example) of order 10^(-2).
You'll have to consider whether this is acceptable or not (it does seem large, but it might be acceptable when compared to your input points which are all of order 10^6).
The mapping you are looking for seems to be affine transformation. Four 3D points not lying in one plain is the exact number of points needed to recover the affine transformation. The latter is, loosely speaking, multiplication by matrix and adding a vector
secondary_system = A * primary_system + t
The problem is now reduced to finding appropriate matrix A and vector t. I think, this code may help you (sorry for bad codestyle -- I'm mathematician, not programmer)
import numpy as np
# input data
ins = np.array([[3531820.440, 1174966.736, 5162268.086],
[3531746.800, 1175275.159, 5162241.325],
[3532510.182, 1174373.785, 5161954.920],
[3532495.968, 1175507.195, 5161685.049]]) # <- primary system
out = np.array([[6089665.610, 3591595.470, 148.810],
[6089633.900, 3591912.090, 143.120],
[6089088.170, 3590826.470, 166.350],
[6088672.490, 3591914.630, 147.440]]) # <- secondary system
p = np.array([3532412.323, 1175511.432, 5161677.111]) #<- transform this point
# finding transformation
l = len(ins)
entry = lambda r,d: np.linalg.det(np.delete(np.vstack([r, ins.T, np.ones(l)]), d, axis=0))
M = np.array([[(-1)**i * entry(R, i) for R in out.T] for i in range(l+1)])
A, t = np.hsplit(M[1:].T/(-M[0])[:,None], [l-1])
t = np.transpose(t)[0]
# output transformation
print("Affine transformation matrix:\n", A)
print("Affine transformation translation vector:\n", t)
# unittests
print("TESTING:")
for p, P in zip(np.array(ins), np.array(out)):
image_p = np.dot(A, p) + t
result = "[OK]" if np.allclose(image_p, P) else "[ERROR]"
print(p, " mapped to: ", image_p, " ; expected: ", P, result)
# calculate points
print("CALCULATION:")
P = np.dot(A, p) + t
print(p, " mapped to: ", P)
This code demonstrates how to recover affine transformation as matrix + vector and tests that initial points are mapped to where they should. You can test this code with Google colab, so you don't have to install anything.
Regarding theory behind this code: it is based on equation presented in "Beginner's guide to mapping simplexes affinely", matrix recovery is described in section "Recovery of canonical notation" and number of points needed to pinpoint the exact affine transformation is discussed in "How many points do we need?" section. The same authors published "Workbook on mapping simplexes affinely" that contains many practical examples of this kind.
Related
Given a rotation matrix R, what is the equivalent transformation so that some point c (the new rotation center) is unchanged under the transformation y = R * c, i.e., change R so that the rotation center is at c rather than the origin. One limitation is that I cannot act on the actual vector to be used, only edit the original transformation (this is because that part is buried in an external library and I cannot change it).
I have something that works but it violates the above limitation:
import numpy as np
# R is a given rotation matrix, v is an arbitrary vector and cen is the desired center
theta = np.pi / 2
cen = np.array([0.5, 0.5])
v = np.array([0, 0])
R = Rot(theta) # ordinary rotation matrix
y = R # (v - center ) + cen
# y = [0, 1] which is the expected result since we have a right angle triangle
# with vertices at (0, 0) (0.5, 0.5) and (0, 1)
I also tried implementing a calculation similar to here but the result I'm getting is not correct
How can I achive the same result but keeping the form y = R*v (or using a rigid transform y = R*v + t) but not changing v like I did?
While writing the question I came upon the (obvious) solution which is just expanding the solution I wrote before and rearranging. In case someone needs this as well the new transformations should be:
y = R_new*v + t_new
where R_new = R, t_new = (I - R)*cen and I is the identity.
Working in Python, I am doing some physics calculations over an NxM grid of values, where N goes from 1 to 3108 and M goes from 1 to 2304 (this corresponds to a large image). I need calculate a value at each and every point in this space, which totals ~ 7 million calculations. My current approach is painfully slow, and I am wondering if there is a way to complete this task and it not take hours...
My first approach was just to use nested for loops, but this seemed like the least efficient way to solve my problem. I have tried using NumPy's nditer and iterating over each axis individually, but I've read that it doesn't actually speed up my computations. Rather than looping through each axis individually, I also tried making a 3-D array and looping through the outer axis as shown in Brian's answer here How can I, in python, iterate over multiple 2d lists at once, cleanly? . Here is the current state of my code:
import numpy as np
x,y = np.linspace(1,3108,num=3108),np.linspace(1,2304,num=2304) # x&y dimensions of image
X,Y = np.meshgrid(x,y,indexing='ij')
all_coords = np.dstack((X,Y)) # moves to 3-D
all_coords = all_coords.astype(int) # sets coords to int
For reference, all_coords looks like this:
array([[[1.000e+00, 1.000e+00],
[1.000e+00, 2.000e+00],
[1.000e+00, 3.000e+00],
...,
[1.000e+00, 2.302e+03],
[1.000e+00, 2.303e+03],
[1.000e+00, 2.304e+03]],
[[2.000e+00, 1.000e+00],
[2.000e+00, 2.000e+00],
[2.000e+00, 3.000e+00],
...,
[2.000e+00, 2.302e+03],
[2.000e+00, 2.303e+03],
[2.000e+00, 2.304e+03]],
and so on. Back to my code...
'''
- below is a function that does a calculation on the full grid using the distance between x0,y0 and each point on the grid.
- the function takes x0,y0 and returns the calculated values across the grid
'''
def do_calc(x0,y0):
del_x, del_y = X-x0, Y-y0
np.seterr(divide='ignore', invalid='ignore')
dmx_ij = (del_x/((del_x**2)+(del_y**2))) # x component
dmy_ij = (del_y/((del_x**2)+(del_y**2))) # y component
return dmx_ij,dmy_ij
# now the actual loop
def do_loop():
dmx,dmy = 0,0
for pair in all_coords:
for xi,yi in pair:
DM = do_calc(xi,yi)
dmx,dmy = dmx+DM[0],dmy+DM[1]
return dmx,dmy
As you might see, this code takes an incredibly long time to run... If there is any way to modify my code such that it doesn't take hours to complete, I would be extremely interested in knowing how to do that. Thanks in advance for the help.
Here is a method that gives a 10,000x speedup at N=310, M=230. As the method scales better than the original code I'd expect a factor of more than a million at the full problem size.
The method exploits the shift invariance of the problem. For example, del_x**2 is essentially the same up to shift at each call of do_calc, so we compute it only once.
If the output of do_calc is weighted before summation the problem is no longer fully translation invariant, and this method doesn't work anymore. The result, however, can then be expressed in terms of linear convolution. At N=310, M=230 this still leaves us with a more than 1,000x speedup. And, again, this will be more at full problem size
Code for original problem
import numpy as np
#N, M = 3108, 2304
N, M = 310, 230
### OP's code
x,y = np.linspace(1,N,num=N),np.linspace(1,M,num=M) # x&y dimensions of image
X,Y = np.meshgrid(x,y,indexing='ij')
all_coords = np.dstack((X,Y)) # moves to 3-D
all_coords = all_coords.astype(int) # sets coords to int
'''
- below is a function that does a calculation on the full grid using the distance between x0,y0 and each point on the grid.
- the function takes x0,y0 and returns the calculated values across the grid
'''
def do_calc(x0,y0):
del_x, del_y = X-x0, Y-y0
np.seterr(divide='ignore', invalid='ignore')
dmx_ij = (del_x/((del_x**2)+(del_y**2))) # x component
dmy_ij = (del_y/((del_x**2)+(del_y**2))) # y component
return np.nan_to_num(dmx_ij), np.nan_to_num(dmy_ij)
# now the actual loop
def do_loop():
dmx,dmy = 0,0
for pair in all_coords:
for xi,yi in pair:
DM = do_calc(xi,yi)
dmx,dmy = dmx+DM[0],dmy+DM[1]
return dmx,dmy
from time import time
t = [time()]
### pp's code
x, y = np.ogrid[-N+1:N-1:2j*N - 1j, -M+1:M-1:2j*M - 1J]
den = x*x + y*y
den[N-1, M-1] = 1
xx = x / den
yy = y / den
for zz in xx, yy:
zz[N:] -= zz[:N-1]
zz[:, M:] -= zz[:, :M-1]
XX = xx.cumsum(0)[N-1:].cumsum(1)[:, M-1:]
YY = yy.cumsum(0)[N-1:].cumsum(1)[:, M-1:]
t.append(time())
### call OP's code for reference
X_OP, Y_OP = do_loop()
t.append(time())
# make sure results are equal
assert np.allclose(XX, X_OP)
assert np.allclose(YY, Y_OP)
print('pp {}\nOP {}'.format(*np.diff(t)))
Sample run:
pp 0.015251636505126953
OP 149.1642508506775
Code for weighted problem:
import numpy as np
#N, M = 3108, 2304
N, M = 310, 230
values = np.random.random((N, M))
x,y = np.linspace(1,N,num=N),np.linspace(1,M,num=M) # x&y dimensions of image
X,Y = np.meshgrid(x,y,indexing='ij')
all_coords = np.dstack((X,Y)) # moves to 3-D
all_coords = all_coords.astype(int) # sets coords to int
'''
- below is a function that does a calculation on the full grid using the distance between x0,y0 and each point on the grid.
- the function takes x0,y0 and returns the calculated values across the grid
'''
def do_calc(x0,y0, v):
del_x, del_y = X-x0, Y-y0
np.seterr(divide='ignore', invalid='ignore')
dmx_ij = (del_x/((del_x**2)+(del_y**2))) # x component
dmy_ij = (del_y/((del_x**2)+(del_y**2))) # y component
return v*np.nan_to_num(dmx_ij), v*np.nan_to_num(dmy_ij)
# now the actual loop
def do_loop():
dmx,dmy = 0,0
for pair, vv in zip(all_coords, values):
for (xi,yi), v in zip(pair, vv):
DM = do_calc(xi,yi, v)
dmx,dmy = dmx+DM[0],dmy+DM[1]
return dmx,dmy
from time import time
from scipy import signal
t = [time()]
x, y = np.ogrid[-N+1:N-1:2j*N - 1j, -M+1:M-1:2j*M - 1J]
den = x*x + y*y
den[N-1, M-1] = 1
xx = x / den
yy = y / den
XX, YY = (signal.fftconvolve(zz, values, 'valid') for zz in (xx, yy))
t.append(time())
X_OP, Y_OP = do_loop()
t.append(time())
assert np.allclose(XX, X_OP)
assert np.allclose(YY, Y_OP)
print('pp {}\nOP {}'.format(*np.diff(t)))
Sample run:
pp 0.12683939933776855
OP 158.35225439071655
Basically I have 2 arrays obtained from a set of data points one array for the x values and one for the y values. I need to numerically integrate the y values with respect to the x values - i.e. an element from the y integrated with respect to the corresponding element in x. This should then generate a new array of elements. I have tried simpson's rule but I get one value back instead of an array. A general idea or approach is all I'm looking for. Any help, however, will be much appreciated.
Thanks.
# check out this:
def integration_by_simpsons_3_8_th_rule(i,X,Y,Fd):
h = X[i]-X[i-1]
y_n = Y[i]
y_n_1 = signal[i-1]
y_n_2 = signal[i-2]
y_n_3 = signal[i-3]
Area = (3/8)*h*( y_n_3 + 3*(y_n_2 + y_n_1) + y_n )
return (X[i-1],Area)
def rolling_integration(X,Y,Fd):
Y_int = []
corres_X = []
for i in range(3,len(signal),1):
x,y = integration_by_simpsons_3_8_th_rule(i,X,Y,Fd)
Y_int.append(float(y))
corres_X.append(float(x))
return (np.array(corres_X)+(np.array(1/(4*float(Fd)))),np.array(Y_int))
#Fd : for phase correction
I'm trying to get the vector coordinates from the polynomial p in the follow code assuming that x,y and z belong to GF(2) but I get error
TypeError: can't initialize vector from nonzero non-list.
How I will be able to fix that?
reset()
var("x")
var("y")
var("z")
pp = 2
k.<t>=GF(2^pp)
VS = k.vector_space()
p = z*x*t^2 + t*y + 1
print VS.coordinates(p)
Maybe you can use the coefficient list of the polynomial as its vectoral coordinates, and then you may convert this list to a vector. But in that case, it is better to define GF(2^2) as GF(4,'a')={0,1,a,a+1}.
For example you may do something like this:
sage
K = GF(4,'a')
R = PolynomialRing(GF(4,'a'),"x")
x = R.gen()
a = K.gen()
p = (a+1)*x^3 + x^2 + a
p.list()
If you need to fix the dimension n to a bigger value than the degree of p, then you may do the following;
n = 6
L = p.list(); l=len(L); i = n-l; L_ = [0]*i; L.extend(L_)
L
gives you the 6-dimensional coordinates of p.
If you need to use this coefficient list as a vector afterwards, you may just use vector(L) instead of L.
I'm programming a function in Python in Autodesk Maya (using PyMel for Maya)
I have three 3D points; p0, p1, p2.
Then they make a rigid transformation, so after the transformation (an affine transformation) I have their new positions; q0, q1, q2.
I also have a fourth point before the transformation; p3. I want to calculate its position after the same transformation; q4.
So I need to calculate the transformation matrix, and then apply it to p4. I don't know how to do either. List = an array of objects
import pymel.core as pm
import pymel.core.datatypes as dt
p0 = dt.Vector(pm.getAttr(list[0]+".tx"), pm.getAttr(list[0]+".ty"), pm.getAttr(list[0]+".tz"))
p1 = dt.Vector(pm.getAttr(list[1]+".tx"), pm.getAttr(list[1]+".ty"), pm.getAttr(list[1]+".tz"))
p2 = dt.Vector(pm.getAttr(list[2]+".tx"), pm.getAttr(list[2]+".ty"), pm.getAttr(list[2]+".tz")
p3 = dt.Vector(pm.getAttr(list[3]+".tx"), pm.getAttr(list[3]+".ty"), pm.getAttr(list[3]+".tz"))
The 3D points are read from animated objects in the Maya scene. So at another frame,
I run this code to get
q0 = dt.Vector(pm.getAttr(list[0]+".tx"), pm.getAttr(list[0]+".ty"), pm.getAttr(list[0]+".tz"))
q1 = dt.Vector(pm.getAttr(list[1]+".tx"), pm.getAttr(list[1]+".ty"), pm.getAttr(list[1]+".tz"))
q2 = dt.Vector(pm.getAttr(list[2]+".tx"), pm.getAttr(list[2]+".ty"), pm.getAttr(list[2]+".tz"))
#q3 = TransformationMatrix between (p0,p1,p2) and (q0,q1,q2), applied to p3
I tried to calculate with vectors, but I ended up with errors due to divisions by zero...
So I figured that a transformation matrix should solve it without problems.
I've got a deadline not far ahead and I REALLY need help solving this!
PLEASE HELP!
Edit:
how to perform coordinates affine transformation using python?
I need this function "solve_affine", but it should take only 3 points from each set instead of 4. And I can't use numpy...
Here's a solution using numpy and scipy. scipy is mostly used to generate random rotations, except for scipy.linalg.norm which is easy to code oneself. The main things used from numpy are cross product and matrix multiplication, which are also easy to code oneself.
The basic idea is this: given three non-collinear points x1,x2,x3, it's possible to find an orthogonal triple of vectors (axes) v1,v2,v3, with v1 in the direction of x2-x1, v2 in the plane spanned by (x2-x1) and (x3-x1), and v3 completing the triple.
The points y1,y2,y3 are rotated and translated relative to x1,x2,x3. The axes w1,w2,w3 generated from y1,y2,y3 are rotated (i.e., no translation) from v1,v2,v3. These two sets of triples are each orthogonal, so it's easy to find the rotation from them: R = W * transpose(V)
Once we have the rotation, finding the translation is simple: y1 = R*x + t, so t = y1 - R*x. It might be a better to use a least-squares solver and combine all three points to get an estimate of t.
import numpy
import scipy.linalg
def rand_rot():
"""Return a random rotation
Return a random orthogonal matrix with determinant 1"""
q, _ = scipy.linalg.qr(numpy.random.randn(3, 3))
if scipy.linalg.det(q) < 0:
# does this ever happen?
print "got a negative det"
q[:, 0] = -q[:, 0]
return q
def rand_noncollinear():
"""Return 3 random non-collinear vectors"""
while True:
b = numpy.random.randn(3, 3)
sigma = scipy.linalg.svdvals(b)
if sigma[2]/sigma[0] > 0.1:
# "very" non-collinear
break
# "nearly" collinear; try again
return b[:, 0], b[:, 1], b[:, 2]
def normalize(a):
"""Return argument normalized"""
return a/scipy.linalg.norm(a)
def colstack(a1, a2, a3):
"""Stack three vectors as columns"""
return numpy.hstack((a1[:, numpy.newaxis],
a2[:, numpy.newaxis],
a3[:, numpy.newaxis]))
def get_axes(a1, a2, a3):
"""Generate orthogonal axes from three non-collinear points"""
# I tried to do this with QR, but something didn't work
b1 = normalize(a2-a1)
b2 = normalize(a3-a1)
b3 = normalize(numpy.cross(b1, b2))
b4 = normalize(numpy.cross(b3, b1))
return b1, b4, b3
# random rotation and translation
r = rand_rot()
t = numpy.random.randn(3)
# three non-collinear points
x1, x2, x3 = rand_noncollinear()
# some other point
x4 = numpy.random.randn(3)
# the images of the above in the transformation.
# y4 is for checking only -- won't be used to estimate r or t
y1, y2, y3, y4 = [numpy.dot(r, x) + t
for x in x1, x2, x3, x4]
v1, v2, v3 = get_axes(x1, x2, x3)
w1, w2, w3 = get_axes(y1, y2, y3)
V = colstack(v1, v2, v3)
W = colstack(w1, w2, w3)
# W = R V, so R = W * inverse(V); but V orthogonal, so inverse(V) is
# transpose(V):
rfound = numpy.dot(W, V.T)
# y1 = R x1 + t, so...
tfound = y1-numpy.dot(r, x1)
# get error on images of x2 and x3, just in case
y2err = scipy.linalg.norm(numpy.dot(rfound, x2) + tfound - y2)
y3err = scipy.linalg.norm(numpy.dot(rfound, x3) + tfound - y3)
# and check error image of x4 -- getting an estimate of y4 is the
# point of all of this
y4err = scipy.linalg.norm(numpy.dot(rfound, x4) + tfound - y4)
print "y2 error: ", y2err
print "y3 error: ", y3err
print "y4 error: ", y4err
Both the description and your code are confusing. Description is a bit vague while the code examples are missing important bits and pieces. So here is how I understand the question:
Knowing three points in two spaces how to construct a transform from space A to space B?
Image 1: How to form a transformation between 2 spaces.
The answer depends on the type of transform the spaces have. You see three points always form a planar span. This means that you can know what the rotation, transform, and uniform scale of the new space is. You can also know the shear on the plane, as well as nonuniform scale. However, you can not know what the shear or nonuniform scale would be in the plane normal direction.
Therefore to make sense the question mutates into how to rotate and translate two spaces to match? this is pretty easy to do Translation part is directly:
trans = q0 - p0
That leaves you with rotation which has been explained in several posts:
python + maya: Rotate Y axis to be along vector
How to convert three dimensional vector to an Euler rotation in software like Maya using python
You can also calculate a scaling factor after this.
I've figured it out
p0p1 = p1-p0
p0p2 = p2-p0
p0p3 = p3-p0
q0q1 = q1-q0
q0q2 = q2-q0
q0q3 = q3-q0
before = dt.Matrix([p0.x, p0.y, p0.z, 0],[p1.x, p1.y, p1.z, 0],[p2.x, p2.y, p2.z, 0], [0,0,0,1]);
after = dt.Matrix([q0.x, q0.y, q0.z, 0],[q1.x, q1.y, q1.z, 0],[q2.x, q2.y, q2.z, 0], [0,0,0,1]);
normal = p0p1.cross(p0p2).normal()
dist = p0p3.dot(normal)
q3 = p3 - dist*normal
transformMatrix = before.inverse()*after
solve = dt.Matrix(q3.x, q3.y, q3.z, 1)*transformMatrix
q3 = dt.Vector(solve[0][0], solve[0][1], solve[0][2])
newNormal = q0q1.cross(q0q2).normal()
q3 = q3 + newNormal*dist
pm.move(list[3], q3, r=False)
The transformation matrix only worked for points that are within the plane p0p1p2. So I solved it by transforming the projected point of p3, then move it out from the plane by the same distance.
If you have a solution that only involves a matrix, feel free to share, it may still help me! :)