Blitz3D C++ geometry code

Here's a BMax version:

Const TWOPI:Float=Pi*2.0		'360 degrees
Const HALFPI:Float=Pi*.5		'90  degrees
Const QUARTERPI:Float=Pi*.25	'45  degrees
Const EPSILON:Float=.000001		'small value
Const INFINITY:Float=10000000.0	'big value

Type TVec

	Field x:Float,y:Float,z:Float

	Method Copy:TVec()
		Local q:TVec=New TVec
		q.x=x; q.y=y; q.z=z
		Return q
	End Method

	Function Create0:TVec()
		Local q:TVec=New TVec
		q.x=0.0; q.y=0.0; q.z=0.0
		Return q
	End Function
	
	Function Create:TVec(x#,y#,z#)
		Local q:TVec=New TVec
		q.x=x#; q.y=y#; q.z=z#
		Return q
	End Function
	
	Rem
	operator Float*() ' ? operator overload
		Return &x;
	}
	
	operator Const Float *() ' ? operator overload
		Return &x;
	}
	
	Float &operator[]( Int n ) ' ? operator overload
		Return (&x)[n]; 
	}
	
	Float operator[]( Int n ) ' ? operator overload
		Return (&x)[n]; 
	}
	End Rem
	
	Method Negate:TVec() ' - operator overload
		Return TVec.Create( -x,-y,-z )
	End Method
	
	Method Multiply:TVec( scale:Float ) ' * operator overload
		Return TVec.Create( x*scale,y*scale,z*scale )
	End Method
	
	Method MultiplyVector:TVec( q:TVec ) ' * operator overload
		Return TVec.Create( x*q.x,y*q.y,z*q.z )
	End Method
	
	Method Divide:TVec( scale:Float ) ' / operator overload
		Return TVec.Create( x/scale,y/scale,z/scale )
	End Method
	
	Method DivideVector:TVec( q:TVec ) ' / operator overload
		Return TVec.Create( x/q.x,y/q.y,z/q.z )
	End Method
	
	Method Add:TVec( q:TVec ) ' + operator overload
		Return TVec.Create( x+q.x,y+q.y,z+q.z )
	End Method
	
	Method Subtract:TVec( q:TVec ) ' - operator overload
		Return TVec.Create( x-q.x,y-q.y,z-q.z )
	End Method
	
	Rem
	Vector &operator*=( Float scale ) ' *= operator overload
		x*=scale;y*=scale;z*=scale;Return *this;
	}
	Vector &operator*=( Const Vector &q ) ' *= operator overload
		x*=q.x;y*=q.y;z*=q.z;Return *this;
	}
	Vector &operator/=( Float scale ) ' /= operator overload
		x/=scale;y/=scale;z/=scale;Return *this;
	}
	Vector &operator/=( Const Vector &q ) ' /= operator overload
		x/=q.x;y/=q.y;z/=q.z;Return *this;
	}
	End Rem
	
	Method Inc( q:TVec ) ' += operator overload
		x:+q.x;y:+q.y;z:+q.z
	End Method
	
	Method Dec( q:TVec ) ' -= operator overload
		x:-q.x;y:-q.y;z:-q.z
	End Method
	
	Method LessThan:Int( q:TVec ) ' < operator overload
		If( Abs(x-q.x)>EPSILON )
			If x<q.x Then Return True Else Return False
		EndIf
		If( Abs(y-q.y)>EPSILON )
			If y<q.y Then Return True Else Return False
		EndIf
		Return Abs(z-q.z)>EPSILON And z<q.z
	End Method
	
	Method Equals:Int( q:TVec ) ' = operator overload
		Return Abs(x-q.x)<=EPSILON And Abs(y-q.y)<=EPSILON And Abs(z-q.z)<=EPSILON
	End Method
	
	Method Inequality:Int( q:TVec ) ' != operator overload
		Return Abs(x-q.x)>EPSILON Or Abs(y-q.y)>EPSILON Or Abs(z-q.z)>EPSILON
	End Method
	
	Method Dot:Float( q:TVec )
		Return x*q.x+y*q.y+z*q.z
	End Method
	
	Method Cross:TVec( q:TVec )
		Return TVec.Create( y*q.z-z*q.y,z*q.x-x*q.z,x*q.y-y*q.x )
	End Method
	
	Method Length:Float()
		Return Sqr(x*x+y*y+z*z)
	End Method
	
	Method Distance:Float( q:TVec )
		Local dx:Float=x-q.x; Local dy:Float=y-q.y; Local dz:Float=z-q.z; Return Sqr(dx*dx+dy*dy+dz*dz)
	End Method
	
	Method Normalized:TVec()
		Local l:Float=Length(); Return TVec.Create( x/l,y/l,z/l )
	End Method
	
	Method Normalize()
		Local l:Float=Length(); x:/l; y:/l; z:/l
	End Method
	
	Method Yaw:Float()
		Return -ATan2( x,z )
	End Method
	
	Method Pitch:Float()
		Return -ATan2( y,Sqr( x*x+z*z ) )
	End Method
	
	Method Clear()
		x=0;y=0;z=0
	End Method
	
End Type

Type TLine

	Field o:TVec,d:TVec
	
	Function Create0:TLine()
		Local line:TLine=New Tline
		line.o=TVec.Create(0.0,0.0,0.0)
		line.d=TVec.Create(0.0,0.0,0.0)
		Return line
	End Function

	Function Create:TLine(o:TVec,d:TVec)
		Local line:TLine=New Tline
		line.o=TVec.Create(o.x,o.y,o.z)
		line.d=TVec.Create(d.x,d.y,d.z)
		Return line
	End Function
	
	Method Add:TLine( q:TVec ) ' + operator overload
		Return Create( o.Add(q),d );
	End Method
	
	Method Subtract:TLine( q:TVec ) ' - operator overload
		Return Create( o.Subtract(q),d );
	End Method
	
	Method Multiply:TVec( q:Float ) ' * operator overload
		Return o.Add(d.Multiply(q))
	End Method
	
	Method Nearest:TVec( q:TVec )
		Return o.Add( d.Multiply( ( d.Dot(q.Subtract(o)) / d.Dot(d) ) ) )
	End Method
	
End Type

Type TPlane

	Field n:TVec
	Field d:Float
	
	Method Copy:TPlane()
		Local plane:TPlane=New TPlane
		plane.n=TVec.Create(n.x,n.y,n.z)
		plane.d=d
		Return plane
	End Method

	Function Create0:TPlane()
		Local plane:TPlane=New TPlane
		plane.n=TVec.Create(0.0,0.0,0.0)
		plane.d=0.0
		Return plane
	End Function

	' normal/offset form
	Function Create:TPlane(n:TVec,d:Float)
		Local plane:TPlane=New TPlane
		plane.n=TVec.Create(n.x,n.y,n.z)
		plane.d=d
		Return plane
	End Function

	' point/normal form
	Function Create1:TPlane(p:TVec,n:TVec)
		Local plane:TPlane=New TPlane
		plane.n=TVec.Create(n.x,n.y,n.z)
		plane.d=n.Negate().Dot(p)
		Return plane
	End Function

	' create plane from tri
	Function CreateFromTri:TPlane(v0:TVec,v1:TVec,v2:TVec)
		Local plane:TPlane=New TPlane
		plane.n=(v1.Subtract(v0)).Cross(v2.Subtract(v0)).normalized()
		plane.d=plane.n.Negate().Dot(v0)
		Return plane
	End Function

	Method Negate:TPlane() ' - operator overload
		Return TPlane.Create( n.Negate(),-d )
	End Method
	
	Method t_intersect:Float( q:TLine )
		Return -Distance(q.o)/n.Dot(q.d)
	End Method
	
	Method IntersectLine:TVec( q:TLine )
		Return q.Multiply(t_intersect(q))
	End Method
	
	Method IntersectPlane:TLine( q:TPlane )
		Local lv:TVec=n.Cross( q.n ).Normalized()
		Return TLine.Create( q.IntersectLine( TLine.Create( Nearest( n.Multiply(-d) ),n.Cross(lv) ) ),lv )
	End Method
	
	Method Nearest:TVec( q:TVec )
		Return q.Subtract(n.Multiply(Distance(q)))
	End Method
	
	Method Negate2()
		n=n.Negate();d=-d
	End Method
	
	Method Distance:Float(q:TVec)
		Return n.Dot(q)+d
	End Method

End Type

Type TQuat

	Field w:Float
	Field v:TVec
	
	Function Create0:TQuat()
		Local q:TQuat=New TQuat
		q.w=1.0
		Return q
	End Function
	
	Function Create:TQuat( w:Float,v:TVec )
		Local q:TQuat=New TQuat
		q.w=1.0
		q.v.x=v.x
		q.v.y=v.y
		q.v.z=v.z
		Return q
	End Function
	
	Method Negate:TQuat() ' - operator overload
		Return TQuat.Create( w,v.Negate() )
	End Method
	
	Method Add:TQuat( q:TQuat ) ' + operator overload
		Return TQuat.Create( w+q.w,v.Add(q.v) )
	End Method
	
	Method Subtract:TQuat( q:TQuat ) ' - operator overload
		Return TQuat.Create( w-q.w,v.Subtract(q.v) )
	End Method
	
	Method Multiply:TQuat( q:TQuat ) ' * operator overload
		Return TQuat.Create( w*q.w-v.dot(q.v),(q.v.cross(v)).Add(q.v.Multiply(w)).Add(v.Multiply(q.w)) )
	End Method
	
	Method MultiplyVector:TVec( q:TVec ) ' * operator overload
		Return ( Self.Multiply(TQuat.Create(0,q)).Multiply(Self.Negate()) ).v
	End Method
	
	Method MultiplyFloat:TQuat( q:Float ) ' * operator overload
		Return TQuat.Create( w*q,v.Multiply(q) )
	End Method
	
	Method Divide:TQuat( q:Float ) ' / operator overload
		Return TQuat.Create( w/q,v.Divide(q) )
	End Method
	
	Method Dot:Float( q:TQuat )
		Return v.x*q.v.x+v.y*q.v.y+v.z*q.v.z+w*q.w
	End Method
	
	Method Length:Float()
		Return Sqr( w*w+v.x*v.x+v.y*v.y+v.z*v.z )
	End Method
	
	Method Normalize()
		w=w/Length()
		v.x=v.x/Length()
		v.y=v.y/Length()
		v.z=v.z/Length()
	End Method
	
	Method Normalized:TQuat()
		Local q:TQuat=New TQuat
		q.w=q.w/Length()
		q.v.x=q.v.x/Length()
		q.v.y=q.v.y/Length()
		q.v.z=q.v.z/Length()
		Return q
	End Method
	
	Method SlerpTo:TQuat( q:TQuat,a:Float )
		Local t:TQuat=q
		Local d:Float=dot(q),b:Float=1-a
		If( d<0 ) t.w=-t.w; t.v=t.v.Negate(); d=-d
		If( d<1-EPSILON )
			Local om:Float=ACos( d )
			Local si:Float=Sin( om )
			a=Sin( a*om )/si
			b=Sin( b*om )/si
		EndIf
		Return (Self.MultiplyFloat(b)).Add(t.MultiplyFloat(a))
	End Method
	
	Method i:TVec()
		Local xz:Float=v.x*v.z,wy:Float=w*v.y
		Local xy:Float=v.x*v.y,wz:Float=w*v.z
		Local yy:Float=v.y*v.y,zz:Float=v.z*v.z
		Return TVec.Create( 1-2*(yy+zz),2*(xy-wz),2*(xz+wy) )
	End Method
	
	Method j:TVec()
		Local yz:Float=v.y*v.z,wx:Float=w*v.x
		Local xy:Float=v.x*v.y,wz:Float=w*v.z
		Local xx:Float=v.x*v.x,zz:Float=v.z*v.z
		Return TVec.Create( 2*(xy+wz),1-2*(xx+zz),2*(yz-wx) )
	End Method
	
	Method k:TVec()
		Local xz:Float=v.x*v.z,wy:Float=w*v.y
		Local yz:Float=v.y*v.z,wx:Float=w*v.x
		Local xx:Float=v.x*v.x,yy:Float=v.y*v.y
		Return TVec.Create( 2*(xz-wy),2*(yz+wx),1-2*(xx+yy) )
	End Method
	
End Type

Type TMat

	Field i:TVec,j:TVec,k:TVec
	
	Method Copy:TMat()
		Local m:TMat=New TMat	
		m.i=TVec.Create(i.x,i.y,i.z)
		m.j=TVec.Create(j.x,j.y,j.z)
		m.k=TVec.Create(k.x,k.y,k.z)
		Return m
	End Method

	Function Create0:TMat()
		Local m:TMat=New TMat
		m.i=TVec.Create(1,0,0); m.j=TVec.Create(0,1,0); m.k=TVec.Create(0,0,1)
		Return m
	End Function
	
	Function Create:TMat( i:TVec,j:TVec,k:TVec )
		Local m:TMat=New TMat
		m.i=i; m.j=j; m.k=k
		Return m
	End Function
	
	Function CreateFromQuat:TMat( q:TQuat )
		Local m:TMat=New TMat
		Local xx:Float=q.v.x*q.v.x,yy:Float=q.v.y*q.v.y,zz:Float=q.v.z*q.v.z
		Local xy:Float=q.v.x*q.v.y,xz:Float=q.v.x*q.v.z,yz:Float=q.v.y*q.v.z
		Local wx:Float=q.w*q.v.x,wy:Float=q.w*q.v.y,wz:Float=q.w*q.v.z
		m.i=TVec.Create( 1-2*(yy+zz),2*(xy-wz),2*(xz+wy) )
		m.j=TVec.Create( 2*(xy+wz),1-2*(xx+zz),2*(yz-wx) )
		m.k=TVec.Create( 2*(xz-wy),2*(yz+wx),1-2*(xx+yy) )
		Return m
	End Function
	
	Function CreateFromAngle:TMat( angle:Float,axis:TVec )
		Local m:TMat=New TMat
		Local u:TVec=axis
		Local c:Float=Cos(angle),s:Float=Sin(angle)
		Local x2:Float=axis.x*axis.x,y2:Float=axis.y*axis.y,z2:Float=axis.z*axis.z
		m.i=TVec.Create( x2+c*(1-x2),u.x*u.y*(1-c)-u.z*s,u.z*u.x*(1-c)+u.y*s )
		m.j=TVec.Create( u.x*u.y*(1-c)+u.z*s,y2+c*(1-y2),u.y*u.z*(1-c)-u.x*s )
		m.k=TVec.Create( u.z*u.x*(1-c)-u.y*s,u.y*u.z*(1-c)+u.x*s,z2+c*(1-z2) )
		Return m
	End Function
	
	Rem
	Vector &operator[]( Int n ){
		Return (&i)[n]; 
	}
	Const Vector &operator[]( Int n )Const{
		Return (&i)[n];
	}
	End Rem
	
	Method Inverse:TMat() ' ~ operator overload - inverse?
		Local m:TMat=TMat.Create0()
		m.i.x=i.x;m.i.y=j.x;m.i.z=k.x
		m.j.x=i.y;m.j.y=j.y;m.j.z=k.y
		m.k.x=i.z;m.k.y=j.z;m.k.z=k.z
		Return m
	End Method
	
	Method Determinant:Float()
		Return i.x*(j.y*k.z-j.z*k.y )-i.y*(j.x*k.z-j.z*k.x )+i.z*(j.x*k.y-j.y*k.x )
	End Method
	
	Method Negate:TMat() ' - operator overload
		Local m:TMat=TMat.Create0()
		Local t:Float=1.0/Determinant()
		m.i.x= t*(j.y*k.z-j.z*k.y);m.i.y=-t*(i.y*k.z-i.z*k.y);m.i.z= t*(i.y*j.z-i.z*j.y)
		m.j.x=-t*(j.x*k.z-j.z*k.x);m.j.y= t*(i.x*k.z-i.z*k.x);m.j.z=-t*(i.x*j.z-i.z*j.x)
		m.k.x= t*(j.x*k.y-j.y*k.x);m.k.y=-t*(i.x*k.y-i.y*k.x);m.k.z= t*(i.x*j.y-i.y*j.x)
		Return m
	End Method

	Method Cofactor:TMat()
		Local m:TMat=TMat.Create0()
		m.i.x= (j.y*k.z-j.z*k.y);m.i.y=-(j.x*k.z-j.z*k.x);m.i.z= (j.x*k.y-j.y*k.x)
		m.j.x=-(i.y*k.z-i.z*k.y);m.j.y= (i.x*k.z-i.z*k.x);m.j.z=-(i.x*k.y-i.y*k.x)
		m.k.x= (i.y*j.z-i.z*j.y);m.k.y=-(i.x*j.z-i.z*j.x);m.k.z= (i.x*j.y-i.y*j.x)
		Return m
	End Method	
	
	Method Equals:Int( q:TMat ) ' == operator overload
		Return i=q.i And j=q.j And k=q.k
	End Method
	
	Method Inequality:Int( q:TMat  ) ' != operator overload
		Return i<>q.i Or j<>q.j Or k<>q.k
	End Method
	
	Method MultiplyVector:TVec( q:TVec ) ' * operator overload
		Return TVec.Create( i.x*q.x+j.x*q.y+k.x*q.z,i.y*q.x+j.y*q.y+k.y*q.z,i.z*q.x+j.z*q.y+k.z*q.z )
	End Method
	
	Method Multiply:TMat( q:TMat ) ' * operator overload
		Local m:TMat=TMat.Create0()
		m.i.x=i.x*q.i.x+j.x*q.i.y+k.x*q.i.z;m.i.y=i.y*q.i.x+j.y*q.i.y+k.y*q.i.z;m.i.z=i.z*q.i.x+j.z*q.i.y+k.z*q.i.z
		m.j.x=i.x*q.j.x+j.x*q.j.y+k.x*q.j.z;m.j.y=i.y*q.j.x+j.y*q.j.y+k.y*q.j.z;m.j.z=i.z*q.j.x+j.z*q.j.y+k.z*q.j.z
		m.k.x=i.x*q.k.x+j.x*q.k.y+k.x*q.k.z;m.k.y=i.y*q.k.x+j.y*q.k.y+k.y*q.k.z;m.k.z=i.z*q.k.x+j.z*q.k.y+k.z*q.k.z
		Return m
	End Method
	
	Method Orthogonalize()
		k.Normalize()
		i=j.Cross( k ).Normalized()
		j=k.Cross( i )
	End Method
	
	Method Orthogonalized:TMat()
		Local m:TMat=TMat.Create0()
		m.Orthogonalize()
		Return m
	End Method
	
End Type

Type TBox

	Field a:TVec,b:TVec
	
	Function Create0:TBox()
		Local box:TBox=New TBox
		box.a=TVec.Create(INFINITY,INFINITY,INFINITY)
		box.b=TVec.Create(-INFINITY,-INFINITY,-INFINITY)
		Return box
	End Function
	
	Function CreateFromPoint:TBox( q:TVec )
		Local box:TBox=New TBox
		box.a=TVec.Create(q.x,q.y,q.z)
		box.b=TVec.Create(q.x,q.y,q.z)
		Return box
	End Function
	
	Function Create:TBox( a:TVec,b:TVec )
		Local box:TBox=New TBox
		box.a=TVec.Create(a.x,a.y,a.z)
		box.b=TVec.Create(b.x,b.y,b.z)
		Return box
	End Function
	
	Function CreateFromLine:TBox( l:TLine )
		Local box:TBox=New TBox
		box.a=TVec.Create(l.o.x,l.o.y,l.o.z)
		box.b=TVec.Create(l.o.x,l.o.y,l.o.z)
		box.UpdateFromVector( l.o.Add(l.d) )
		Return box
	End Function
	
	Method Clear()
		a.x=INFINITY;a.y=INFINITY;a.z=INFINITY
		b.x=-INFINITY;b.y=-INFINITY;b.z=-INFINITY
	End Method
	
	Method Empty:Int()
		Return b.x<a.x Or b.y<a.y Or b.z<a.z
	End Method
	
	Method Centre:TVec()
		Return TVec.Create( (a.x+b.x)*.5,(a.y+b.y)*.5,(a.z+b.z)*.5 )
	End Method
	
	Method Corner:TVec( n:Int )
		Local x#=n&1;If x Then x=b.x Else x=a.x
		Local y#=n&2;If y Then y=b.y Else y=a.y
		Local z#=n&4;If z Then z=b.z Else z=a.z
		Return TVec.Create( x,y,z )
	End Method
	
	Method UpdateFromVector( q:TVec )
		If q.x<a.x Then a.x=q.x
		If q.y<a.y Then a.y=q.y
		If q.z<a.z Then a.z=q.z
		If q.x>b.x Then b.x=q.x
		If q.y>b.y Then b.y=q.y
		If q.z>b.z Then b.z=q.z
	End Method
	
	Method UpdateFromBox( q:TBox )
		If q.a.x<a.x Then a.x=q.a.x
		If q.a.y<a.y Then a.y=q.a.y
		If q.a.z<a.z Then a.z=q.a.z
		If q.b.x>b.x Then b.x=q.b.x
		If q.b.y>b.y Then b.y=q.b.y
		If q.b.z>b.z Then b.z=q.b.z
	End Method
	
	Method Overlaps:Int( q:TBox )
		Local x#=b.x<q.b.x;If x Then x=b.x Else x=q.b.x
		Local y#=b.y<q.b.y;If y Then y=b.y Else y=q.b.y
		Local z#=b.z<q.b.z;If z Then z=b.z Else z=q.b.z
		Return x And y And z
	End Method
	
	Method Expand( n:Float )
		a.x:-n;a.y:-n;a.z:-n;b.x:+n;b.y:+n;b.z:+n
	End Method
	
	Method Width:Int()
		Return b.x-a.x
	End Method
	
	Method Height:Int()
		Return b.y-a.y
	End Method
	
	Method Depth:Int()
		Return b.z-a.z
	End Method
	
	Method Contains:Int( q:TVec )
		Return q.x>=a.x And q.x<=b.x And q.y>=a.y And q.y<=b.y And q.z>=a.z And q.z<=b.z
	End Method
	
End Type

Type TTransform

	Field m:TMat
	Field v:TVec

	Function Create0:TTransform()
		Local tform:TTransform=New TTransform
		tform.m:TMat=TMat.Create0()
		tform.v:TVec=TVec.Create0()
		Return tform
	End Function

	Function CreateFromMatrix:TTransform(m:TMat)
		Local tform:TTransform=New TTransform
		tform.m=m.Copy()
		Return tform
	End Function
	
	Function CreateFromVector:TTransform(v:TVec)
		Local tform:TTransform=New TTransform
		tform.v=TVec.Create(v.x,v.y,v.z)
		Return tform
	End Function
	
	Function Create:TTransform(m:TMat,v:TVec)
		Local tform:TTransform=New TTransform
		tform.m=m.Copy()
		tform.v=TVec.Create(v.x,v.y,v.z)
		Return tform
	End Function
	
	Method Negate:TTransform() ' - operator overload
		Local t:TTransform=New TTransform
		t.m=m.Negate();t.v=t.m.MultiplyVector(v.Negate())
		Return t
	End Method
	
	Method Inverse:TTransform() ' ~ operator overload
		Local t:TTransform=New TTransform
		t.m=m.Inverse();t.v=t.m.MultiplyVector(v.Negate())
		Return t
	End Method
	
	Method MultiplyVector:TVec( q:TVec ) ' * operator overload
		Return m.MultiplyVector(q).Add(v)
	End Method
	
	Method MultiplyLine:TLine( q:TLine ) ' * operator overload
		Local t:TVec=Self.MultiplyVector(q.o)
		Return TLine.Create( t,Self.MultiplyVector(q.o.Add(q.d)).Subtract(t) )
	End Method
	
	Method MultiplyBox:TBox( q:TBox ) ' * operator overload
		Local t:TBox=TBox.CreateFromPoint(Self.MultiplyVector(q.corner(0)))
		For Local k:Int=1 To 8
			t.UpdateFromVector( Self.MultiplyVector(q.corner(k)) )
		Next
		Return t
	End Method
	
	Method MultiplyTransform:TTransform( q:TTransform ) ' operator overload
		Local t:TTransform=New TTransform
		t.m=m.Multiply(q.m);t.v=m.MultiplyVector(q.v.Add(v))
		Return t
	End Method
	
	Method Equals:Int( q:TTransform ) ' == operator overload
		Return m.Equals(q.m) And v.Equals(q.v)
	End Method
	
	Method Inequality:Int( q:TTransform ) ' != operator overload
		Return Not Equals( q )
	End Method
	
End Type

Function TransformRadius:Float( r:Float,t:TMat )
	Local sq_3:Float=Sqr(1.0/3.0)
	Return t.MultiplyVector( TVec.Create(sq_3,sq_3,sq_3) ).length()*r
End Function

Function PitchMatrix:TMat( q:Float )
	Return TMat.Create( TVec.Create(1,0,0),TVec.Create(0,Cos(q),Sin(q)),TVec.Create(0,-Sin(q),Cos(q)) )
End Function

Function YawMatrix:TMat( q:Float )
	Return TMat.Create( TVec.Create(Cos(q),0,Sin(q)),TVec.Create(0,1,0),TVec.Create(-Sin(q),0,Cos(q)) )
End Function

Function RollMatrix:TMat( q:Float )
	Return TMat.Create( TVec.Create(Cos(q),Sin(q),0),TVec.Create(-Sin(q),Cos(q),0),TVec.Create(0,0,1) )
End Function

Function MatrixPitch:Float( m:TMat )
	Return m.k.pitch()
End Function

Function MatrixYaw:Float( m:TMat )
	Return m.k.yaw()
End Function

Function MatrixRoll:Float( m:TMat )
	Return ATan2( m.i.y,m.j.y )
End Function

Function ScaleMatrix:TMat( x:Float,y:Float,z:Float )
	Return TMat.Create( TVec.Create( x,0,0 ),TVec.Create( 0,y,0 ),TVec.Create( 0,0,z ) )
End Function

Function ScaleMatrix1:TMat( scale:TVec )
	Return TMat.Create( TVec.Create( scale.x,0,0 ),TVec.Create( 0,scale.y,0 ),TVec.Create( 0,0,scale.z ) )
End Function

Function PitchQuat:TQuat( p:Float )
	Return TQuat.Create( Cos(p/-2),TVec.Create( Sin(p/-2),0,0 ) )
End Function

Function YawQuat:TQuat( y:Float )
	Return TQuat.Create( Cos(y/2),TVec.Create( 0,Sin(y/2),0 ) )
End Function

Function RollQuat:TQuat( r:Float )
	Return TQuat.Create( Cos(r/-2),TVec.Create( 0,0,Sin(r/-2) ) )
End Function

Function RotationMatrix:TMat( p:Float,y:Float,r:Float )
	Return YawMatrix(y).Multiply(PitchMatrix(p)).Multiply(RollMatrix(r))
End Function

Function RotationMatrix1:TMat( rot:TVec )
	Return YawMatrix(rot.y).Multiply(PitchMatrix(rot.x)).Multiply(RollMatrix(rot.z))
EndFunction

Function QuatPitch:Float( q:TQuat )
	Return q.k().Pitch()
End Function

Function QuatYaw:Float( q:TQuat )
	Return q.k().Yaw()
End Function

Function QuatRoll:Float( q:TQuat )
	Return MatrixRoll( TMat.CreateFromQuat(q) )
End Function

Function MatrixQuat:TQuat( p:TMat )
	Local m:TMat m=p
	m.Orthogonalize()
	Local t:Float=m.i.x+m.j.y+m.k.z,w:Float,x:Float,y:Float,z:Float
	If( t>EPSILON )
		t=Sqr( t+1 )*2
		x=(m.k.y-m.j.z)/t
		y=(m.i.z-m.k.x)/t
		z=(m.j.x-m.i.y)/t
		w=t/4
	Else If( m.i.x>m.j.y And m.i.x>m.k.z )
		t=Sqr( m.i.x-m.j.y-m.k.z+1 )*2
		x=t/4
		y=(m.j.x+m.i.y)/t
		z=(m.i.z+m.k.x)/t
		w=(m.k.y-m.j.z)/t
	Else If( m.j.y>m.k.z )
		t=Sqr( m.j.y-m.k.z-m.i.x+1 )*2
		x=(m.j.x+m.i.y)/t
		y=t/4
		z=(m.k.y+m.j.z)/t
		w=(m.i.z-m.k.x)/t
	Else
		t=Sqr( m.k.z-m.j.y-m.i.x+1 )*2
		x=(m.i.z+m.k.x)/t
		y=(m.k.y+m.j.z)/t
		z=t/4
		w=(m.j.x-m.i.y)/t
	EndIf
	Return TQuat.Create( w,TVec.Create( x,y,z ) )
End Function

Heavy useage of this is likely to be quite a bit slower than C++ due to the Bmax functions not being inlined and also due to the GC having to collect lots of vector objects.