3D OpenGL Engine

[EDIT]

I THINK this works. It's kind of hard to test, but I think it's right. Use VectorX,Y,Z() to return values. This is probably one of the toughest routines to code, because it is so difficult to test.

Function TFormPoint(x#,y#,z#,src_entity,dest_entity)
vectorx=x
vectory=y
vectorz=z
;Transform from source to world
If src_entity
	x=x*EntityScaleX(src_entity)
	y=y*EntityScaleY(src_entity)
	z=z*EntityScaleZ(src_entity)
	parentx#=entityx(src_entity)
	parenty#=entityy(src_entity)
	parentz#=entityz(src_entity)
	qx#=entityqx(src_entity)
	qy#=entityqy(src_entity)
	qz#=entityqz(src_entity)
	qw#=entityqw(src_entity)
	mat00#=(1.0-2.0*qx#*qx#-2.0*qz#*qz#)
	mat01#=-(2.0*qz#*qy#+2.0*qw#*qx#)
	mat02#=2.0*qw#*qz#-2.0*qx#*qy#
	mat10#=-(2.0*qz#*qy#-2.0*qw#*qx#)
	mat11#=1.0-2.0*qy#*qy#-2.0*qx#*qx#
	mat12#=2.0*qx#*qz#+2.0*qw#*qy#
	mat20#=-(2.0*qx#*qy#+2.0*qw#*qz#)
	mat21#=(2.0*qw#*qy#-2.0*qx#*qz#)
	mat22#=1.0-2.0*qz#*qz#-2.0*qy#*qy#
	vectorx#=x#*mat00#+y*mat10#+z*mat20#
	vectory#=x#*mat01#+y*mat11#+z*mat21#
	vectorz#=x#*mat02#+y*mat12#+z*mat22#
	vectorx#=vectorx#+entityx(src_entity)
	vectory#=vectory#+entityy(src_entity)
	vectorz#=vectorz#+entityz(src_entity)
	x#=vectorx#
	y#=vectory#
	z#=vectorz#
	parent=GetParent(src_entity)
	If parent
		tformpoint vectorx,vectory,vectorz,parent,0
		EndIf
	EndIf
;Transform from world to destination
If dest_entity
	parent=GetParent(dest_entity)
	If parent
		tformpoint vectorx,vectory,vectorz,0,parent
		EndIf
	x=x-entityx(dest_entity)
	y=y-entityy(dest_entity)
	z=z-entityz(dest_entity)
	qx#=entityqx(dest_entity)
	qy#=entityqy(dest_entity)
	qz#=entityqz(dest_entity)
	qw#=-entityqw(dest_entity)
	mat00#=(1.0-2.0*qx#*qx#-2.0*qz#*qz#)
	mat01#=-(2.0*qz#*qy#+2.0*qw#*qx#)
	mat02#=2.0*qw#*qz#-2.0*qx#*qy#
	mat10#=-(2.0*qz#*qy#-2.0*qw#*qx#)
	mat11#=1.0-2.0*qy#*qy#-2.0*qx#*qx#
	mat12#=2.0*qx#*qz#+2.0*qw#*qy#
	mat20#=-(2.0*qx#*qy#+2.0*qw#*qz#)
	mat21#=(2.0*qw#*qy#-2.0*qx#*qz#)
	mat22#=1.0-2.0*qz#*qz#-2.0*qy#*qy#
	vectorx#=x#*mat00#+y*mat10#+z*mat20#
	vectory#=x#*mat01#+y*mat11#+z*mat21#
	vectorz#=x#*mat02#+y*mat12#+z*mat22#
	vectorx=vectorx/EntityScaleX(dest_entity)
	vectory=vectory/EntityScaleY(dest_entity)
	vectorz=vectorz/EntityScaleZ(dest_entity)
	EndIf
End Function