BRL.vec, BRL.matrix

It would be nice if BlitzMax had official vector, matrix and quaternion functions. Mine work, but I would prefer to rely on something standard.

My vector commands are like this. It's nice because you can do stuff like MulQuat( Vec4(x#,y#,z#,w#), entity.quaternion ) or entity.quaternion = entity.rotation.ToQuat()

Type TVec2
field x#,y#

Type TVec3 extends TVec2
field z#

Type TVec4 extends TVec3
field w#

Function Vec3:TVec3(x#,y#,z#)
vec:TVec3=new tvec3
vec.x=x
vec.y=y
vec.z=z
return vec
EndFunction

Yep, that would be very nice.

Here is my whole vector and matrix code. I just converted it to a more OO approach. The matrix code was partially based on MiniB3D mat code, with quaternion rotation added.

Rem
bbdoc:
EndRem
Type TVec2
	'expose
	
	Field x#,y#
	
	Method Magnitude:Float()
		Return Sqr(x*x+y*y)
	EndMethod
	
	Method Normalize()
		Local m#=Magnitude()
		x:/m
		y:/m
	EndMethod

	Method Flip:TVec2()
		Local v:TVec2=New TVec2
		v.x=-x
		v.y=-y
		Return v
	EndMethod

	Method Copy:TVec2()
		Local vec:TVec2=New TVec2
		vec.x=x
		vec.y=y
		Return vec
	EndMethod

	Method ToAngle:Float()
		Return ATan2(y,x)
	EndMethod

	Function FromAngle:TVec2(angle#)
		Local vec:TVec2=New tvec2
		vec.x=Cos(angle)
		vec.y=Sin(angle)
		Return vec
	EndFunction

	

	Method ToString:String()
		Return x+", "+y
	EndMethod

EndType

Rem
bbdoc:
EndRem
Type TVec3 Extends TVec2
	'expose
	
	Field z#
	
	Method Magnitude:Float()
		Return Sqr(x*x+y*y+z*z)
	EndMethod
	
	Method Normalize()
		Local m#=Magnitude()
		x:/m
		y:/m
		z:/m
	EndMethod

	Method Sub:TVec3(vec:TVec3)
		Local newvec:TVec3=New tvec3
		newvec.x=x-vec.x
		newvec.y=y-vec.y
		newvec.z=z-vec.z
		Return newvec
	EndMethod

	Method Reverse:TVec3()
		Local v:TVec3=New TVec3
		v.x=-x
		v.y=-y
		v.z=-z
		Return v
	EndMethod

	Method Invert:TVec3()
		Local v:TVec3=New TVec3
		v.x=1.0/x
		v.y=1.0/y
		v.z=1.0/z
		Return v
	EndMethod

	Method Copy:TVec3()
		Local vec:TVec3=New TVec3
		vec.x=x
		vec.y=y
		vec.z=z
		Return vec
	EndMethod

	Method ToQuat:TVec4()
		Local cr#=Cos(-z/2.0)
		Local cp#=Cos(x/2.0)
		Local cy#=Cos(y/2.0)
		Local sr#=Sin(-z/2.0)
		Local sp#=Sin(x/2.0)
		Local sy#=Sin(y/2.0)
		Local cpcy#=cp#*cy#
		Local spsy#=sp#*sy#
		Local spcy#=sp#*cy#
		Local cpsy#=cp#*sy#
		Local quat:TVec4=New TVec4
		quat.w=cr#*cpcy#+sr#*spsy#
		quat.x=sr#*cpcy#-cr#*spsy#
		quat.y=cr#*spcy#+sr#*cpsy#
		quat.z=cr#*cpsy#-sr#*spcy#
		Return quat
	EndMethod

	Method ToString$()
		Return x+", "+y+", "+z
	EndMethod

EndType

Rem
bbdoc:
EndRem
Type TVec4 Extends TVec3
	'expose
	
	Const QuatToEulerAccuracy#=0.001
	
	Field w#
	
	Method ToString$()
		Return x+", "+y+", "+z+", "+w
	EndMethod

	Method Magnitude:Float()
		Return Sqr(x*x+y*y+z*z+w*w)
	EndMethod
	
	Method Normalize()
		Local m#=Magnitude()
		x:/m
		y:/m
		z:/m
		w:/m
	EndMethod

	Method Invert:TVec4()
		Local v:TVec4=New TVec4
		v.x=1.0/x
		v.y=1.0/y
		v.z=1.0/z
		v.w=1.0/w
		Return v
	EndMethod

	Method Reverse:TVec4()
		Local v:TVec4=New TVec4
		v.x=-x
		v.y=-y
		v.z=-z
		v.w=-w
		Return v
	EndMethod

	Method Copy:TVec4()
		Local vec:TVec4=New TVec4
		vec.x=x
		vec.y=y
		vec.z=z
		vec.w=w
		Return vec
	EndMethod

	Method ToEuler:TVec3()
		Local sint#=(2.0*w*y)-(2.0*x*z)
		Local cost_temp#=1.0-(sint#*sint#)
		Local cost#,sinv#,cosv#,sinf#,cosf#
		If Abs(cost_temp#)>QuatToEulerAccuracy
			cost#=Sqr(cost_temp#)
		Else
			cost#=0.0
		EndIf
		If Abs(cost#)>QuatToEulerAccuracy
			sinv#=((2.0*y*z)+(2.0*w*x))/cost#
			cosv#=(1.0-(2.0*x*x)-(2.0*y*y))/cost#
			sinf#=((2.0*x*y)+(2.0*w*z))/cost#
			cosf#=(1.0-(2.0*y*y)-(2.0*z*z))/cost#
		Else
			sinv#=(2.0*w*x)-(2.0*y*z)
			cosv#=1.0-(2.0*x*x)-(2.0*z*z)
			sinf#=0.0
			cosf#=1.0
		EndIf
		Local euler:TVec3=New TVec3
		euler.z=-ATan2(sinv#,cosv#)
		euler.x=ATan2(sint#,cost#)
		euler.y=ATan2(sinf#,cosf#)
		Return euler
	EndMethod

EndType

Rem
bbdoc:
End Rem
Function Vec2:TVec2(x#,y#)
	Local vec:TVec2=New TVec2
	vec.x=x
	vec.y=y
	Return vec
EndFunction

Rem
bbdoc:
End Rem
Function Vec3:TVec3(x#,y#,z#)
	Local vec:TVec3=New TVec3
	vec.x=x
	vec.y=y
	vec.z=z
	Return vec
EndFunction

Rem
bbdoc:
End Rem
Function Vec4:TVec4(x#,y#,z#,w#)
	Local vec:TVec4=New TVec4
	vec.x=x
	vec.y=y
	vec.z=z
	vec.w=w
	Return vec
EndFunction

Rem
bbdoc:
End Rem
Function FlipQuat:TVec4(quat:TVec4)
	quat=quat.copy()
	quat.w=-quat.w
	Return quat
EndFunction

Rem
bbdoc:
End Rem
Function MulQuat:TVec4(quat0:TVec4,quat1:TVec4)
	Local ax#,ay#,az#,aw#,bx#,by#,bz#,bw#,a#,b#,c#,d#,e#,f#,g#,h#
	Local quat:TVec4
	ax=quat0.x
	ay=quat0.y
	az=quat0.z
	bw=quat0.w
	bx=quat1.x
	by=quat1.y
	bz=quat1.z
	bw=quat1.w	
	a#=(Aw#+Ax#)*(Bw#+Bx#)
	b#=(Az#-Ay#)*(By#-Bz#)
	c#=(Aw#-Ax#)*(By#+Bz#)
	d#=(Ay#+Az#)*(Bw#-Bx#)
	e#=(Ax#+Az#)*(Bx#+By#)
	f#=(Ax#-Az#)*(Bx#-By#)
	g#=(Aw#+Ay#)*(Bw#-Bz#)
	h#=(Aw#-Ay#)*(Bw#+Bz#)
	quat=New tvec4
	quat.w=b#+(-e#-f#+g#+h#)/2.0
	quat.x=a#-(e#+f#+g#+h#)/2.0
	quat.y=c#+(e#-f#+g#-h#)/2.0
	quat.z=d#+(e#-f#-g#+h#)/2.0
	Return quat
EndFunction

Rem
bbdoc:
End Rem
Function PointPlaneDistance:Float(point:TVec3,plane:TVec4)
	Return (plane.w-plane.x*point.x-plane.y*point.y-plane.z*point.z)
EndFunction

Rem
bbdoc:
End Rem
Function Dot:Float(vec0:TVec3,vec1:TVec3)
	Return vec0.x*vec1.x+vec0.y*vec1.y+vec0.z*vec1.z
EndFunction

Rem
bbdoc:
End Rem
Function Cross:TVec3(vec0:TVec3,vec1:TVec3)
	Local vec:TVec3=New tvec3
	vec.x=vec0.y*vec1.z-vec0.z*vec1.y
	vec.y=vec0.z*vec1.x-vec0.x*vec1.z
	vec.z=vec0.x*vec1.y-vec0.y*vec1.x
	Return vec
EndFunction

Rem
bbdoc:
End Rem
Type TMatrix

	Field grid#[4,4]

	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
	
	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]
		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
	EndMethod

	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]
	EndMethod
	
	Method Inverse:TMatrix()
		Local mat:TMatrix=New TMatrix
		mat.loadidentity()
		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
	
	' 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
	EndMethod

	Method TForm4(position:TVec3,quaternion:TVec4,scale:TVec3)
		Local px#,py#,pz#,qx#,qy#,qz#,qw#,sx#,sy#,sz#
		px=position.x
		py=position.y
		pz=position.z
		qx=quaternion.x
		qy=quaternion.y
		qz=quaternion.z
		qw=quaternion.w
		sx=scale.x
		sy=scale.y
		sz=scale.z
		
		' 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*qw*qx)*sx
		grid[2,0]=-(2.0*qx*qy+2.0*qw*qz)*sx
		
		grid[0,1]=-(2.0*qz*qy+2.0*qw*qx)*sy
		grid[1,1]=(1.0-2.0*qy*qy-2.0*qx*qx)*sy
		grid[2,1]=-(2.0*qw*qy-2.0*qx*qz)*sy
		
		grid[0,2]=(2.0*qw*qz-2.0*qx*qy)*sz
		grid[1,2]=(2.0*qx*qz+2.0*qw*qy)*sz
		grid[2,2]=(1.0-2.0*qz*qz-2.0*qy*qy)*sz
		
	End Method
	
	Method Translate(vec:TVec3)
		grid[3,0] = grid#[0,0]*vec.x# + grid#[1,0]*vec.y# + grid#[2,0]*vec.z# + grid#[3,0]
		grid[3,1] = grid#[0,1]*vec.x# + grid#[1,1]*vec.y# + grid#[2,1]*vec.z# + grid#[3,1]
		grid[3,2] = grid#[0,2]*vec.x# + grid#[1,2]*vec.y# + grid#[2,2]*vec.z# + grid#[3,2]
	EndMethod

	Method Scale(vec:TVec3)
		grid[0,0]:*vec.x#
		grid[0,1]:*vec.x#
		grid[0,2]:*vec.x#
		grid[1,0]:*vec.y#
		grid[1,1]:*vec.y#
		grid[1,2]:*vec.y#
		grid[2,0]:*vec.z#
		grid[2,1]:*vec.z#
		grid[2,2]:*vec.z# 
	EndMethod
	
	Method ExtractPosition:TVec3()
		Local vec:TVec3
		vec=New tvec3
		vec.x=grid[3,0]
		vec.y=grid[3,1]
		vec.z=grid[3,2]
		Return vec
	EndMethod
	
	'Don't know how to extract the scale relative to the rotation.  Maybe rotate by the reversed rotation and get the scale?
	'Method ExtractScale()
		'x=Sqr(grid[0,0]*grid[0,0]+grid[0,1]*grid[0,1]+grid[0,2]*grid[0,2])
		'y=Sqr(grid[1,0]*grid[1,0]+grid[1,1]*grid[1,1]+grid[1,2]*grid[1,2])
		'z=Sqr(grid[2,0]*grid[2,0]+grid[2,1]*grid[2,1]+grid[2,2]*grid[2,2])
	'EndMethod
	
	Method ExtractQuat:TVec4()
		Local x#,y#,z#,w#
		Local fourWSquaredMinus1:Float = +grid[0,0]+grid[1,1]+grid[2,2]
		Local fourXSquaredMinus1:Float = +grid[0,0]-grid[1,1]-grid[2,2]
		Local fourYSquaredMinus1:Float = +grid[1,1]-grid[0,0]-grid[2,2]
		Local fourZSquaredMinus1:Float = +grid[2,2]-grid[0,0]-grid[1,1]
		Local biggestIndex=0
		Local fourBiggestSquaredMinus1:Float=fourWSquaredMinus1
		If fourXSquaredMinus1 > fourBiggestSquaredMinus1
			fourBiggestSquaredMinus1 = fourXSquaredMinus1 
			biggestIndex=1
		EndIf
		If fourYSquaredMinus1 > fourBiggestSquaredMinus1
			fourBiggestSquaredMinus1 = fourYSquaredMinus1 
			biggestIndex=2
		EndIf
		If fourZSquaredMinus1 > fourBiggestSquaredMinus1
			fourBiggestSquaredMinus1 = fourZSquaredMinus1 
			biggestIndex=3
		EndIf
		Local biggestValue:Float=Sqr(fourBiggestSquaredMinus1+1.0)*0.5
		Local mult:Float=0.25/biggestValue
		Select BiggestIndex
			Case 0
				w#=BiggestValue
				x#=(grid[1,2]-grid[2,1])*mult
				y#=-(grid[2,0]-grid[0,2])*mult
				z#=-(grid[0,1]-grid[1,0])*mult
			Case 1
				x#=-BiggestValue
				w#=-( grid[1,2] - grid[2,1] )*mult
				y#=( grid[0,1] + grid[1,0] )*mult
				z#=( grid[2,0] + grid[0,2] )*mult
			Case 2
				y#=BiggestValue
				w#=-(grid[2,0]-grid[0,2])*mult
				x#=-(grid[0,1]+grid[1,0])*mult
				z#=(grid[1,2]+grid[2,1])*mult
			Case 3
				z#=-BiggestValue
				w#=(grid[0,1]-grid[1,0])*mult
				x#=(grid[2,0]+grid[0,2])*mult
				y#=-(grid[1,2]+grid[2,1])*mult
		EndSelect
		If w<-0.0
			x=-x
			y=-y
			z=-z
			w=-w
		EndIf
		Return vec4(x,y,z,w)
	EndMethod
	
	Method ExtractEuler:TVec3()
		Local a#,b#,x#,y#,z#
		x=-ATan2( grid[2,1],Sqr( grid[2,0]*grid[2,0]+grid[2,2]*grid[2,2] ) )
		a#=grid[2,0]
		b#=grid[2,2]
		If a#<=0.0001 And a#>=-0.0001 Then a#=0
		If b#<=0.0001 And b#>=-0.0001 Then b#=0
		y=-ATan2(a#,b#)
		a#=grid[0,1]
		b#=grid[1,1]
		If a#<=0.0001 And a#>=-0.0001 Then a#=0
		If b#<=0.0001 And b#>=-0.0001 Then b#=0
		z=ATan2(a#,b#)
		Return vec3(x,y,z)
	EndMethod
	
	Method Rotate4(quat:TVec4)
		Local qx#,qy#,qz#,qw#
		
		'Scale removed for speed / doesn't work right anyways
		'sx#=1'Sqr(grid[0,0]*grid[0,0]+grid[0,1]*grid[0,1]+grid[0,2]*grid[0,2])
		'sy#=1'Sqr(grid[1,0]*grid[1,0]+grid[1,1]*grid[1,1]+grid[1,2]*grid[1,2])
		'sz#=1'Sqr(grid[2,0]*grid[2,0]+grid[2,1]*grid[2,1]+grid[2,2]*grid[2,2])
		
		quat=MulQuat(ExtractQuat(),quat)
		qx#=quat.x
		qy#=quat.y
		qz#=quat.z
		w#=quat.w
		
		grid[0,3]=0.0
		grid[1,3]=0.0
		grid[2,3]=0.0
		grid[3,3]=1.0
		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
	EndMethod
	
	Method Rotate(euler:TVec3)
		
		Local rx#,ry#,rz#
		Local cos_ang#,sin_ang#
	
		rx=euler.x
		ry=euler.y
		rz=euler.z
		
		' 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#
	
	EndMethod

	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

	EndMethod
	
	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
	
	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#

	EndMethod
	
EndType

Josh, are you using Vec4 in place of a Quaternion?

EDIT: Nm, I see that you are. That's pretty clever.

Type TVec2
   Field x:Float, y:Float
End Type

Type TVec3 Extends TVec2
   Field z:Float
End Type

Type TQuat Extends TVec3
   Field w:Float
End Type

Function Vec2:TVec2(x#=0, y#=0)
   Local v:TVec2 = New TVec2
   v.x = x
   v.y = y
   Return v
End Function

Function Vec3:TVec3(x#=0, y#=0, z#=0)
   Local v:TVec3 = New TVec3
   v.x = x
   v.y = y
   v.z = z
   Return v
End Function

Function Quat:TQuat(w#=0, x#=0, y#=0, z#=0)
   Local q:TQuat = New TQuat
   q.w = w
   q.x = x
   q.y = y
   q.z = z
   Return q
End Function

I love it man!! :)

I also use vec4's for planes and colors. That's why I call it a vec4 and not a quat:

Type TEntity

Field color:TVec4=New TVec4

Method SetColor(r,g,b,a=255)
color.x=float(r)/255.0
color.y=float(g)/255.0
color.z=float(b)/255.0
color.w=float(a)/255.0
EndMethod

glLightfv GL_LIGHT0,GL_DIFFUSE,entity.color

All that GLSL makes me think in terms of vectors. My advice is to convert everything to OO vectors, because if you mix OO vectors with a lot of individual floats you won't get the full advantage of how much it shrinks your code.

One question about your code (which is veeeeery helpful!). When you create a Tform4 Matrix, how do you get the original quaternion for the original position?

In other words, I have an object at (0,0,0) that is pointing (in my arbitrary system) away from the camera (the 0 yaw) with an up-pitch of 20 degrees and no roll and at the original scale.

So I create a Tvec3 with 0,0,0 for the position to pass, and a TVec3 with 1,1,1 for the scale to pass. How do I figure out what to store in the TVec4 to pass as the quarternion? Would I just EulertoQuat (20,0,0) and pass the results?

Store a quat for the entity and only update it when the use inputs a euler rotation.

Yeah... but what quat do I store? How do you "baseline" quats? What does 0,0,0,0 describe? If I knew what the baseline was for a quat, I could just update with euler as I created the object [start with a quat at 0,0,0,0 and then rotatepitch(-20)]. But does 0,0,0,0 equal pointing away from me down the Z axis? Or do I need to decide what 0,0,0,0 is and then translate whatever the result is into that frame of reference?

0,0,0,1

Josh, I found out you can't have the same method names for each Type this way. For example:

v2:TVec2 = TVec2.Create()
v3:TVec3 = TVec3.Create()

It throws an error "Compile Error Overriding method differs by type". That kinda sucks...unless there's something I'm missing.

Yes you can, post the code, cos you've made a mistake somewhere.
TVec3 IS a TVec2 so isnt a different type

Type TVec2

   Field x:Float, y:Float

	Function Create:tVec2()
		Return New tvec2
	EndFunction
	
End Type

Type TVec3 Extends TVec2

   Field z:Float

	Function Create:tVec3()
		Return New tvec3
	EndFunction

End Type

v2:TVec2 = TVec2.Create()
v3:TVec3 = TVec3.Create()

This works. What have you done differently

Hmm...lemme check.

Ok, I have no idea. But I was getting the error above. Now I can't duplicate it. :(

EDIT: Ok I duplicated it. Posting the code in a couple minutes.

Ok here's the problem code:

Type TVec2

   Field x:Float, y:Float

	Function Create:tVec2()
		Return New tvec2
	EndFunction
	
	Method Set(x#, y#)
		self.x = x
		self.y = y
	End Method
	
End Type

Type TVec3 Extends TVec2

   Field z:Float

	Function Create:tVec3()
		Return New tvec3
	EndFunction
	
	Method Set(x#, y#, z#)
		self.x = x
		self.y = y
		self.z = z
	End Method

End Type

v2:TVec2 = TVec2.Create()
v3:TVec3 = TVec3.Create()

the problem is in the Method Set: you can not ovveride a method with different numbers of parameters
A possibile solution is

type tvec2
...
Method Set(x#, y#,z#=0)
		self.x = x
		self.y = y
	End Metho
...

Another vote for built-in vector types. I'm tired of writing my own every time, so I am!

I like vec3(x,y,z) better than TVec3.Create().

You could do this:

Type TVec2

   Field x:Float, y:Float

	Function Create:tVec2()
		Return New tvec2
	EndFunction
	
	Method Set(x#, y#, z#=0)
		self.x = x
		self.y = y
	End Method
	
End Type

Type TVec3 Extends TVec2

   Field z:Float

	Function Create:tVec3()
		Return New tvec3
	EndFunction
	
	Method Set(x#, y#, z#)
		self.x = x
		self.y = y
		self.z = z
	End Method

End Type

Or this:

Type TVec2

   Field x:Float, y:Float

	Function Create:tVec2()
		Return New tvec2
	EndFunction
	
	Method Set(vec#[])
		self.x = vec[0]
		self.y = vec[1]
	End Method
	
End Type

Type TVec3 Extends TVec2

   Field z:Float

	Function Create:tVec3()
		Return New tvec3
	EndFunction
	
	Method Set(vec#[])
		self.x = vec[0]
		self.y = vec[1]
		self.z = vec[2]
	End Method

End Type

vec3=NewTVec3
vec3.set [0.0,1.0,2.0]

Oh, this is nice. I have a lot of this already but there are some nice additions to my code in there. Thanks Josh (and others for the additional code :) )

I think stuff that's easy to remember is the good too. Like when you say a = b + c. You're saying to yourself:

a equals b plus c

Why not name the functions like that to make em easy to remember?

a = b.plus(c)
a = b.dividedby(c)
a = b.times(c)
a = b.plus(c.minus(d))

You get the idea. I converted my vec lib to this cuz it's more intuitive.

'Vector Math Simplified Function Names

SuperStrict

Local a:TVec3 = Vec3()
Local b:TVec3 = Vec3(1,1,1)
Local c:TVec3 = Vec3(1,2,3)

'Examples

'a = a + b
a = a.Plus( b )
Print a.x+", "+a.y+", "+a.z

'a = a - b
a = a.Minus( b )
Print a.x+", "+a.y+", "+a.z

'a = a + b + c
a = a.Plus( b.Plus( c ) )
Print a.x+", "+a.y+", "+a.z

'a = c * (b + b)
a = c.Times( b.Plus( b ) )
Print a.x+", "+a.y+", "+a.z

'a = b / c
a = b.DividedBy( c )
Print a.x+", "+a.y+", "+a.z

'a = -b
a = b.Inverse()
Print a.x+", "+a.y+", "+a.z

End


Rem
bbdoc:
EndRem
Type TVec3
	Field x:Float, y:Float, z:Float

	Method Copy:TVec3()
		Local res:TVec3 = New TVec3
		res.x = self.x
		res.y = self.y
		res.z = self.z
		Return res
	End Method

	Method DividedBy:TVec3(v:TVec3)
		Local res:TVec3 = New TVec3
		res.x = self.x / v.x
		res.y = self.y / v.y
		res.z = self.z / v.z
		Return res
	End Method

	Method DividedByScalar:TVec3(divisor#)
		Local res:TVec3 = New TVec3
		res.x = self.x / divisor
		res.y = self.y / divisor
		res.z = self.z / divisor
		Return res
	End Method
	
	Method Inverse:TVec3()
		Local v:TVec3 = New TVec3
		v.x = -self.x
		v.y = -self.y
		v.z = -self.z
		Return v
	End Method

	Method Magnitude:Float()
		Return Sqr(self.x * self.x + self.y * self.y + self.z * self.z)
	End Method
	
	Method Minus:TVec3(v:TVec3)
		Local res:TVec3 = New TVec3
		res.x = self.x - v.x
		res.y = self.y - v.y
		res.z = self.z - v.z
		Return res
	End Method
	
	Method Normalize()
		Local m# = self.Magnitude()
		self.x :/ m
		self.y :/ m
		self.z :/ m
	End Method

	Method Plus:TVec3(v:TVec3)
		Local res:TVec3 = New TVec3
		res.x = self.x + v.x
		res.y = self.y + v.y
		res.z = self.z + v.z
		Return res
	End Method

	Method Set(x#, y#, z#)
		self.x = x
		self.y = y
		self.z = z
	End Method

	Method Times:TVec3(v:TVec3)
		Local res:TVec3 = New TVec3
		res.x = self.x * v.x
		res.y = self.y * v.y
		res.z = self.z * v.z
		Return res
	End Method

	Method TimesScalar:TVec3(scalar#)
		Local res:TVec3 = New TVec3
		res.x = self.x * scalar
		res.y = self.y * scalar
		res.z = self.z * scalar
		Return res
	End Method

	Method Zero()
		self.x = 0
		self.y = 0
		self.z = 0
	End Method

End Type

Rem
bbdoc:
EndRem
Function Vec3:TVec3(x#=0, y#=0, z#=0)
	Local v:TVec3 = New TVec3
	v.x = x
	v.y = y
	v.z = z
	Return v
End Function

Here's what I would really like:

vec1:TVec3=New TVec3
vec2:TVec3=New TVec3

vec3:TVec3 = vec1 + vec2

Operator overloading would be a cool feature for a future BMAX update! :)

I also prefer this:

TVec3 vec

to this:

Local vec:TVec3=New TVec3

Hi,

I do in fact have a set math types, but they are far from complete - there are plenty of Throw "TODO"s in there and a significant amount of untested code.

So I don't want to release them as 'official' modules just yet, but perhaps they could be released as sort of a community project where we can all contribute to making them bulletproof?

Sounds like a nice additional feature to the BlitzMax language, Mark.

And it would be cool if you would try to do such a release.
It might just go better than expected, and could even be considered for future additions/features as well.. (here's hoping!)

Thanks, if you just posted some unofficial code, I might learn something new from it. The MiniB3D code had a few good ideas I used to improve my matrix math.

Releasing them and making them into a community solution sounds like a good idea.

I also agree with the sentiment about operator overloading. Vector, Quaternion and Matrix classes just cry out for operator overloading, in my book. I have my classes as Chroma has his, but operator overloading is the ideal for me.

Hi,

OK, here's my current geom.bmx file - all subject to change etc.

A couple of points:

* Designed to match b3d orientation.

* Objects are designed to be 'immutable' - ie: once created, it's generally expected they're not gonna be modified. If you stick to this rule, you can safely share objects around without having to copy them in case they're modified.

* I've avoided embedding objects-within-objects for speed reasons - eg: TLine could have origin:TVec3 and delta:TVec3 members, but it'd be a bit slower. Ditto arrays.

* Although TMat4 is a 4x4 matrix, many methods assume it's also affine. Used to call them AffineInverse() etc, but it turned out to be most of them.

Strict

Import BRL.Math

Global Infinity#=1 Shl 23

Function Vec3:TVec3( x#,y#,z# )
	Local t:TVec3=New TVec3
	t.x=x
	t.y=y
	t.z=z
	Return t
End Function

Function Vec4:TVec4( x#,y#,z#,w# )
	Local t:TVec4=New TVec4
	t.x=x
	t.y=y
	t.z=z
	t.w=w
	Return t
End Function

Function Line:TLine( x#,y#,z#,dx#,dy#,dz# )
	Local t:TLine=New TLine
	t.x=x
	t.y=y
	t.z=z
	t.dx=dx
	t.dy=dy
	t.dz=dz
	Return t
End Function

Function Quat:TQuat( x#,y#,z#,w# )
	Local t:TQuat=New TQuat
	t.x=x
	t.y=y
	t.z=z
	t.w=w
	Return t
End Function

Function Plane:TPlane( nx#,ny#,nz#,d# )
	Local t:TPlane=New TPlane
	t.nx=nx
	t.ny=ny
	t.nz=nz
	t.d=d
	Return t
End Function

Function UlpsEqual( x#,y#,maxUlps=8 )
	Return DeltaUlps( x,y )<=maxUlps
End Function

Function DeltaUlps( x#,y# )
	Local ix=(Int Ptr Varptr x)[0]
	Local iy=(Int Ptr Varptr y)[0]
	If ix<0 ix=$80000000-ix
	If iy<0 iy=$80000000-iy
	Return Abs( ix-iy )
End Function

Type TVec3 {value}

	Field x#,y#,z# {attribute}
	
	Method Copy:TVec3()
		Return Vec3( x,y,z )
	End Method
	
	Method ToString$()
		Return "Vec3("+x+","+y+","+z+")"
	End Method
	
	Method ToVec4:TVec4()
		Return Vec4( x,y,z,1 )
	End Method
	
	Method ToField$()
		Return x+","+y+","+z
	End Method
	
	Method FromField:TVec3( t$ )
		Local bits$[]=t.Split( "," )
		If bits.length<>3 Throw "Format error"
		x=bits[0].ToFloat()
		y=bits[1].ToFloat()
		z=bits[2].ToFloat()
		Return Self
	End Method

	Method Length#()
		Return Sqr( x*x+y*y+z*z )
	End Method
	
	Method Dot#( v:TVec3 )
		Return x*v.x+y*v.y+z*v.z
	End Method
	
	Method Inverse:TVec3()
		Return Vec3( -x,-y,-z )
	End Method
	
	Method Reciprocal:TVec3()
		Return Vec3( 1/x,1/y,1/z )
	End Method
	
	Method Normalize:TVec3()
		Local t#=Sqr( x*x+y*y+z*z )
		Return Vec3( x/t,y/t,z/t )
	End Method
	
	Method Scale:TVec3( scale# )
		Return Vec3( x*scale,y*scale,z*scale )
	End Method 
	
	Method DistanceTo#( v:TVec3 )
		Local dx#=x-v.x,dy#=y-v.y,dz#=z-v.z
		Return Sqr( dx*dx+dy*dy+dz*dz )
	End Method
	
	Method Plus:TVec3( v:TVec3 )
		Return Vec3( x+v.x,y+v.y,z+v.z )
	End Method
	
	Method Minus:TVec3( v:TVec3 )
		Return Vec3( x-v.x,y-v.y,z-v.z )
	End Method
	
	Method Times:TVec3( v:TVec3 )
		Return Vec3( x*v.x,y*v.y,z*v.z )
	End Method
	
	Method DividedBy:TVec3( v:TVec3 )
		Return Vec3( x/v.x,y/v.y,z/v.z )
	End Method
	
	Method Cross:TVec3( v:TVec3 )
		Return Vec3( y*v.z-z*v.y,z*v.x-x*v.z,x*v.y-y*v.x )
	End Method
	
	Method Blend:TVec3( v:TVec3,alpha# )
		Local beta#=1-alpha
		Return Vec3( x*beta+v.x*alpha,y*beta+v.y*alpha,z*beta+v.z*alpha )
	End Method

End Type

Type TVec4 Extends TVec3

	Field w# {attribute}
	
	Method Copy:TVec4()
		Return Vec4( x,y,z,w )
	End Method
	
	Method ToString$()
		Return "Vec4("+x+","+y+","+z+","+w+")"
	End Method
	
	Method ToVec4:TVec4()
		Return Self
	End Method
	
End Type

Type TLine

	Field x#,y#,z#,dx#,dy#,dz# {attribute}
	
	Method Copy:TLine()
		Return Line( x,y,z,dx,dy,dz )
	End Method
	
	Method ToString$()
		Return "Line("+x+","+y+","+z+","+dx+","+dy+","+dz+")"
	End Method
	
	Method Origin:TVec3()
		Return Vec3( x,y,z )
	End Method
	
	Method Delta:TVec3()
		Return Vec3( dx,dy,dz )
	End Method
	
	Method EndPoint:TVec3()
		Return Vec3( x+dx,y+dy,z+dz )
	End Method
	
	Method Evaluate:TVec3( time# )
		Return Vec3( x+dx*time,y+dy*time,z+dz*time )
	End Method
	
	Method DistanceTo#( t:TLine )
		Local wx#=x-t.x
		Local wy#=y-t.y
		Local wz#=z-t.z
		Local a#=dx*dx+dy*dy+dz*dz
		Local b#=dx*t.dx+dy*t.dy+dz*t.dz
		Local c#=t.dx*t.dx+t.dy*t.dy+t.dz*t.dz
		Local d#=dx*wx+dy*wy+dz*wz
		Local e#=t.dx*wx+t.dy*wy+t.dz*wz
		Local q#=a*c-b*b
		Local st#=b*e-c*d
		Local tt#=a*e-b*d
		Local tx#=wx+(st*dx-tt*t.dx)/q
		Local ty#=wy+(st*dy-tt*t.dy)/q
		Local tz#=wz+(st*dz-tt*t.dz)/q
		Return Sqr( tx*tx+ty*ty+tz*tz )
	End Method
	
	Function FromEndPoints:TLine( p0:TVec3,p1:TVec3 )
		Local t:TLine=New TLine
		t.x=p0.x;t.y=p0.y;t.z=p0.z
		t.dx=p1.x-p0.x;t.dy=p1.y-p0.y;t.dz=p1.z-p0.z;
		Return t
	End Function
	
End Type

Type TQuat

	Field x#,y#,z#,w# {attribute}
	
	Method Copy:TQuat()
		Return Quat( x,y,z,w )
	End Method
	
	Method ToString$()
		Return "Quat("+x+","+y+","+z+","+w+")"
	End Method
	
	Method Inverse:TQuat()
		Return Quat( -x,-y,-z,w )
	End Method
	
	Method ToAngleAxis( angle# Var,axis:TVec3 Var )
		Local t#=ACos(w)
		angle=t*2
		If angle>0
			t=1/Sin(t)
			axis=Vec3( x*t,y*t,z*t )
		Else
			axis=Vec3(0,0,0)
		EndIf
	End Method
	
	Method Yaw#()
		Return ATan2( 2*y*w-2*z*x,1-2*y*y-2*x*x )
	End Method
	
	Method Pitch#()
		Return -ASin( 2*z*y+2*x*w )
	End Method
	
	Method Roll#()
		Return -ATan2( 2*z*w-2*y*x,1-2*z*z-2*x*x )
	End Method
	
	Method ToYawPitchRoll( yaw# Var,pitch# Var,roll# Var )
		Local x2#=x*x,y2#=y*y,z2#=z*z
		yaw#=ATan2( 2*y*w-2*z*x,1-2*y2-2*x2 )
		pitch#=-ASin( 2*z*y+2*x*w )
		roll#=-ATan2( 2*z*w-2*y*x,1-2*z2-2*x2 )
	End Method
	
	Method Times:TQuat( q:TQuat )
		Local x#=w*q.x + x*q.w + y*q.z - z*q.y
		Local y#=w*q.y + y*q.w + z*q.x - x*q.z
		Local z#=w*q.z + z*q.w + x*q.y - y*q.x
		Local w#=w*q.w - x*q.x - y*q.y - z*q.z
		Return Quat( x,y,z,w )
	End Method

	Method Slerp:TQuat( q:TQuat,a# )
		Local b#=1-a,f
		Local d#=x*q.x+y*q.y+z*q.z+w*q.w
		If d<0
			d=-d
			f=True
		EndIf
		If d<1
			Local om#=acos_( d )
			Local si#=sin_( om )
			a=sin_( a*om )/si
			b=sin_( b*om )/si
		EndIf
		If f a=-a
		Return Quat( x*b + q.x*a , y*b + q.y*a , z*b + q.z*a , w*b + q.w*a )
	End Method
	
	Function FromAngleAxis:TQuat( angle#,axis:TVec3 )
		angle:/2
		Local s#=Sin(angle)
		Local t:TQuat=New TQuat
		t.x=s*axis.x
		t.y=s*axis.y
		t.z=s*axis.z
		t.w=Cos(angle)
		Return t
	End Function
	
	Function FromYawPitchRoll:TQuat( yaw#,pitch#,roll# )
		Local c1#=Cos(-roll/2)
		Local s1#=Sin(-roll/2)
		Local c2#=Cos(-pitch/2)
		Local s2#=Sin(-pitch/2)
		Local c3#=Cos( yaw/2)
		Local s3#=Sin( yaw/2)
		Local c1_c2#=c1*c2
		Local s1_s2#=s1*s2
		Local t:TQuat=New TQuat
		t.x=c1*s2*c3 - s1*c2*s3;
		t.y=c1_c2*s3 + s1_s2*c3;
		t.z=s1*c2*c3 + c1*s2*s3;
		t.w=c1_c2*c3 - s1_s2*s3;
		Return t
	End Function	

End Type

Type TPlane

	Field nx#,ny#,nz#,d# {attribute}
	
	Method Copy:TPlane()
		Return Plane( nx,ny,nz,d )
	End Method
	
	Method ToString$()
		Return "Plane("+nx+","+ny+","+nz+","+d+")"
	End Method
	
	Method MajorAxis()
		Local ax#=Abs(nx),ay#=Abs(ny),az#=Abs(nz)
		If ax>ay And ax>az Return 0
		If ay>az Return 1
		Return 2
	End Method
	
	Method SolveX:TVec3( p:TVec3 )
		Return Vec3( (p.y*ny+p.z*nz+d)/-nx,p.y,p.z )
	End Method
	
	Method SolveY:TVec3( p:TVec3 )
		Return Vec3( p.x,(p.x*nx+p.z*nz+d)/-ny,p.z )
	End Method
	
	Method SolveZ:TVec3( p:TVec3 )
		Return Vec3( p.x,p.y,(p.x*nx+p.y*ny+d)/-nz )
	End Method
	
	Method Inverse:TPlane()
		Return Plane( -nx,-ny,-nz,-d )
	End Method
	
	Method Normal:TVec3()
		Return Vec3( nx,ny,nz )
	End Method
	
	Method Normalize:TPlane()
		Local i#=1/Sqr(nx*nx+ny*ny+nz*nz)
		Return Plane( nx*i,ny*i,nz*i,d*i )
	End Method
	
	Method t_IntersectLine#( t:TLine )
		Return -DistanceToPoint( t.Origin() ) / Normal().Dot( t.Delta() )
	End Method
	
	Method DistanceToPoint#( p:TVec3 )
		Return nx*p.x+ny*p.y+nz*p.z+d
	End Method
	
	Function FromPointNormal:TPlane( p:TVec3,n:TVec3 )
		Return Plane( n.x,n.y,n.z,-p.Dot(n) )
	End Function
	
	Function FromTriangle:TPlane( v0:TVec3,v1:TVec3,v2:TVec3 )
		Local va:TVec3=v2.Minus(v1)
		Local vb:TVec3=v0.Minus(v1)
		Return FromPointNormal( v1,va.Cross(vb).Normalize() )
	End Function
	
	Function Ground:TPlane()
		Return Plane( 0,1,0,0 )
	End Function

End Type

Type TMat4

	Field ix#,iy#,iz#,iw# {attribute}
	Field jx#,jy#,jz#,jw# {attribute}
	Field kx#,ky#,kz#,kw# {attribute}
	Field tx#,ty#,tz#,tw# {attribute}
	
	Method Copy:TMat4()
		Local t:TMat4=New TMat4
		MemCopy t,Self,SizeOf Self
		Return t
	End Method
	
	Method ToString$()
		Local t$="Mat4{~n"
		t:+ix+","+iy+","+iz+","+iw+"~n"
		t:+jx+","+jy+","+jz+","+jw+"~n"
		t:+kx+","+ky+","+kz+","+kw+"~n"
		t:+tx+","+ty+","+tz+","+tw+"~n"
		Return t+"}~n"
	End Method
	
	Method glLoadMatrix()
		glLoadMatrixf Varptr ix
	End Method
	
	Method Translation:TVec3()
		Return Vec3(tx,ty,tz)
	End Method
	
	Method Rotation:TQuat()
		Local sc:TVec3=Scale()
		Local iv:TVec3=Vec3(ix,iy,iz).Scale(1/sc.x)
		Local jv:TVec3=Vec3(jx,jy,jz).Scale(1/sc.y)
		Local kv:TVec3=Vec3(kx,ky,kz).Scale(1/sc.z)
		Local x#,y#,z#,w#
		Local t#=iv.x+jv.y+kv.z+1
		If t>0
			t=Sqr(t)*2
			x=(kv.y-jv.z)/t
			y=(iv.z-kv.x)/t
			z=(jv.x-iv.y)/t
			w=t/4
		Else If iv.x>jv.y And iv.x>kv.z
			t=Sqr(iv.x-jv.y-kv.z+1)*2
			x=t/4
			y=(jv.x+iv.y)/t
			z=(iv.z+kv.x)/t
			w=(kv.y-jv.z)/t
		Else If jv.y>kv.z
			t=Sqr(jv.y-kv.z-iv.x+1)*2
			x=(jv.x+iv.y)/t
			y=t/4
			z=(kv.y+jv.z)/t
			w=(iv.z-kv.x)/t
		Else
			t=Sqr(kv.z-jv.y-iv.x+1)*2
			x=(iv.z+kv.x)/t
			y=(kv.y+jv.z)/t
			z=t/4
			w=(jv.x-iv.y)/t
		EndIf
		Return Quat( x,y,z,w )
	End Method
	
	Method Scale:TVec3()
		Return Vec3( Vec3(ix,iy,iz).Length(),Vec3(jx,jy,jz).Length(),Vec3(kx,ky,kz).Length() )
	End Method
	
	Method Times:TMat4( m:TMat4 )
		Local t:TMat4=New TMat4
		t.ix= ix*m.ix + jx*m.iy + kx*m.iz + tx*m.iw
		t.iy= iy*m.ix + jy*m.iy + ky*m.iz + ty*m.iw
		t.iz= iz*m.ix + jz*m.iy + kz*m.iz + tz*m.iw
		t.iw= iw*m.ix + jw*m.iy + kw*m.iz + tw*m.iw
		t.jx= ix*m.jx + jx*m.jy + kx*m.jz + tx*m.jw
		t.jy= iy*m.jx + jy*m.jy + ky*m.jz + ty*m.jw
		t.jz= iz*m.jx + jz*m.jy + kz*m.jz + tz*m.jw
		t.jw= iw*m.jx + jw*m.jy + kw*m.jz + tw*m.jw
		t.kx= ix*m.kx + jx*m.ky + kx*m.kz + tx*m.kw
		t.ky= iy*m.kx + jy*m.ky + ky*m.kz + ty*m.kw
		t.kz= iz*m.kx + jz*m.ky + kz*m.kz + tz*m.kw
		t.kw= iw*m.kx + jw*m.ky + kw*m.kz + tw*m.kw
		t.tx= ix*m.tx + jx*m.ty + kx*m.tz + tx*m.tw
		t.ty= iy*m.tx + jy*m.ty + ky*m.tz + ty*m.tw
		t.tz= iz*m.tx + jz*m.ty + kz*m.tz + tz*m.tw
		t.tw= iw*m.tx + jw*m.ty + kw*m.tz + tw*m.tw
		Return t
	End Method
	
	Method TimesPoint:TVec3( v:TVec3 )
'	Rem
		Return Vec3(..
			ix*v.x + jx*v.y + kx*v.z + tx,..
			iy*v.x + jy*v.y + ky*v.z + ty,..
			iz*v.x + jz*v.y + kz*v.z + tz )
'	End Rem
'		Local x#=ix*v.x + jx*v.y + kx*v.z + tx
'		Local y#=iy*v.x + jy*v.y + ky*v.z + ty
'		Local z#=iz*v.x + jz*v.y + kz*v.z + tz
'		Local w#=iw*v.x + jw*v.y + kw*v.z + tw
'		Return Vec3( x/w,y/w,z/w )
	End Method
	
	Method TimesVector:TVec3( v:TVec3 )
		Return Vec3(..
			ix*v.x + jx*v.y + kx*v.z,..
			iy*v.x + jy*v.y + ky*v.z,..
			iz*v.x + jz*v.y + kz*v.z )
	End Method

	Method TimesNormal:TVec3( v:TVec3 )
		Local m:TMat4=Inverse()
		Return Vec3(..
			m.ix*v.x + m.iy*v.y + m.iz*v.z,..
			m.jx*v.x + m.jy*v.y + m.jz*v.z,..
			m.kx*v.x + m.ky*v.y + m.kz*v.z )
	End Method
	
	Method TimesPlane:TPlane( p:TPlane )
		Local m:TMat4=Inverse()
		Return Plane(..
			m.ix*p.nx + m.iy*p.ny + m.iz*p.nz + m.iw*p.d,..
			m.jx*p.nx + m.jy*p.ny + m.jz*p.nz + m.jw*p.d,..
			m.kx*p.nx + m.ky*p.ny + m.kz*p.nz + m.kw*p.d,..
			m.tx*p.nx + m.ty*p.ny + m.tz*p.nz + m.tw*p.d ).Normalize()
	End Method
	
	Method TimesLine:TLine( t:TLine )
		Return TLine.FromEndPoints( TimesPoint( t.Origin() ),TimesPoint( t.EndPoint() ) )
	End Method
	
	Method Determinant#()
		Assert Abs(iw)<=.001 And Abs(jw)<=.001 And Abs(kw<=.001) And tw>=1-.001
		Return ix*(jy*kz-jz*ky) - iy*(jx*kz-jz*kx) + iz*(jx*ky-jy*kx)
	End Method
	
	Method I:TVec3()
		Return Vec3(ix,iy,iz)
	End Method
	
	Method J:TVec3()
		Return Vec3(jx,jy,jz)
	End Method
		
	Method K:TVec3()
		Return Vec3(kx,ky,kz)
	End Method
		
	Method Cofactor:TMat4()
		Local t:TMat4=New TMat4
		t.ix= (jy*kz-jz*ky) ; t.iy=-(jx*kz-jz*kx) ; 	t.iz= (jx*ky-jy*kx)
		t.jx=-(iy*kz-iz*ky) ; t.jy= (ix*kz-iz*kx) ; 	t.jz=-(ix*ky-iy*kx)
		t.kx= (iy*jz-iz*jy) ; t.ky=-(ix*jz-iz*jx) ; 	t.kz= (ix*jy-iy*jx)
		Return t
	End Method
	
	Method Inverse:TMat4()
		Local c#=1/Determinant()
		Local t:TMat4=New TMat4
		t.ix= c * ( jy*kz - jz*ky )
		t.iy=-c * ( iy*kz - iz*ky )
		t.iz= c * ( iy*jz - iz*jy )
		t.jx=-c * ( jx*kz - jz*kx )
		t.jy= c * ( ix*kz - iz*kx )
		t.jz=-c * ( ix*jz - iz*jx )
		t.kx= c * ( jx*ky - jy*kx )
		t.ky=-c * ( ix*ky - iy*kx )
		t.kz= c * ( ix*jy - iy*jx )
		t.tx=-( tx*t.ix + ty*t.jx + tz*t.kx )
		t.ty=-( tx*t.iy + ty*t.jy + tz*t.ky )
		t.tz=-( tx*t.iz + ty*t.jz + tz*t.kz )
		t.tw=1
		Return t
	End Method
	
	Method Transpose:TMat4()
		Local t:TMat4=New TMat4
		t.ix=ix ; t.iy=jx ; t.iz=kx ; t.iw=tx
		t.jx=iy ; t.jy=jy ; t.jz=ky ; t.jw=ty
		t.kx=iz ; t.ky=jz ; t.kz=kz ; t.kw=tz
		t.tx=iw ; t.ty=jw ; t.tz=kw ; t.tw=tw
		Return t
	End Method
	
	Function FromDirection:TMat4( dir:TVec3,up:TVec3=Null )
		If Not up up=Vec3(0,1,0)
		Local j:TVec3=up.Normalize()
		Local k:TVec3=dir.Normalize()
		Local i:TVec3=j.Cross(k).Normalize()
		j=k.Cross(i).Normalize()
		Local t:TMat4=New TMat4
		t.ix=i.x ; t.iy=i.y ; t.iz=i.z
		t.jx=j.x ; t.jy=j.y ; t.jz=j.z
		t.kx=k.x ; t.ky=k.y ; t.kz=k.z
		t.tw=1
		Return t
	End Function
	
	Function FromTranslation:TMat4( v:TVec3 )
		Local t:TMat4=New TMat4
		t.ix=1 ; t.jy=1 ;	t.kz=1
		t.tx=v.x ; t.ty=v.y ; t.tz=v.z
		t.tw=1
		Return t
	End Function
	
	Function FromRotation:TMat4( q:TQuat )
		Local t:TMat4=New TMat4
		Local xx#=q.x*q.x,yy#=q.y*q.y,zz#=q.z*q.z
		Local xy#=q.x*q.y,xz#=q.x*q.z,yz#=q.y*q.z
		Local wx#=q.w*q.x,wy#=q.w*q.y,wz#=q.w*q.z
		t.ix=1-2*(yy+zz) ; t.iy=  2*(xy-wz) ; t.iz=  2*(xz+wy)
		t.jx=  2*(xy+wz) ; t.jy=1-2*(xx+zz) ; t.jz=  2*(yz-wx)
		t.kx=  2*(xz-wy) ; t.ky=  2*(yz+wx) ; t.kz=1-2*(xx+yy)
		t.tw=1
		Return t
	End Function
	
	Function FromScale:TMat4( v:TVec3 )
		Local t:TMat4=New TMat4
		t.ix=v.x
		t.jy=v.y
		t.kz=v.z
		t.tw=1
		Return t
	End Function
	
	Function FromUniformScale:TMat4( sc# )
		Local t:TMat4=New TMat4
		t.ix=sc
		t.jy=sc
		t.kz=sc
		t.tw=1
		Return t
	End Function
	
	Function FromTransRotScale:TMat4( trans:TVec3,rot:TQuat,scale:TVec3 )
		Local t:TMat4
		If trans
			t=FromTranslation(trans)
			If rot t=t.Times(FromRotation(rot))
			If scale t=t.Times(FromScale(scale))
		Else If rot
			t=FromRotation(rot)
			If scale t=t.Times(FromScale(scale))
		Else If scale
			t=FromScale(scale)
		Else
			t=Identity()
		EndIf
		Return t
	End Function
	
	Function FromYaw:TMat4( yaw# )	'untested!
		Local t:TMat4=New TMat4
		Local s#=Sin(yaw),c#=Cos(yaw)
		t.ix=c  ; t.iz=s ; t.jy=1
		t.kx=-s ; t.kz=c ; t.tw=1
		Return t
	End Function
	
	Function FromPitch:TMat4( pitch# )	'untested!
		Local t:TMat4=New TMat4
		Local s#=Sin(pitch),c#=Cos(pitch)
		t.ix=1  ; t.jy=c ; t.jz=s
		t.ky=-s ; t.kz=c ; t.tw=1
		Return t
	End Function
	
	Function FromRoll:TMat4( roll# )	'untested!
		Local t:TMat4=New TMat4
		Local s#=Sin(roll),c#=Cos(roll)
		t.ix=c ; t.iy=s ; t.jx=-s
		t.jy=c ; t.kz=1 ; t.tw=1
		Return t
	End Function
	
	Function FromYawPitchRoll:TMat4( yaw#,pitch#,roll# )
		Local t:TMat4
		If yaw
			t=FromYaw(yaw)
			If pitch t=t.Times(FromPitch(pitch))
			If roll t=t.Times(FromRoll(roll))
		Else If pitch
			t=FromPitch(pitch)
			If roll t=t.Times(FromRoll(roll))
		Else If roll
			t=FromRoll(roll)
		Else
			t=Identity()
		EndIf
		Return t
	End Function
	
	Function FromOrtho:TMat4( l#,r#,b#,t#,n#,f# )
		Local q:TMat4=New TMat4
		q.ix=2/(r-l)
		q.tx=-(r+l)/(r-l)
		q.jy=2/(t-b)
		q.ty=-(t+b)/(t-b)
		q.kz=2/(f-n)
		q.tz=-(f+n)/(f-n)
		q.tw=1
		Return q
	End Function
	
	Function FromFrustum:TMat4( near_left#,near_right#,near_bottom#,near_top#,near#,far# )
		Local t:TMat4=New TMat4
		Local near2#=near*2
		Local w#=near_right-near_left
		Local h#=near_top-near_bottom
		Local d#=far-near
		t.ix=near2/w
		t.jy=near2/h
		t.kx=(near_right+near_left)/w
		t.ky=(near_top+near_bottom)/h
		t.kz=(far+near)/d
		t.kw=1
		t.tz=-(far*near2)/d
		Return t
	End Function
	
	Function FromInfiniteFrustum:TMat4( near_left#,near_right#,near_bottom#,near_top#,near# )
		Local t:TMat4=New TMat4
		Local near2#=near*2
		Local w#=near_right-near_left
		Local h#=near_top-near_bottom
		Local e#=.001
		t.ix=near2/w
		t.jy=near2/w
		t.kx=(near_right+near_left)/w
		t.ky=(near_top+near_bottom)/h
		t.kz=1-e
		t.kw=1
		t.tz=near*(e-1)
		Return t
	End Function

	Function Identity:TMat4()
		Local t:TMat4=New TMat4
		t.ix=1 ; t.jy=1 ; t.kz=1 ; t.tw=1
		Return t
	End Function

End Type