Implementing Quaternions

Okay, my quat code worked right off the bat. Run this in Blitz3D and you will see it produces identical results:

Graphics3D 640,480,32,2

Dim mat#(3,3)


SeedRnd MilliSecs()

e=CreatePivot()

x#=Rnd(-180,180)
y#=Rnd(-180,180)
z#=Rnd(-180,180)

RotateEntity e,x,y,z
RotateMatrix x,y,z

displaymatrix e

WaitKey
End

Function DisplayMatrix(e)
	Print "Blitz3D:"
	Print GetMatElement(e,0,0)+", "+GetMatElement(e,0,1)+", "+GetMatElement(e,0,2)
	Print GetMatElement(e,1,0)+", "+GetMatElement(e,1,1)+", "+GetMatElement(e,1,2)
	Print GetMatElement(e,2,0)+", "+GetMatElement(e,2,1)+", "+GetMatElement(e,2,2)
	Print GetMatElement(e,3,0)+", "+GetMatElement(e,3,1)+", "+GetMatElement(e,3,2)
	Print ""
	Print "My matrix:"
	Print mat(0,0)+", "+mat(0,1)+", "+mat(0,2)
	Print mat(1,0)+", "+mat(1,1)+", "+mat(1,2)
	Print mat(2,0)+", "+mat(2,1)+", "+mat(2,2)
	Print mat(3,0)+", "+mat(3,1)+", "+mat(3,2)
End Function

Function RotateMatrix(x#,y#,z#)
	EulerAsQuat x,y,z
	RotateMatrix4 VECTOR_X,VECTOR_Y,VECTOR_Z,VECTOR_W
End Function

Function PositionMatrix(x#,y#,z#)
	mat(3,0)=x
	mat(3,1)=y
	mat(3,2)=z
End Function

Function RotateMatrix4(x#,y#,z#,w#)
	mat(0,0)=(1.0-2.0*x*x-2.0*z*z)
	mat(1,0)=-(2.0*z*y-2.0*w*x)
	mat(2,0)=-(2.0*x*y+2.0*w*z)
	mat(0,1)=-(2.0*z*y+2.0*w*x)
	mat(1,1)=(1.0-2.0*y*y-2.0*x*x)
	mat(2,1)=-(2.0*w*y-2.0*x*z)
	mat(0,2)=2.0*w*z-2.0*x*y
	mat(1,2)=2.0*x*z+2.0*w*y
	mat(2,2)=1.0-2.0*z*z-2.0*y*y
End Function

Global VECTOR_X#
Global VECTOR_Y#
Global VECTOR_Z#
Global VECTOR_W#

Function EulerAsQuat(pitch#,yaw#,roll#)
	cr#=Cos(-roll#/2.0)
	cp#=Cos(pitch#/2.0)
	cy#=Cos(yaw#/2.0)
	sr#=Sin(-roll#/2.0)
	sp#=Sin(pitch#/2.0)
	sy#=Sin(yaw#/2.0)
	cpcy#=cp#*cy#
	spsy#=sp#*sy#
	spcy#=sp#*cy#
	cpsy#=cp#*sy#
	VECTOR_w#=cr#*cpcy#+sr#*spsy#
	VECTOR_x#=sr#*cpcy#-cr#*spsy#
	VECTOR_y#=cr#*spcy#+sr#*cpsy#
	VECTOR_z#=cr#*cpsy#-sr#*spcy#
End Function

Now added additional matrix rotation methods based on quaternions:

	'Uses Quaternion instead of Eulers
	Method TForm4(px#,py#,pz#,qx#,qy#,qz#,w#,sx#,sy#,sz#)
	
		' identity
		grid[0,3]=0.0
		grid[1,3]=0.0
		grid[2,3]=0.0
		grid[3,3]=1.0
	
		' translate
		grid[3,0] = px#
		grid[3,1] = py#
		grid[3,2] = pz#
	
		' rotate + scale
		grid[0,0]=(1.0-2.0*qx*qx-2.0*qz*qz)*sx
		grid[1,0]=-(2.0*qz*qy-2.0*w*qx)*sx
		grid[2,0]=-(2.0*qx*qy+2.0*w*qz)*sx
		
		grid[0,1]=-(2.0*qz*qy+2.0*w*qx)*sy
		grid[1,1]=(1.0-2.0*qy*qy-2.0*qx*qx)*sy
		grid[2,1]=-(2.0*w*qy-2.0*qx*qz)*sy
		
		grid[0,2]=(2.0*w*qz-2.0*qx*qy)*sz
		grid[1,2]=(2.0*qx*qz+2.0*w*qy)*sz
		grid[2,2]=(1.0-2.0*qz*qz-2.0*qy*qy)*sz
	End Method

	
	Method Rotate4(qx#,qy#,qz#,w#)
		
		grid[0,0]=(1.0-2.0*qx*qx-2.0*qz*qz)
		grid[1,0]=-(2.0*qz*qy-2.0*w*qx)
		grid[2,0]=-(2.0*qx*qy+2.0*w*qz)
		
		grid[0,1]=-(2.0*qz*qy+2.0*w*qx)
		grid[1,1]=(1.0-2.0*qy*qy-2.0*qx*qx)
		grid[2,1]=-(2.0*w*qy-2.0*qx*qz)
		
		grid[0,2]=2.0*w*qz-2.0*qx*qy
		grid[1,2]=2.0*qx*qz+2.0*w*qy
		grid[2,2]=1.0-2.0*qz*qz-2.0*qy*qy
		
	EndMethod