quaternion 회전을 파이썬으로 구현한것

a가 회전할 축으로 쓸 단위방향벡터, θ을 회전할 각도, p를 회전시킬 위치벡터라고 할때

q=(a*sinθ,cosθ), p=(p,1)

이라고 할때

qp(qˆ) (오른쪽은 q의 inverse)를 하면 점 p를 a를 축으로 2*θ만큼 회전한 위치가 나온다.

이걸 행렬로 나타낼 수 있다.

(real-time rendering 3rd)

M*p를 하면 위와 똑같은 결과가 나온다.

위를 파이썬으로 구현해보았다

import math
class vec:
 def __init__(self, v1, v2, v3, v4):
 self.v=[]
 self.v.append(v1)
 self.v.append(v2)
 self.v.append(v3)
 self.v.append(v4)
class matrix:
 def __init__(self):
 self.m=[]
 for i in range (4):
 self.m.append([])
 for j in range (4):
 self.m[i].append(0)
 for i in range(4):
 self.m[i][i]=1
def crossproduct(v1, v2):
 ret = vec(v1.v[1]*v2.v[2] - v1.v[2]*v2.v[1] , v1.v[2]*v2.v[0] - v1.v[0]*v2.v[2], v1.v[0]*v2.v[1] - v1.v[1]*v2.v[0], 0)
 return ret
def dotproduct(v1, v2):
 ret = v1.v[0]*v2.v[0] + v1.v[1]*v2.v[1] + v1.v[2]*v2.v[2]
 return ret
def vcMul(v, c):
 ret = vec(v.v[0]*c, v.v[1]*c, v.v[2]*c,v.v[3])
 return ret
def vAdd(v1, v2):
 ret = vec(v1.v[0] + v2.v[0], v1.v[1] + v2.v[1], v1.v[2] + v2.v[2], 1)
 return ret
def qqMul(v1, v2):
 t1=crossproduct(v1,v2)
 t2=vcMul(v1,v2.v[3])
 t3=vcMul(v2,v1.v[3])
 w=v1.v[3]*v2.v[3] - dotproduct(v1,v2)
 sum1 = vAdd(t1,t2)
 sum2 = vAdd(sum1,t3)
 ret = vec(sum2.v[0],sum2.v[1],sum2.v[2],w)
 return ret
def qcMul(v,c):
 ret = vec(v.v[0]*c,v.v[1]*c,v.v[2]*c,v.v[3]*c)
 return ret
def vNorm(v):
 l = math.sqrt(v.v[0]*v.v[0] + v.v[1]*v.v[1] + v.v[2]*v.v[2])
 ret = vec(v.v[0]/l,v.v[1]/l,v.v[2]/l,v.v[3])
 return ret
def qConjugate(v):
 ret = vec(-v.v[0],-v.v[1],-v.v[2],v.v[3])
 return ret
def qNorm(v):
 return math.sqrt(v.v[0]*v.v[0] + v.v[1]*v.v[1] + v.v[2]*v.v[2] + v.v[3]*v.v[3] )
def qInverse(v):
 tmp=qNorm(v)
 tmp2=1/(tmp*tmp)
 ret = qcMul(qConjugate(v),tmp2)
 return ret
def qUnit(v,r):
 v1=vNorm(v)
 v2=vcMul(v1, math.sin(r))
 ret = vec(v2.v[0],v2.v[1],v2.v[2],math.cos(r))
 return ret
def printmat(m):
 for i in range(4):
 print(m.m[i])
def qTOm(q):
 ret=matrix()
 ret.m[0][0]=1-2*(q.v[1]*q.v[1] + q.v[2]*q.v[2])
 ret.m[0][1]=2*(q.v[0]*q.v[1] - q.v[3]*q.v[2])
 ret.m[0][2]=2*(q.v[0]*q.v[2] + q.v[3]*q.v[1])

ret.m[1][0]=2*(q.v[0]*q.v[1] + q.v[3]*q.v[2])
 ret.m[1][1]=1-2*(q.v[0]*q.v[0] + q.v[2]*q.v[2])
 ret.m[1][2]=2*(q.v[1]*q.v[2] - q.v[3]*q.v[0])

ret.m[2][0]=2*(q.v[0]*q.v[2] - q.v[3]*q.v[1])
 ret.m[2][1]=2*(q.v[1]*q.v[2] + q.v[3]*q.v[0])
 ret.m[2][2]=1-2*(q.v[1]*q.v[1] + q.v[0]*q.v[0])
 return ret
def mvMul(m, v):
 tmp=[]
 for i in range(4):
 tmp.append(0)
 for i in range(4):
 for j in range(4):
 tmp[i] = tmp[i]+m.m[i][j]*v.v[j]
 ret = vec(tmp[0],tmp[1],tmp[2],tmp[3])
 return ret

p = vec(0,1,0,0)
a = vec(1,1,1,0)
print("p")
print(p.v)

q=qUnit(a,math.pi/1.3)
rmat = qTOm(q)
qi=qInverse(q)
r1=qqMul(p,qi)
print("p*qi")
print(r1.v)
r2=qqMul(q,r1)
print("after")
print(r2.v)
r3=mvMul(rmat,p)
print(r3.v)