Type TVector

	' MiniB3D functions
	
	' Extra - offer extra functionality that Blitz3D does not

	Field x#,y#,z#
	
	Const EPSILON=.0001

	Function Create:TVector(x#,y#,z#)
	
		Local vec:TVector=New TVector
		vec.x=x
		vec.y=y
		vec.z=z
		
		Return vec
		
	End Function
	
	Method Copy:TVector()
	
		Local vec:TVector=New TVector
	
		vec.x=x
		vec.y=y
		vec.z=z
	
		Return vec
	
	End Method
	
	Method Add:TVector(vec:TVector)
	
		Local new_vec:TVector=New TVector
		
		new_vec.x=x+vec.x
		new_vec.y=y+vec.y
		new_vec.z=z+vec.z
		
		Return new_vec
	
	End Method
	
	Method Subtract:TVector(vec:TVector)
	
		Local new_vec:TVector=New TVector
		
		new_vec.x=x-vec.x
		new_vec.y=y-vec.y
		new_vec.z=z-vec.z
		
		Return new_vec
	
	End Method
	
	Method Multiply:TVector(val#)
	
		Local new_vec:TVector=New TVector
		
		new_vec.x=x*val#
		new_vec.y=y*val#
		new_vec.z=z*val#
		
		Return new_vec
	
	End Method
	
	Method Divide:TVector(val#)
	
		Local new_vec:TVector=New TVector
		
		new_vec.x=x/val#
		new_vec.y=y/val#
		new_vec.z=z/val#
		
		Return new_vec
	
	End Method
	
	Method Dot:Float(vec:TVector)
	
		Return (x#*vec.x#)+(y#*vec.y#)+(z#*vec.z#)
	
	End Method
	
	Method Cross:TVector(vec:TVector)
	
		Local new_vec:TVector=New TVector
		
		new_vec.x=(y*vec.z)-(z*vec.y)
		new_vec.y=(z*vec.x)-(x*vec.z)
		new_vec.z=(x*vec.y)-(y*vec.x)
		
		Return new_vec
	
	End Method
	
	Method Normalize()
	
		Local d#=1/Sqr(x*x+y*y+z*z)
		x:*d
		y:*d
		z:*d
		
	End Method
	
	Method Length#()
			
		Return Sqr(x*x+y*y+z*z)

	End Method
	
	Method SquaredLength#()
	
		Return x*x+y*y+z*z

	End Method
	
	Method SetLength#(val#)
	
		Normalize()
		x=x*val
		y=y*val
		z=z*val

	End Method
	
	Method Compare( with:Object )
		Local q:TVector=TVector(with)
		If x-q.x>EPSILON Return 1
		If q.x-x>EPSILON Return -1
		If y-q.y>EPSILON Return 1
		If q.y-y>EPSILON Return -1
		If z-q.z>EPSILON Return 1
		If q.z-z>EPSILON Return -1
		Return 0
	End Method

	' Function by patmaba
	Function VectorYaw#(vx#,vy#,vz#)

		Return ATan2(-vx#,vz#)
	
	End Function

	' Function by patmaba
	Function VectorPitch#(vx#,vy#,vz#)

		Local ang#=ATan2(Sqr(vx#*vx#+vz#*vz#),vy#)-90.0

		If ang#<=0.0001 And ang#>=-0.0001 Then ang#=0
	
		Return ang#
	
	End Function

End Type

Type TMatrix

	Field grid#[4,4]
	
	' MiniB3D functions
	
	' Extra - offer extra functionality that Blitz3D does not
	
	Method LoadIdentity()
	
		grid[0,0]=1.0
		grid[1,0]=0.0
		grid[2,0]=0.0
		grid[3,0]=0.0
		grid[0,1]=0.0
		grid[1,1]=1.0
		grid[2,1]=0.0
		grid[3,1]=0.0
		grid[0,2]=0.0
		grid[1,2]=0.0
		grid[2,2]=1.0
		grid[3,2]=0.0
		
		grid[0,3]=0.0
		grid[1,3]=0.0
		grid[2,3]=0.0
		grid[3,3]=1.0
	
	End Method
	
	' copy - create new copy and returns it
	
	Method Copy:TMatrix()
	
		Local mat:TMatrix=New TMatrix
	
		mat.grid[0,0]=grid[0,0]
		mat.grid[1,0]=grid[1,0]
		mat.grid[2,0]=grid[2,0]
		mat.grid[3,0]=grid[3,0]
		mat.grid[0,1]=grid[0,1]
		mat.grid[1,1]=grid[1,1]
		mat.grid[2,1]=grid[2,1]
		mat.grid[3,1]=grid[3,1]
		mat.grid[0,2]=grid[0,2]
		mat.grid[1,2]=grid[1,2]
		mat.grid[2,2]=grid[2,2]
		mat.grid[3,2]=grid[3,2]
		
		' do not remove
		mat.grid[0,3]=grid[0,3]
		mat.grid[1,3]=grid[1,3]
		mat.grid[2,3]=grid[2,3]
		mat.grid[3,3]=grid[3,3]
		
		Return mat
	
	End Method
	
	' overwrite - overwrites self with matrix passed as parameter
	
	Method Overwrite(mat:TMatrix)
	
		grid[0,0]=mat.grid[0,0]
		grid[1,0]=mat.grid[1,0]
		grid[2,0]=mat.grid[2,0]
		grid[3,0]=mat.grid[3,0]
		grid[0,1]=mat.grid[0,1]
		grid[1,1]=mat.grid[1,1]
		grid[2,1]=mat.grid[2,1]
		grid[3,1]=mat.grid[3,1]
		grid[0,2]=mat.grid[0,2]
		grid[1,2]=mat.grid[1,2]
		grid[2,2]=mat.grid[2,2]
		grid[3,2]=mat.grid[3,2]
		
		grid[0,3]=mat.grid[0,3]
		grid[1,3]=mat.grid[1,3]
		grid[2,3]=mat.grid[2,3]
		grid[3,3]=mat.grid[3,3]
		
	End Method
	
	Method Inverse:TMatrix()

		Local mat:TMatrix=New TMatrix
	
		Local tx#=0
		Local ty#=0
		Local tz#=0
	
	  	' The rotational part of the matrix is simply the transpose of the
	  	' original matrix.
	  	mat.grid[0,0] = grid[0,0]
	  	mat.grid[1,0] = grid[0,1]
	  	mat.grid[2,0] = grid[0,2]
	
		mat.grid[0,1] = grid[1,0]
		mat.grid[1,1] = grid[1,1]
		mat.grid[2,1] = grid[1,2]
	
		mat.grid[0,2] = grid[2,0]
		mat.grid[1,2] = grid[2,1]
		mat.grid[2,2] = grid[2,2]
	
		' The right column vector of the matrix should always be [ 0 0 0 1 ]
		' in most cases. . . you don't need this column at all because it'll 
		' never be used in the program, but since this code is used with GL
		' and it does consider this column, it is here.
		mat.grid[0,3] = 0 
		mat.grid[1,3] = 0
		mat.grid[2,3] = 0
		mat.grid[3,3] = 1
	
		' The translation components of the original matrix.
		tx = grid[3,0]
		ty = grid[3,1]
		tz = grid[3,2]
	
		' Result = -(Tm * Rm) To get the translation part of the inverse
		mat.grid[3,0] = -( (grid[0,0] * tx) + (grid[0,1] * ty) + (grid[0,2] * tz) )
		mat.grid[3,1] = -( (grid[1,0] * tx) + (grid[1,1] * ty) + (grid[1,2] * tz) )
		mat.grid[3,2] = -( (grid[2,0] * tx) + (grid[2,1] * ty) + (grid[2,2] * tz) )
	
		Return mat

	End Method
	
		' unoptimised, unused
	Method Multiply0(mat:TMatrix)
	
		Local new_mat:TMatrix=New TMatrix
	
		Local sum#=0
	
		Local row=0
		Local col=0
	
		For row=0 To 3
			For col=0 To 3
				For Local r=0 To 3
					Local a#=grid#[r,col]
					Local b#=mat.grid#[row,r]
					Local c#=a#*b#
					sum#=sum#+c#
				Next
				new_mat.grid#[row,col]=sum#
				sum#=0
			Next
		Next
	
		For row=0 To 3
			For col=0 To 3
				grid[row,col]=new_mat.grid[row,col]
			Next
		Next
		
	End Method
	
	' optimised
	Method Multiply(mat:TMatrix)
	
		Local m00# = grid#[0,0]*mat.grid#[0,0] + grid#[1,0]*mat.grid#[0,1] + grid#[2,0]*mat.grid#[0,2] + grid#[3,0]*mat.grid#[0,3]
		Local m01# = grid#[0,1]*mat.grid#[0,0] + grid#[1,1]*mat.grid#[0,1] + grid#[2,1]*mat.grid#[0,2] + grid#[3,1]*mat.grid#[0,3]
		Local m02# = grid#[0,2]*mat.grid#[0,0] + grid#[1,2]*mat.grid#[0,1] + grid#[2,2]*mat.grid#[0,2] + grid#[3,2]*mat.grid#[0,3]
		'Local m03# = grid#[0,3]*mat.grid#[0,0] + grid#[1,3]*mat.grid#[0,1] + grid#[2,3]*mat.grid#[0,2] + grid#[3,3]*mat.grid#[0,3]
		Local m10# = grid#[0,0]*mat.grid#[1,0] + grid#[1,0]*mat.grid#[1,1] + grid#[2,0]*mat.grid#[1,2] + grid#[3,0]*mat.grid#[1,3]
		Local m11# = grid#[0,1]*mat.grid#[1,0] + grid#[1,1]*mat.grid#[1,1] + grid#[2,1]*mat.grid#[1,2] + grid#[3,1]*mat.grid#[1,3]
		Local m12# = grid#[0,2]*mat.grid#[1,0] + grid#[1,2]*mat.grid#[1,1] + grid#[2,2]*mat.grid#[1,2] + grid#[3,2]*mat.grid#[1,3]
		'Local m13# = grid#[0,3]*mat.grid#[1,0] + grid#[1,3]*mat.grid#[1,1] + grid#[2,3]*mat.grid#[1,2] + grid#[3,3]*mat.grid#[1,3]
		Local m20# = grid#[0,0]*mat.grid#[2,0] + grid#[1,0]*mat.grid#[2,1] + grid#[2,0]*mat.grid#[2,2] + grid#[3,0]*mat.grid#[2,3]
		Local m21# = grid#[0,1]*mat.grid#[2,0] + grid#[1,1]*mat.grid#[2,1] + grid#[2,1]*mat.grid#[2,2] + grid#[3,1]*mat.grid#[2,3]
		Local m22# = grid#[0,2]*mat.grid#[2,0] + grid#[1,2]*mat.grid#[2,1] + grid#[2,2]*mat.grid#[2,2] + grid#[3,2]*mat.grid#[2,3]
		'Local m23# = grid#[0,3]*mat.grid#[2,0] + grid#[1,3]*mat.grid#[2,1] + grid#[2,3]*mat.grid#[2,2] + grid#[3,3]*mat.grid#[2,3]
		Local m30# = grid#[0,0]*mat.grid#[3,0] + grid#[1,0]*mat.grid#[3,1] + grid#[2,0]*mat.grid#[3,2] + grid#[3,0]*mat.grid#[3,3]
		Local m31# = grid#[0,1]*mat.grid#[3,0] + grid#[1,1]*mat.grid#[3,1] + grid#[2,1]*mat.grid#[3,2] + grid#[3,1]*mat.grid#[3,3]
		Local m32# = grid#[0,2]*mat.grid#[3,0] + grid#[1,2]*mat.grid#[3,1] + grid#[2,2]*mat.grid#[3,2] + grid#[3,2]*mat.grid#[3,3]
		'Local m33# = grid#[0,3]*mat.grid#[3,0] + grid#[1,3]*mat.grid#[3,1] + grid#[2,3]*mat.grid#[3,2] + grid#[3,3]*mat.grid#[3,3]
	
		grid[0,0]=m00
		grid[0,1]=m01
		grid[0,2]=m02
		'grid[0,3]=m03
		grid[1,0]=m10
		grid[1,1]=m11
		grid[1,2]=m12
		'grid[1,3]=m13
		grid[2,0]=m20
		grid[2,1]=m21
		grid[2,2]=m22
		'grid[2,3]=m23
		grid[3,0]=m30
		grid[3,1]=m31
		grid[3,2]=m32
		'grid[3,3]=m33
		
	End Method
	
	Method TForm(px#,py#,pz#,rx#,ry#,rz#,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
		
		Local cos_ang1#=Cos(ry)
		Local sin_ang1#=Sin(ry)

		Local cos_ang2#=Cos(rx)
		Local sin_ang2#=Sin(rx)
		
		Local cos_ang3#=Cos(rz)
		Local sin_ang3#=Sin(rz)

		grid[2,0] = (sin_ang1 * cos_ang2) * sz#
		grid[2,1] = (-sin_ang2) * sz#
		grid[2,2] = (cos_ang1 * cos_ang2) * sz#
	
		grid[1,0] = ((cos_ang1# * -sin_ang3#) + ((sin_ang1 * sin_ang2) * cos_ang3#)) * sy#
		grid[1,1] = (cos_ang2 * cos_ang3#) * sy#
		grid[1,2] = (-sin_ang1# * -sin_ang3#) + ((cos_ang1 * sin_ang2) * cos_ang3#) * sy#

		grid[0,0] = ((cos_ang1# * cos_ang3#) + ((sin_ang1 * sin_ang2) * sin_ang3#)) * sx#
		grid[0,1] = (cos_ang2 * sin_ang3#) * sx#
		grid[0,2] = ((-sin_ang1# * cos_ang3#) + ((cos_ang1 * sin_ang2) * sin_ang3#)) * sx#
	
	End Method

	' unoptimised, unused
	Method Translate0(x#,y#,z#)
	
		Local mat:TMatrix=New TMatrix
	
		mat.grid[0,0]=1
		mat.grid[1,0]=0
		mat.grid[2,0]=0
		mat.grid[3,0]=x#
		mat.grid[0,1]=0
		mat.grid[1,1]=1
		mat.grid[2,1]=0
		mat.grid[3,1]=y#
		mat.grid[0,2]=0
		mat.grid[1,2]=0
		mat.grid[2,2]=1
		mat.grid[3,2]=z#
		
		mat.grid[0,3]=0
		mat.grid[1,3]=0
		mat.grid[2,3]=0
		mat.grid[3,3]=1
		
		Multiply(mat)
	
	End Method
	
	' optimised
	Method Translate(x#,y#,z#)
	
		grid[3,0] = grid#[0,0]*x# + grid#[1,0]*y# + grid#[2,0]*z# + grid#[3,0]
		grid[3,1] = grid#[0,1]*x# + grid#[1,1]*y# + grid#[2,1]*z# + grid#[3,1]
		grid[3,2] = grid#[0,2]*x# + grid#[1,2]*y# + grid#[2,2]*z# + grid#[3,2]

	End Method
		
	' unoptimised, unused
	Method Scale0(x#,y#,z#)
	
		Local mat:TMatrix=New TMatrix
	
		mat.grid[0,0]=x#
		mat.grid[1,0]=0
		mat.grid[2,0]=0
		mat.grid[3,0]=0
		mat.grid[0,1]=0
		mat.grid[1,1]=y#
		mat.grid[2,1]=0
		mat.grid[3,1]=0
		mat.grid[0,2]=0
		mat.grid[1,2]=0
		mat.grid[2,2]=z#
		mat.grid[3,2]=0
		
		mat.grid[0,3]=0
		mat.grid[1,3]=0
		mat.grid[2,3]=0
		mat.grid[3,3]=1
		
		Multiply(mat)
	
	End Method
	
	' optimised
	Method Scale(x#,y#,z#)
	
		grid[0,0] = grid#[0,0]*x#
		grid[0,1] = grid#[0,1]*x#
		grid[0,2] = grid#[0,2]*x#

		grid[1,0] = grid#[1,0]*y#
		grid[1,1] = grid#[1,1]*y#
		grid[1,2] = grid#[1,2]*y#

		grid[2,0] = grid#[2,0]*z#
		grid[2,1] = grid#[2,1]*z#
		grid[2,2] = grid#[2,2]*z# 
	
	End Method

	Method Rotate(rx#,ry#,rz#)
	
		Local cos_ang#,sin_ang#
	
		' yaw
	
		cos_ang#=Cos(ry#)
		sin_ang#=Sin(ry#)
	
		Local m00# = grid#[0,0]*cos_ang + grid#[2,0]*-sin_ang#
		Local m01# = grid#[0,1]*cos_ang + grid#[2,1]*-sin_ang#
		Local m02# = grid#[0,2]*cos_ang + grid#[2,2]*-sin_ang#

		grid[2,0] = grid#[0,0]*sin_ang# + grid#[2,0]*cos_ang
		grid[2,1] = grid#[0,1]*sin_ang# + grid#[2,1]*cos_ang
		grid[2,2] = grid#[0,2]*sin_ang# + grid#[2,2]*cos_ang

		grid[0,0]=m00#
		grid[0,1]=m01#
		grid[0,2]=m02#
		
		' pitch
		
		cos_ang#=Cos(rx#)
		sin_ang#=Sin(rx#)
	
		Local m10# = grid#[1,0]*cos_ang + grid#[2,0]*sin_ang
		Local m11# = grid#[1,1]*cos_ang + grid#[2,1]*sin_ang
		Local m12# = grid#[1,2]*cos_ang + grid#[2,2]*sin_ang

		grid[2,0] = grid#[1,0]*-sin_ang + grid#[2,0]*cos_ang
		grid[2,1] = grid#[1,1]*-sin_ang + grid#[2,1]*cos_ang
		grid[2,2] = grid#[1,2]*-sin_ang + grid#[2,2]*cos_ang

		grid[1,0]=m10
		grid[1,1]=m11
		grid[1,2]=m12
		
		' roll
		
		cos_ang#=Cos(rz#)
		sin_ang#=Sin(rz#)

		m00# = grid#[0,0]*cos_ang# + grid#[1,0]*sin_ang#
		m01# = grid#[0,1]*cos_ang# + grid#[1,1]*sin_ang#
		m02# = grid#[0,2]*cos_ang# + grid#[1,2]*sin_ang#

		grid[1,0] = grid#[0,0]*-sin_ang# + grid#[1,0]*cos_ang#
		grid[1,1] = grid#[0,1]*-sin_ang# + grid#[1,1]*cos_ang#
		grid[1,2] = grid#[0,2]*-sin_ang# + grid#[1,2]*cos_ang#

		grid[0,0]=m00#
		grid[0,1]=m01#
		grid[0,2]=m02#
	
	End Method

	' unoptimised, unused
	Method RotatePitch0(ang#)
	
		Local mat:TMatrix=New TMatrix
	
		mat.grid[0,0]=1
		mat.grid[1,0]=0
		mat.grid[2,0]=0
		mat.grid[3,0]=0
		mat.grid[0,1]=0
		mat.grid[1,1]=Cos(ang#)
		mat.grid[2,1]=-Sin(ang#)
		mat.grid[3,1]=0
		mat.grid[0,2]=0
		mat.grid[1,2]=Sin(ang#)
		mat.grid[2,2]=Cos(ang#)
		mat.grid[3,2]=0
		
		mat.grid[0,3]=0
		mat.grid[1,3]=0
		mat.grid[2,3]=0
		mat.grid[3,3]=1
		
		Multiply(mat)
	
	End Method
	
	' optimised
	Method RotatePitch(ang#)
	
		Local cos_ang#=Cos(ang#)
		Local sin_ang#=Sin(ang#)
	
		Local m10# = grid#[1,0]*cos_ang + grid#[2,0]*sin_ang
		Local m11# = grid#[1,1]*cos_ang + grid#[2,1]*sin_ang
		Local m12# = grid#[1,2]*cos_ang + grid#[2,2]*sin_ang

		grid[2,0] = grid#[1,0]*-sin_ang + grid#[2,0]*cos_ang
		grid[2,1] = grid#[1,1]*-sin_ang + grid#[2,1]*cos_ang
		grid[2,2] = grid#[1,2]*-sin_ang + grid#[2,2]*cos_ang

		grid[1,0]=m10
		grid[1,1]=m11
		grid[1,2]=m12

	End Method
	
	' unoptimised, unused
	Method RotateYaw0(ang#)
	
		Local mat:TMatrix=New TMatrix
	
		mat.grid[0,0]=Cos(ang#)
		mat.grid[1,0]=0
		mat.grid[2,0]=Sin(ang#)
		mat.grid[3,0]=0
		mat.grid[0,1]=0
		mat.grid[1,1]=1
		mat.grid[2,1]=0
		mat.grid[3,1]=0
		mat.grid[0,2]=-Sin(ang#)
		mat.grid[1,2]=0
		mat.grid[2,2]=Cos(ang#)
		mat.grid[3,2]=0
		
		mat.grid[0,3]=0
		mat.grid[1,3]=0
		mat.grid[2,3]=0
		mat.grid[3,3]=1
		
		Multiply(mat)
	
	End Method
	
	' optimised
	Method RotateYaw(ang#)
	
		Local cos_ang#=Cos(ang#)
		Local sin_ang#=Sin(ang#)
	
		Local m00# = grid#[0,0]*cos_ang + grid#[2,0]*-sin_ang#
		Local m01# = grid#[0,1]*cos_ang + grid#[2,1]*-sin_ang#
		Local m02# = grid#[0,2]*cos_ang + grid#[2,2]*-sin_ang#

		grid[2,0] = grid#[0,0]*sin_ang# + grid#[2,0]*cos_ang
		grid[2,1] = grid#[0,1]*sin_ang# + grid#[2,1]*cos_ang
		grid[2,2] = grid#[0,2]*sin_ang# + grid#[2,2]*cos_ang

		grid[0,0]=m00#
		grid[0,1]=m01#
		grid[0,2]=m02#

	End Method
	
	' unoptimised, unused
	Method RotateRoll0(ang#)
	
		Local mat:TMatrix=New TMatrix
	
		mat.grid[0,0]=Cos(ang#)
		mat.grid[1,0]=-Sin(ang#)
		mat.grid[2,0]=0
		mat.grid[3,0]=0
		mat.grid[0,1]=Sin(ang#)
		mat.grid[1,1]=Cos(ang#)
		mat.grid[2,1]=0
		mat.grid[3,1]=0
		mat.grid[0,2]=0
		mat.grid[1,2]=0
		mat.grid[2,2]=1
		mat.grid[3,2]=0
		
		mat.grid[0,3]=0
		mat.grid[1,3]=0
		mat.grid[2,3]=0
		mat.grid[3,3]=1
		
		Multiply(mat)
	
	End Method
	
	' optimised
	Method RotateRoll(ang#)
	
		Local cos_ang#=Cos(ang#)
		Local sin_ang#=Sin(ang#)

		Local m00# = grid#[0,0]*cos_ang# + grid#[1,0]*sin_ang#
		Local m01# = grid#[0,1]*cos_ang# + grid#[1,1]*sin_ang#
		Local m02# = grid#[0,2]*cos_ang# + grid#[1,2]*sin_ang#

		grid[1,0] = grid#[0,0]*-sin_ang# + grid#[1,0]*cos_ang#
		grid[1,1] = grid#[0,1]*-sin_ang# + grid#[1,1]*cos_ang#
		grid[1,2] = grid#[0,2]*-sin_ang# + grid#[1,2]*cos_ang#

		grid[0,0]=m00#
		grid[0,1]=m01#
		grid[0,2]=m02#

	End Method
		
End Type

Type TQuaternion

	Field w#,x#,y#,z#

	' MiniB3D functions
	
	' Extra - offer extra functionality that Blitz3D does not

	Function QuatToMat(w#,x#,y#,z#,mat:TMatrix Var)
	
		Local q:Float[4]
		q[0]=w
		q[1]=x
		q[2]=y
		q[3]=z
		
		Local xx#=q[1]*q[1]
		Local yy#=q[2]*q[2]
		Local zz#=q[3]*q[3]
		Local xy#=q[1]*q[2]
		Local xz#=q[1]*q[3]
		Local yz#=q[2]*q[3]
		Local wx#=q[0]*q[1]
		Local wy#=q[0]*q[2]
		Local wz#=q[0]*q[3]
	
		mat.grid[0,0]=1-2*(yy+zz)
		mat.grid[0,1]=  2*(xy-wz)
		mat.grid[0,2]=  2*(xz+wy)
		mat.grid[1,0]=  2*(xy+wz)
		mat.grid[1,1]=1-2*(xx+zz)
		mat.grid[1,2]=  2*(yz-wx)
		mat.grid[2,0]=  2*(xz-wy)
		mat.grid[2,1]=  2*(yz+wx)
		mat.grid[2,2]=1-2*(xx+yy)
		mat.grid[3,3]=1
	
		For Local iy=0 To 3
			For Local ix=0 To 3
				xx#=mat.grid[ix,iy]
				If xx#<0.0001 And xx#>-0.0001 Then xx#=0
				mat.grid[ix,iy]=xx#
			Next
		Next
	
	End Function
	
	Function QuatToEuler(w#,x#,y#,z#,pitch# Var,yaw# Var,roll# Var)
	
		Local q:Float[4]
		q[0]=w
		q[1]=x
		q[2]=y
		q[3]=z
		
		Local xx#=q[1]*q[1]
		Local yy#=q[2]*q[2]
		Local zz#=q[3]*q[3]
		Local xy#=q[1]*q[2]
		Local xz#=q[1]*q[3]
		Local yz#=q[2]*q[3]
		Local wx#=q[0]*q[1]
		Local wy#=q[0]*q[2]
		Local wz#=q[0]*q[3]
	
		Local mat:TMatrix=New TMatrix
		
		mat.grid[0,0]=1-2*(yy+zz)
		mat.grid[0,1]=  2*(xy-wz)
		mat.grid[0,2]=  2*(xz+wy)
		mat.grid[1,0]=  2*(xy+wz)
		mat.grid[1,1]=1-2*(xx+zz)
		mat.grid[1,2]=  2*(yz-wx)
		mat.grid[2,0]=  2*(xz-wy)
		mat.grid[2,1]=  2*(yz+wx)
		mat.grid[2,2]=1-2*(xx+yy)
		mat.grid[3,3]=1
	
		For Local iy=0 To 3
			For Local ix=0 To 3
				xx#=mat.grid[ix,iy]
				If xx#<0.0001 And xx#>-0.0001 Then xx#=0
				mat.grid[ix,iy]=xx#
			Next
		Next
	
		pitch#=ATan2( mat.grid[2,1],Sqr( mat.grid[2,0]*mat.grid[2,0]+mat.grid[2,2]*mat.grid[2,2] ) )
		yaw#=ATan2(mat.grid[2,0],mat.grid[2,2])
		roll#=ATan2(mat.grid[0,1],mat.grid[1,1])
				
		'If pitch#=nan# Then pitch#=0
		'If yaw#  =nan# Then yaw#  =0
		'If roll# =nan# Then roll# =0
	
	End Function
			
	Function Slerp(Ax#,Ay#,Az#,Aw#,Bx#,By#,Bz#,Bw#,Cx# Var,Cy# Var,Cz# Var,Cw# Var,t#)
	
		If Abs(ax-bx)<0.001 And Abs(ay-by)<0.001 And Abs(az-bz)<0.001 And Abs(aw-bw)<0.001
			cx#=ax
			cy#=ay
			cz#=az
			cw#=aw
			Return True
		EndIf
		
		Local cosineom#=Ax#*Bx#+Ay#*By#+Az#*Bz#+Aw#*Bw#
		Local scaler_w#
		Local scaler_x#
		Local scaler_y#
		Local scaler_z#
		
		If cosineom# <= 0.0
			cosineom#=-cosineom#
			scaler_w#=-Bw#
			scaler_x#=-Bx#
			scaler_y#=-By#
			scaler_z#=-Bz#
		Else
			scaler_w#=Bw#
			scaler_x#=Bx#
			scaler_y#=By#
			scaler_z#=Bz#
		EndIf
		
		Local scale0#
		Local scale1#
		
		If (1.0 - cosineom#)>0.0001
			Local omega#=ACos(cosineom#)
			Local sineom#=Sin(omega#)
			scale0#=Sin((1.0-t#)*omega#)/sineom#
			scale1#=Sin(t#*omega#)/sineom#
		Else
			scale0#=1.0-t#
			scale1#=t#
		EndIf
			
		cw#=scale0#*Aw#+scale1#*scaler_w#
		cx#=scale0#*Ax#+scale1#*scaler_x#
		cy#=scale0#*Ay#+scale1#*scaler_y#
		cz#=scale0#*Az#+scale1#*scaler_z#
		
	End Function
		
End Type

Type TPlane

	Field equation#[4]
	Field origin:TVector
	Field normal:TVector
	Field d# ' dot product of plane
	
	Const PLANE_THICKNESS_EPSILON#=0.0001
	Const POINT_IN_FRONT_OF_PLANE=1
	Const POINT_BEHIND_PLANE=-1
	Const POINT_ON_PLANE=0
	
	Const POLYGON_STRADDLING_PLANE=2
	Const POLYGON_IN_FRONT_OF_PLANE=1
	Const POLYGON_BEHIND_PLANE=-1
	Const POLYGON_COPLANAR_WITH_PLANE=0
	
	Const TRIANGLE_STRADDLING_PLANE=2
	Const TRIANGLE_IN_FRONT_OF_PLANE=1
	Const TRIANGLE_BEHIND_PLANE=-1
	Const TRIANGLE_COPLANAR_WITH_PLANE=0

	' MiniB3D functions
	
	' Internal - not recommended for general use

	Function CreatePlane:TPlane(plane_origin:TVector,plane_normal:TVector)
		Local plane:TPlane=New TPlane
		plane.normal=plane_normal.Copy()
		plane.origin=plane_origin.Copy()
		plane.equation[0] = plane_normal.x
		plane.equation[1] = plane_normal.y
		plane.equation[2] = plane_normal.z
		plane.equation[3] = -(plane_normal.x*plane_origin.x+plane_normal.y*plane_origin.y+plane_normal.z*plane_origin.z)
		plane.d=plane.normal.Dot(plane.origin)
		Return plane
	End Function
	
	Function CreatePlaneFromTriangle:TPlane(p1:TVector,p2:TVector,p3:TVector)
		Local plane:TPlane=New TPlane
		plane.normal = (p2.Subtract(p1)).Cross(p3.Subtract(p1))
		plane.normal.Normalize()
		plane.origin=p1.Copy()
		plane.equation[0] = plane.normal.x
		plane.equation[1] = plane.normal.y
		plane.equation[2] = plane.normal.z
		plane.equation[3] = -(plane.normal.x*plane.origin.x+plane.normal.y*plane.origin.y+plane.normal.z*plane.origin.z)
		plane.d=plane.normal.Dot(plane.origin)
		Return plane
	End Function
	
	Method isFrontFacingTo(direction:TVector)
		Local dot# = normal.Dot(direction)
		Return dot <= 0
	End Method
	
	Method signedDistanceTo!(point:TVector)
		Return (point.Dot(normal)) + equation[3]
	End Method
	
	Method ClassifyPointToPlane(point:TVector)

		Local dist#=signedDistanceTo(point)

		If dist>PLANE_THICKNESS_EPSILON
			Return POINT_IN_FRONT_OF_PLANE
		Else
			If dist<-PLANE_THICKNESS_EPSILON
				Return POINT_BEHIND_PLANE
			Else
				Return POINT_ON_PLANE
			EndIf
		EndIf 

	End Method
	
	Method ClassifyPolygonToPlane(poly:TPolygon)
	
		Local numInFront=0
		Local numBehind=0
		
		Local numVerts=poly.no_verts
		
		For Local i=0 Until numVerts
		
			Local p:TVector=poly.GetVertex(i)
			
			Select ClassifyPointToPlane(p)
				Case POINT_IN_FRONT_OF_PLANE
					numInFront:+1
				Case POINT_BEHIND_PLANE
					numBehind:+1
			End Select
		
		Next
		
		If numBehind<>0 And numInFront<>0 Then Return POLYGON_STRADDLING_PLANE
		If numInFront<>0 Return POLYGON_IN_FRONT_OF_PLANE
		If numBehind<>0 Return POLYGON_BEHIND_PLANE
		
		Return POLYGON_COPLANAR_WITH_PLANE
	
	End Method
	
	Method ClassifyTriangleToPlane(a:TVector,b:TVector,c:TVector)
	
		Local numInFront=0
		Local numBehind=0
		
		If ClassifyPointToPlane(a)=POINT_IN_FRONT_OF_PLANE Then numInFront:+1
		If ClassifyPointToPlane(b)=POINT_IN_FRONT_OF_PLANE Then numInFront:+1
		If ClassifyPointToPlane(c)=POINT_IN_FRONT_OF_PLANE Then numInFront:+1
		
		If ClassifyPointToPlane(a)=POINT_BEHIND_PLANE Then numBehind:+1
		If ClassifyPointToPlane(b)=POINT_BEHIND_PLANE Then numBehind:+1
		If ClassifyPointToPlane(c)=POINT_BEHIND_PLANE Then numBehind:+1
		
		If numBehind<>0 And numInFront<>0 Then Return TRIANGLE_STRADDLING_PLANE
		If numInFront<>0 Return TRIANGLE_IN_FRONT_OF_PLANE
		If numBehind<>0 Return TRIANGLE_BEHIND_PLANE
		
		Return TRIANGLE_COPLANAR_WITH_PLANE
	
	End Method

	Method ClosestPointTo:TVector(p:TVector)

		Local t#=normal.Dot(p)-d ' if plane equation not normalised, append " / normal.Dot(normal)"
		Return p.Subtract(normal.Multiply(t))

	End Method
	
	Function IntersectEdgeAgainstPlane:TVector(a:TVector,b:TVector,p:TPlane)

		' Compute the t value for the directed line ab intersecting the plane
		Local ab:TVector=b.Subtract(a)
		Local t#=(p.d-p.normal.Dot(a))/p.normal.Dot(ab)
	
		' If t in [0..1] compute and return intersection point
		If t>=0.0 And t<=1.0
			Local q:TVector=a.Add(ab.Multiply(t))
			Return q
		EndIf
		
		' Else no intersection
		Return Null

	End Function
	
	Function IntersectSegmentPlane(a:TVector,b:TVector,p:TPlane,t# Var,q:TVector Var)
	
		' Compute the t value for the directed line ab intersecting the plane
		Local ab:TVector=b.Subtract(a)
		t#=(p.d-p.normal.Dot(a))/p.normal.Dot(ab)
	
		' If t in [0..1] compute and return intersection point
		If t>=0.0 And t<=1.0
			q:TVector=a.Add(ab.Multiply(t))
			Return 1
		EndIf
		
		' Else no intersection
		Return 0
	
	End Function

End Type

Type TPolygon

	Field no_verts
	
	Field array_size=1
	Field verts:TVector[array_size]
	
	Field surf:TSurface=Null
	Field tri ' tri no
	
	' MiniB3D functions
	
	' Internal - not recommended for general use
	
	Function MeshToPolygons:TPolygon[](mesh:TMesh)
	
		Local no_polys=0
	
		Local s,surf:TSurface
	
		For s=1 To mesh.CountSurfaces()
	
			surf:TSurface=mesh.GetSurface(s)
	
			no_polys=no_polys+surf.CountTriangles()
	
		Next
		
		Local polys:TPolygon[no_polys]
		Local poly_no=0
	
		For s=1 To mesh.CountSurfaces()
	
			surf:TSurface=mesh.GetSurface(s)
	
			For Local t=0 Until surf.CountTriangles()
			
				Local v0=surf.TriangleVertex(t,0)
				Local v1=surf.TriangleVertex(t,1)
				Local v2=surf.TriangleVertex(t,2)
				
				polys:TPolygon[poly_no]=New TPolygon
	
				polys[poly_no].AddVertex(TVector.Create(surf.VertexX(v0),surf.VertexY(v0),surf.VertexZ(v0)))
				polys[poly_no].AddVertex(TVector.Create(surf.VertexX(v1),surf.VertexY(v1),surf.VertexZ(v1)))
				polys[poly_no].AddVertex(TVector.Create(surf.VertexX(v2),surf.VertexY(v2),surf.VertexZ(v2)))
				polys[poly_no].surf=surf
				polys[poly_no].tri=t
				
				poly_no=poly_no+1
				
			Next
	
		Next
		
		Return polys
		
	End Function

	Function SplitPolygons(polys:TPolygon[],plane:TPlane,front_polys:TPolygon[] Var,back_polys:TPolygon[] Var)
	
		Local no_front_polys=0
		Local no_back_polys=0
	
		For Local poly:TPolygon=EachIn polys
	
			Local front_poly:TPolygon
			Local back_poly:TPolygon
	
			' if tri intersects plane
			SplitPolygon(poly,plane,front_poly,back_poly)
		
			If front_poly<>Null
		
				If no_front_polys>=front_polys.length
					If no_front_polys<>0
						front_polys=front_polys[..no_front_polys*2]
					Else
						front_polys=front_polys[..1]
					EndIf
				EndIf
				front_polys[no_front_polys]=front_poly
				no_front_polys:+1
	
			EndIf
			
			If back_poly<>Null
		
				If no_back_polys>=back_polys.length
					If no_back_polys<>0
						back_polys=back_polys[..no_back_polys*2]
					Else
						back_polys=back_polys[..1]
					EndIf
				EndIf
				back_polys[no_back_polys]=back_poly
				no_back_polys:+1
				
			EndIf
				
		Next
		
		' convert quad polys to tris	
		front_polys=TPolygon.QuadToTris(front_polys)
		back_polys=TPolygon.QuadToTris(back_polys)
	
	End Function
	
	Function SplitPolygon(poly:TPolygon,plane:TPlane,front_poly:TPolygon Var,back_poly:TPolygon Var)
	
		Const MAX_POINTS=10
	
		Local numFront=0,numBack=0
		Local frontVerts:TVector[MAX_POINTS]
		Local backVerts:TVector[MAX_POINTS]
	
		' Test all edges (a, b) starting with edge from last to first vertex
		Local numVerts=poly.no_verts
		Local a:TVector=poly.GetVertex(numVerts-1)
		Local aSide=plane.ClassifyPointToPlane(a)
		
		' Loop over all edges given by vertex pair (n-1, n)
		For Local n=0 Until numVerts
		
			Local b:TVector=poly.GetVertex(n)
			Local bSide=plane.ClassifyPointToPlane(b)
			
			If bSide=TPlane.POINT_IN_FRONT_OF_PLANE
			
				If aSide=TPlane.POINT_BEHIND_PLANE
				
					' Edge (a, b) straddles, output intersection point to both sides
					' Consistently clip edge as ordered going from in front -> behind
					Local i:TVector=TPlane.IntersectEdgeAgainstPlane(b,a,plane)
					If plane.ClassifyPointToPlane(i)<>TPlane.POINT_ON_PLANE Then DebugStop
					numFront:+1
					numBack:+1
					frontVerts[numFront]=i
					backVerts[numBack]=i
					
				EndIf
				
				' In all three cases, output b to the front side
				numFront:+1
				frontVerts[numFront]=b
				
			Else If bSide=TPlane.POINT_BEHIND_PLANE
			
				If aSide=TPlane.POINT_IN_FRONT_OF_PLANE
				
					' Edge (a, b) straddles, output intersection point to both sides
					Local i:TVector=TPlane.IntersectEdgeAgainstPlane(a,b,plane)
					If plane.ClassifyPointToPlane(i)<>TPlane.POINT_ON_PLANE Then DebugStop
					numFront:+1
					numBack:+1
					frontVerts[numFront]=i
					backVerts[numBack]=i
					
				Else If aSide=TPlane.POINT_ON_PLANE
				
					' Output a when edge (a,b) goes from 'on' to 'behind' plane
					numBack:+1
					backVerts[numBack]=a
					
				EndIf
				
				' In all three cases, output b to the back side
				numBack:+1
				backVerts[numBack]=b
				
			Else
			
				' b is on the plane. In all three cases output b to the front side
				numFront:+1
				frontVerts[numFront]=b
				
				' in one case, also output b to the backside
				If aSide=TPlane.POINT_BEHIND_PLANE
					numBack:+1
					backVerts[numBack]=b
				EndIf
				
			EndIf
			
			a=b
			aSide=bSide
				
		Next
		
		front_poly=TPolygon.CreateFromPoints(frontVerts)
		back_poly=TPolygon.CreateFromPoints(backVerts)
		
		If front_poly<>Null
			front_poly.surf=poly.surf
			front_poly.tri=poly.tri
		EndIf
		If back_poly<>Null
			back_poly.surf=poly.surf
			back_poly.tri=poly.tri
		EndIf
	
	End Function
	
	Function CreateFromPoints:TPolygon(points:TVector[])
	
		Local poly:TPolygon=New TPolygon
	
		For Local point:TVector=EachIn points

			poly.AddVertex(point)

		Next
		
		If poly.no_verts>0 Then Return poly Else Return Null

	End Function
	
	Function QuadToTris:TPolygon[](polys:TPolygon[])

		Local no_polys=polys.length

		For Local i=0 Until no_polys

			' if array size exceeds no.of polys, and no polys left, exit loop
			If polys[i]=Null Then Exit

			' quad to 2 tris
			If polys[i].no_verts=4

				Local no_polys2=polys.length

				' new tri
				polys=polys[..no_polys2+1]
				polys[no_polys2]=New TPolygon
				polys[no_polys2].AddVertex(polys[i].verts[0])
				polys[no_polys2].AddVertex(polys[i].verts[2])
				polys[no_polys2].AddVertex(polys[i].verts[3])
				polys[no_polys2].surf=polys[i].surf
				polys[no_polys2].tri=polys[i].tri
				
				' remove vertex from quad to make tri
				polys[i].verts=polys[i].verts[..3]
				polys[i].no_verts=3
				
			EndIf

		Next
		
		Return polys
	
	End Function
	
	Method AddVertex(point:TVector)
	
		verts[no_verts]=point.Copy()
		no_verts:+1
		
		If no_verts=>array_size
			array_size=array_size*2
			verts=verts[..array_size]
		EndIf
			
	End Method
	
	Method GetVertex:TVector(i)
	
		Return verts[i]
			
	End Method

End Type

Type AABB

	Field mini#[3]
	Field maxi#[3]
	
	' MiniB3D functions
	
	' Extra - offers extra functionality that Blitz3D does not
	
	Function AABBAABBIntersection(a:AABB,b:AABB)

		If a.maxi[0]<b.mini[0] Or a.mini[0]>b.maxi[0] Return 0
		If a.maxi[1]<b.mini[1] Or a.mini[1]>b.maxi[1] Return 0
		If a.maxi[2]<b.mini[2] Or a.mini[2]>b.maxi[2] Return 0
		
		Return True

	End Function
	
	Function SphereAABBIntersection(s:TSphere,a:AABB)
	
		Local sx#=s.c.x#
		Local sy#=s.c.y#
		Local sz#=s.c.z#
		Local sr#=s.r#
		Local bx#=a.mini#[0]
		Local by#=a.mini#[1]
		Local bz#=a.mini#[2]
		Local bw#=a.maxi#[0]-bx#
		Local bh#=a.maxi#[1]-by#
		Local bd#=a.maxi#[2]-bz#
	
		Local dmin#=0
		Local sr2#=sr*sr
		
		' x axis
		If sx < bx
	
			dmin=dmin+((sx-bx)*(sx-bx))
			
		Else If sx>(bx+bw)
	
			dmin=dmin+(((sx-(bx+bw)))*((sx-(bx+bw))))
	
		EndIf
	
		' y axis
		If sy < by
	
			dmin=dmin+((sy-by)*(sy-by))
			
		Else If sy>(by+bh)
	
			dmin=dmin+(((sy-(by+bh)))*((sy-(by+bh))))
	
		EndIf
	
		' z axis
		If sz < bz
	
			dmin=dmin+((sz-bz)*(sz-bz))
	
		Else If sz>(bz+bd)
	
			dmin=dmin+(((sz-(bz+bd)))*((sz-(bz+bd))))
	
		EndIf
	
		If dmin#<=sr2# Then Return True Else Return False
	
	End Function

End Type

Type TSphere

	Field c:TVector=New TVector
	Field r#

End Type
