#
# PFTrack python script lockObjectMotion.py
#
# Takes object motion from a geometry track and converts to camera
# motion, keeping the object locked in position in the first frame
#

import math
import pfpy

def pfNodeName():
	return 'Lock object motion'
		
def quaternionInverse(q):
	return (-q[0],-q[1],-q[2],q[3])

def quaternionNormalize(q):
	n = 1.0/math.sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3])
	return (n*q[0],n*q[1],n*q[2],n*q[3])

def quaternionMult(a, b):
	return (a[3]*b[0]+a[0]*b[3]+a[1]*b[2]-a[2]*b[1],
			a[3]*b[1]-a[0]*b[2]+a[1]*b[3]+a[2]*b[0],
			a[3]*b[2]+a[0]*b[1]-a[1]*b[0]+a[2]*b[3],
			a[3]*b[3]-a[0]*b[0]-a[1]*b[1]-a[2]*b[2])

def quaternionToMatrix(q):
    return (1.0-2.0*(q[1]*q[1]+q[2]*q[2]),
            2.0*(q[0]*q[1]-q[3]*q[2]),
            2.0*(q[0]*q[2]+q[3]*q[1]),
            2.0*(q[0]*q[1]+q[3]*q[2]),
            1.0-2.0*(q[0]*q[0]+q[2]*q[2]),
            2.0*(q[1]*q[2]-q[3]*q[0]),
            2.0*(q[0]*q[2]-q[3]*q[1]),
            2.0*(q[1]*q[2]+q[3]*q[0]),
            1.0-2.0*(q[0]*q[0]+q[1]*q[1]))

def matrixMult4x4(a, b):
  return (a[0]*b[0]+a[1]*b[4]+a[2]*b[8]+a[3]*b[12],
		  a[0]*b[1]+a[1]*b[5]+a[2]*b[9]+a[3]*b[13],
		  a[0]*b[2]+a[1]*b[6]+a[2]*b[10]+a[3]*b[14],
		  a[0]*b[3]+a[1]*b[7]+a[2]*b[11]+a[3]*b[15],
		  a[4]*b[0]+a[5]*b[4]+a[6]*b[8]+a[7]*b[12],
		  a[4]*b[1]+a[5]*b[5]+a[6]*b[9]+a[7]*b[13],
		  a[4]*b[2]+a[5]*b[6]+a[6]*b[10]+a[7]*b[14],
		  a[4]*b[3]+a[5]*b[7]+a[6]*b[11]+a[7]*b[15],
		  a[8]*b[0]+a[9]*b[4]+a[10]*b[8]+a[11]*b[12],
		  a[8]*b[1]+a[9]*b[5]+a[10]*b[9]+a[11]*b[13],
		  a[8]*b[2]+a[9]*b[6]+a[10]*b[10]+a[11]*b[14],
		  a[8]*b[3]+a[9]*b[7]+a[10]*b[11]+a[11]*b[15],
		  a[12]*b[0]+a[13]*b[4]+a[14]*b[8]+a[15]*b[12],
		  a[12]*b[1]+a[13]*b[5]+a[14]*b[9]+a[15]*b[13],
		  a[12]*b[2]+a[13]*b[6]+a[14]*b[10]+a[15]*b[14],
		  a[12]*b[3]+a[13]*b[7]+a[14]*b[11]+a[15]*b[15])

def vectorMult4x4(m, v):
	n = 1.0/(v[0]*m[12]+v[1]*m[13]+v[2]*m[14]+m[15])
	return ((v[0]*m[0]+v[1]*m[1]+v[2]*m[2]+m[3])*n,
			(v[0]*m[4]+v[1]*m[5]+v[2]*m[6]+m[7])*n,
			(v[0]*m[8]+v[1]*m[9]+v[2]*m[10]+m[11])*n)

def buildTransformationMatrix(t, q):
	T = (1.0,0.0,0.0,t[0], 0.0,1.0,0.0,t[1], 0.0,0.0,1.0,t[2], 0.0,0.0,0.0,1.0)
	r = quaternionToMatrix(quaternionInverse(q))
	R = (r[0],r[1],r[2],0.0, r[3],r[4],r[5],0.0, r[6],r[7],r[8],0.0, 0.0,0.0,0.0,1.0)
	return matrixMult4x4(T,R)

def buildInverseTransformationMatrix(t, q):
	T = (1.0,0.0,0.0,-t[0], 0.0,1.0,0.0,-t[1], 0.0,0.0,1.0,-t[2], 0.0,0.0,0.0,1.0)
	r = quaternionToMatrix(q)
	R = (r[0],r[1],r[2],0.0, r[3],r[4],r[5],0.0, r[6],r[7],r[8],0.0, 0.0,0.0,0.0,1.0)
	return matrixMult4x4(R,T)

def main():

	if pfpy.getNumCameras() > 0 and pfpy.getNumMeshes() > 0 :

		# fetch the first camera and mesh
		cam0 = pfpy.getCameraRef(0)
		mesh0 = pfpy.getMeshRef(0)

		inp = cam0.getInPoint()
		outp = cam0.getOutPoint()

		# take copies to read from safely
		c = cam0.copy()
		m = mesh0.copy()
		
		# keep the camera in position in the first frame, but transfer the relative object motion in other frames to the camera
		objT0 = m.getTranslation(inp)
		objQ0 = m.getQuaternionRotation(inp)
		objM0 = buildTransformationMatrix(objT0, objQ0)
			
		f= inp+1
		while (f <= outp) :

			# object pose in this frame 
			objT = m.getTranslation(f)
			objQ = m.getQuaternionRotation(f)
			objiM = buildInverseTransformationMatrix(objT, objQ)
			
			# map the relative camera position back to the object in the first frame
			t = vectorMult4x4(objM0, vectorMult4x4(objiM, c.getTranslation(f)))

			# and likewise for the camera rotation
			q = quaternionNormalize(quaternionMult(c.getQuaternionRotation(f), quaternionMult(quaternionInverse(objQ), objQ0)))

			# position the camera
			cam0.setTranslation(f, t)
			cam0.setQuaternionRotation(f, q)

			# object is no longer moving
			mesh0.setTranslation(f, objT0)
			mesh0.setQuaternionRotation(f, objQ0)

			print('Positioned camera in frame %d'%f)
			f += 1

		# cleanup
		c.freeCopy()
		m.freeCopy()