"""
ArcBall.py -- Math utilities, vector, matrix types and ArcBall quaternion rotation class

>>> unit_test_ArcBall_module ()
unit testing ArcBall
Quat for first drag
[ 0.08438914 -0.08534209 -0.06240178  0.99080837]
First transform
[[ 0.97764552 -0.1380603   0.15858325  0.        ]
 [ 0.10925253  0.97796899  0.17787792  0.        ]
 [-0.17964739 -0.15657592  0.97119039  0.        ]
 [ 0.          0.          0.          1.        ]]
LastRot at end of first drag
[[ 0.97764552 -0.1380603   0.15858325]
 [ 0.10925253  0.97796899  0.17787792]
 [-0.17964739 -0.15657592  0.97119039]]
Quat for second drag
[ 0.00710336  0.31832787  0.02679029  0.94757545]
Second transform
[[ 0.88022292 -0.08322023 -0.46720669  0.        ]
 [ 0.14910145  0.98314685  0.10578787  0.        ]
 [ 0.45052907 -0.16277808  0.8777966   0.        ]
 [ 0.          0.          0.          1.00000001]]
"""

import Numeric
import copy
from math import sqrt

# //assuming IEEE-754(GLfloat), which i believe has max precision of 7 bits
Epsilon = 1.0e-5


class ArcBallT:
	def __init__ (self, NewWidth, NewHeight):
		self.m_StVec = Vector3fT ()
		self.m_EnVec = Vector3fT ()
		self.m_AdjustWidth = 1.0
		self.m_AdjustHeight = 1.0
		self.setBounds (NewWidth, NewHeight)

	def __str__ (self):
		str_rep = ""
		str_rep += "StVec = " + str (self.m_StVec)
		str_rep += "\nEnVec = " + str (self.m_EnVec)
		str_rep += "\n scale coords %f %f" % (self.m_AdjustWidth, self.m_AdjustHeight)
		return str_rep

	def setBounds (self, NewWidth, NewHeight):
		# //Set new bounds
		assert (NewWidth > 1.0 and NewHeight > 1.0), "Invalid width or height for bounds."
		# //Set adjustment factor for width/height
		self.m_AdjustWidth = 1.0 / ((NewWidth - 1.0) * 0.5)
		self.m_AdjustHeight = 1.0 / ((NewHeight - 1.0) * 0.5)

	def _mapToSphere (self, NewPt):
		# Given a new window coordinate, will modify NewVec in place
		X = 0
		Y = 1
		Z = 2

		NewVec = Vector3fT ()
		# //Copy paramter into temp point
		TempPt = copy.copy (NewPt)
		# //Adjust point coords and scale down to range of [-1 ... 1]
		TempPt [X] = (NewPt [X] * self.m_AdjustWidth) - 1.0
		TempPt [Y] = 1.0 - (NewPt [Y] * self.m_AdjustHeight)
		# //Compute the square of the length of the vector to the point from the center
		length = sum (Numeric.dot (TempPt, TempPt))
		# //If the point is mapped outside of the sphere... (length > radius squared)
		if (length > 1.0):
			# //Compute a normalizing factor (radius / sqrt(length))
			norm    = 1.0 / sqrt (length);

			# //Return the "normalized" vector, a point on the sphere
			NewVec [X] = TempPt [X] * norm;
			NewVec [Y] = TempPt [Y] * norm;
			NewVec [Z] = 0.0;
		else:			# //Else it's on the inside
			# //Return a vector to a point mapped inside the sphere sqrt(radius squared - length)
			NewVec [X] = TempPt [X]
			NewVec [Y] = TempPt [Y]
			NewVec [Z] = sqrt (1.0 - length)

		return NewVec

	def click (self, NewPt):
		# //Mouse down (Point2fT
		self.m_StVec = self._mapToSphere (NewPt)
		return

	def drag (self, NewPt):
		# //Mouse drag, calculate rotation (Point2fT Quat4fT)
		""" drag (Point2fT mouse_coord) -> new_quaternion_rotation_vec
		"""
		X = 0
		Y = 1
		Z = 2
		W = 3

		self.m_EnVec = self._mapToSphere (NewPt)

		# //Compute the vector perpendicular to the begin and end vectors
		# Perp = Vector3fT ()
		Perp = Vector3fCross(self.m_StVec, self.m_EnVec);

		NewRot = Quat4fT ()
		# //Compute the length of the perpendicular vector
		if (Vector3fLength(Perp) > Epsilon):		#    //if its non-zero
			# //We're ok, so return the perpendicular vector as the transform after all
			NewRot[X] = Perp[X];
			NewRot[Y] = Perp[Y];
			NewRot[Z] = Perp[Z];
			# //In the quaternion values, w is cosine (theta / 2), where theta is rotation angle
			NewRot[W] = Vector3fDot(self.m_StVec, self.m_EnVec);
		else:		#                            //if its zero
			# //The begin and end vectors coincide, so return a quaternion of zero matrix (no rotation)
			NewRot.X = NewRot.Y = NewRot.Z = NewRot.W = 0.0;
			
		return NewRot


# ##################### Math utility ##########################################


def Matrix4fT ():
	return Numeric.identity (4, 'f')

def Matrix3fT ():
	return Numeric.identity (3, 'f')

def Quat4fT ():
	return Numeric.zeros (4, 'f')

def Vector3fT ():
	return Numeric.zeros (3, 'f')

def Point2fT (x = 0.0, y = 0.0):
	pt = Numeric.zeros (2, 'f')
	pt [0] = x
	pt [1] = y
	return pt

def Vector3fDot(u, v):
	# Dot product of two 3f vectors
	dotprod = Numeric.dot (u,v)
	return dotprod

def Vector3fCross(u, v):
	# Cross product of two 3f vectors
	X = 0
	Y = 1
	Z = 2
	cross = Numeric.zeros (3, 'f')
	cross [X] = (u[Y] * v[Z]) - (u[Z] * v[Y])
	cross [Y] = (u[Z] * v[X]) - (u[X] * v[Z])
	cross [Z] = (u[X] * v[Y]) - (u[Y] * v[X])
	return cross

def Vector3fLength (u):
	mag_squared = sum(Numeric.dot (u,u))
	mag = sqrt (mag_squared)
	return mag
	
def Matrix3fSetIdentity ():
	return Numeric.identity (3, 'f')

def Matrix3fMulMatrix3f (matrix_a, matrix_b):
	return Numeric.matrixmultiply (matrix_a, matrix_b)




def Matrix4fSVD (NewObj):
	X = 0
	Y = 1
	Z = 2
	s = sqrt ( 
		( (NewObj [X][X] * NewObj [X][X]) + (NewObj [X][Y] * NewObj [X][Y]) + (NewObj [X][Z] * NewObj [X][Z]) +
		(NewObj [Y][X] * NewObj [Y][X]) + (NewObj [Y][Y] * NewObj [Y][Y]) + (NewObj [Y][Z] * NewObj [Y][Z]) +
		(NewObj [Z][X] * NewObj [Z][X]) + (NewObj [Z][Y] * NewObj [Z][Y]) + (NewObj [Z][Z] * NewObj [Z][Z]) ) / 3.0 )
	return s

def Matrix4fSetRotationScaleFromMatrix3f(NewObj, three_by_three_matrix):
	# Modifies NewObj in-place by replacing its upper 3x3 portion from the 
	# passed in 3x3 matrix.
	# NewObj = Matrix4fT ()
	NewObj [0:3,0:3] = three_by_three_matrix
	return NewObj

# /**
# * Sets the rotational component (upper 3x3) of this matrix to the matrix
# * values in the T precision Matrix3d argument; the other elements of
# * this matrix are unchanged; a singular value decomposition is performed
# * on this object's upper 3x3 matrix to factor out the scale, then this
# * object's upper 3x3 matrix components are replaced by the passed rotation
# * components, and then the scale is reapplied to the rotational
# * components.
# * @param three_by_three_matrix T precision 3x3 matrix
# */
def Matrix4fSetRotationFromMatrix3f (NewObj, three_by_three_matrix):
	scale = Matrix4fSVD (NewObj)

	NewObj = Matrix4fSetRotationScaleFromMatrix3f(NewObj, three_by_three_matrix);
	scaled_NewObj = NewObj * scale			 # Matrix4fMulRotationScale(NewObj, scale);
	return scaled_NewObj

def Matrix3fSetRotationFromQuat4f (q1):
	# Converts the H quaternion q1 into a new equivalent 3x3 rotation matrix. 
	X = 0
	Y = 1
	Z = 2
	W = 3

	NewObj = Matrix3fT ()
	n = sum (Numeric.dot (q1, q1))
	s = 0.0
	if (n > 0.0):
		s = 2.0 / n
	xs = q1 [X] * s;  ys = q1 [Y] * s;  zs = q1 [Z] * s
	wx = q1 [W] * xs; wy = q1 [W] * ys; wz = q1 [W] * zs
	xx = q1 [X] * xs; xy = q1 [X] * ys; xz = q1 [X] * zs
	yy = q1 [Y] * ys; yz = q1 [Y] * zs; zz = q1 [Z] * zs
	# This math all comes about by way of algebra, complex math, and trig identities.
	# See Lengyel pages 88-92
	NewObj [X][X] = 1.0 - (yy + zz);	NewObj [Y][X] = xy - wz; 			NewObj [Z][X] = xz + wy;
	NewObj [X][Y] =       xy + wz; 		NewObj [Y][Y] = 1.0 - (xx + zz);	NewObj [Z][Y] = yz - wx;
	NewObj [X][Z] =       xz - wy; 		NewObj [Y][Z] = yz + wx;          	NewObj [Z][Z] = 1.0 - (xx + yy)

	return NewObj






def unit_test_ArcBall_module ():
	# Unit testing of the ArcBall calss and the real math behind it.
	# Simulates a click and drag followed by another click and drag.
	print "unit testing ArcBall"
	Transform = Matrix4fT ()
	LastRot = Matrix3fT ()
	ThisRot = Matrix3fT ()

	ArcBall = ArcBallT (640, 480)
	# print "The ArcBall with NO click"
	# print ArcBall
	# First click
	LastRot = copy.copy (ThisRot)
	mouse_pt = Point2fT (500,250)
	ArcBall.click (mouse_pt)
	# print "The ArcBall with first click"
	# print ArcBall
	# First drag
	mouse_pt = Point2fT (475, 275)
	ThisQuat = ArcBall.drag (mouse_pt)
	# print "The ArcBall after first drag"
	# print ArcBall
	# print
	# print
	print "Quat for first drag"
	print ThisQuat
	ThisRot = Matrix3fSetRotationFromQuat4f (ThisQuat)
	# Linear Algebra matrix multiplication A = old, B = New : C = A * B
	ThisRot = Matrix3fMulMatrix3f (LastRot, ThisRot)
	Transform = Matrix4fSetRotationFromMatrix3f (Transform, ThisRot)
	print "First transform"
	print Transform
	# Done with first drag


	# second click
	LastRot = copy.copy (ThisRot)
	print "LastRot at end of first drag"
	print LastRot
	mouse_pt = Point2fT (350,260)
	ArcBall.click (mouse_pt)
	# second drag
	mouse_pt = Point2fT (450, 260)
	ThisQuat = ArcBall.drag (mouse_pt)
	# print "The ArcBall"
	# print ArcBall
	print "Quat for second drag"
	print ThisQuat
	ThisRot = Matrix3fSetRotationFromQuat4f (ThisQuat)
	ThisRot = Matrix3fMulMatrix3f (LastRot, ThisRot)
	# print ThisRot
	Transform = Matrix4fSetRotationFromMatrix3f (Transform, ThisRot)
	print "Second transform"
	print Transform
	# Done with second drag
	LastRot = copy.copy (ThisRot)

def _test ():
	# This will run doctest's unit testing capability.
	# see http://www.python.org/doc/current/lib/module-doctest.html
	#
	# doctest introspects the ArcBall module for all docstrings
	# that look like interactive python sessions and invokes
	# the same commands then and there as unit tests to compare
	# the output generated. Very nice for unit testing and
	# documentation.
	import doctest, ArcBall
	return doctest.testmod (ArcBall)

if __name__ == "__main__":
	# Invoke our function that runs python's doctest unit testing tool.
	_test ()

# unit_test ()