OpenGL geometry code

Blitz3D Forums/Blitz3D Programming/OpenGL geometry code

JoshK

(Posted 2005) [#1]

These routines produce the same results as tests run against Blitz3D. Includes movement, transformations, and rotations:

Global vectorx#
Global vectory#
Global vectorz#
Global vectorw#

Function VectorX#()
Return vectorx
End Function

Function VectorY#()
Return vectory
End Function

Function VectorZ#()
Return vectorz
End Function

Function VectorW#()
Return vectorw
End Function

Function Magnitude#(nx#,ny#,nz#)
Return Sqr(nx#*nx#+ny#*ny#+nz#*nz#)
End Function

Function Normalize(nx#,ny#,nz#)
If nx=0 And ny=0 And nz=0 Return
m#=Magnitude(nx#,ny#,nz#)
vectorx#=nx#/m#
vectory#=ny#/m#
vectorz#=nz#/m#
End Function

Function DotProduct#(Ax#,Ay#,Az#,Bx#,By#,Bz#)
Return Ax#*Bx#+Ay#*By#+Az#*Bz#
End Function

Function CrossProduct(Ax#,Ay#,Az#,Bx#,By#,Bz#)
vectorx#=Ay#*Bz#-Az#*By#
vectory#=Az#*Bx#-Ax#*Bz#
vectorz#=Ax#*By#-Ay#*Bx#
End Function

Function ReverseVector(nx#,ny#,nz#)
vectorx#=-nx#
vectory#=-ny#
vectorz#=-nz#
End Function

Function AddVector(Ax#,Ay#,Az#,Bx#,By#,Bz#)
vectorx#=ax#+bx#
vectory#=ay#+by#
vectorz#=az#+bz#
End Function

Function SubVector(Ax#,Ay#,Az#,Bx#,By#,Bz#)
vectorx#=ax#-bx#
vectory#=ay#-by#
vectorz#=az#-bz#
End Function

Function MulVector(Ax#,Ay#,Az#,Bx#,By#,Bz#)
vectorx#=ax*bx
vectory#=ay*by
vectorz#=az*bz
End Function

Function AddVectorScalar(nx#,ny#,nz#,v#)
vectorx#=nx#+v#
vectory#=ny#+v#
vectorz#=nz#+v#
End Function

Function SubVectorScalar(nx#,ny#,nz#,v#)
vectorx#=nx#-v#
vectory#=ny#-v#
vectorz#=nz#-v#
End Function

Function MulVectorScalar(nx#,ny#,nz#,v#)
vectorx#=nx#*v#
vectory#=ny#*v#
vectorz#=nz#*v#
End Function

Function DivVectorScalar(nx#,ny#,nz#,v#)
vectorx#=nx#/v#
vectory#=ny#/v#
vectorz#=nz#/v#
End Function

Function EulerAsQuat(pitch#,yaw#,roll#)
cr#=Cos(-roll#/2.0)
cp#=Cos(pitch#/2.0)
cy#=Cos(yaw#/2.0)
sr#=Sin(-roll#/2.0)
sp#=Sin(pitch#/2.0)
sy#=Sin(yaw#/2.0)
cpcy#=cp#*cy#
spsy#=sp#*sy#
spcy#=sp#*cy#
cpsy#=cp#*sy#
vectorw#=cr#*cpcy#+sr#*spsy#
vectorx#=sr#*cpcy#-cr#*spsy#
vectory#=cr#*spcy#+sr#*cpsy#
vectorz#=cr#*cpsy#-sr#*spcy#
End Function

Function QuatAsEuler(x#,y#,z#,w#)
sint#=(2.0*w*y)-(2.0*x*z)
cost_temp#=1.0-sint*sint
If Abs(cost_temp#)>0.001
	cost#=Sqr(cost_temp)
	Else
	cost#=0.0
	EndIf
If Abs(cost#)>0.001
	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
vectorz#=-ATan2(sinv,cosv)
vectorx#=ATan2(sint,cost)
vectory#=ATan2(sinf,cosf)
End Function

Function MulQuat(Ax#,Ay#,Az#,Aw#,Bx#,By#,Bz#,Bw#)
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#)
vectorw#=b#+(-e#-f#+g#+h#)/2.0
vectorx#=a#-(e#+f#+g#+h#)/2.0
vectory#=c#+(e#-f#+g#-h#)/2.0
vectorz#=d#+(e#-f#-g#+h#)/2.0
End Function

Function Slerp(Ax#,Ay#,Az#,Aw#,Bx#,By#,Bz#,Bw#,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
	vectorx#=ax
	vectory#=ay
	vectorz#=az
	vectorw#=aw
	Return True
	EndIf
cosineom#=Ax#*Bx#+Ay#*By#+Az#*Bz#+Aw#*Bw#
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
If (1.0 - cosineom#)>0.0001
	omega#=ACos(cosineom#)
	sineom#=Sin(omega#)
	scale0#=Sin((1.0-t#)*omega#)/sineom#
	scale1#=Sin(t#*omega#)/sineom#
	Else
	scale0#=1.0-t#
	scale1#=t#
	EndIf
vectorw#=scale0#*Aw#+scale1#*scaler_w#
vectorx#=scale0#*Ax#+scale1#*scaler_x#
vectory#=scale0#*Ay#+scale1#*scaler_y#
vectorz#=scale0#*Az#+scale1#*scaler_z#
End Function

Function TurnQuat(Ax#,Ay#,Az#,Aw#,x#,y#,z#)
EulerAsQuat(x#,y#,z#)
mulquat(ax#,ay#,az#,aw#,vectorx#,vectory#,vectorz#,vectorw#)
End Function

Function PointPlaneDistance#(x#,y#,z#,nx#,ny#,nz#,d#)
Return (d-nx*x-ny*y-nz*z)
End Function

Function LineIntersectsPlane(Ax#,Ay#,Az#,Bx#,By#,Bz#,nx#,ny#,nz#,d#)
u#=(nx*Ax+ny*Ay+nz*Az+d)/(nx*(Ax-Bx)+ny*(Ay-By)+nz*(Az-Bz))
vectorx=Ax+(Bx-Ax)*u
vectory=Ay+(By-Ay)*u
vectorz=Az+(Bz-Az)*u
If u>=0.0 And u<=1.0 Return True
End Function

Function TriangleNormal#(Ax#,Ay#,Az#,Bx#,By#,Bz#,Cx#,Cy#,Cz#)
SubVector Bx#,By#,Bz#,Ax#,Ay#,Az#
ux#=VectorX()
uy#=VectorY()
uz#=VectorZ()
SubVector Cx#,Cy#,Cz#,Bx#,By#,Bz#
vx#=VectorX()
vy#=VectorY()
vz#=VectorZ()
CrossProduct vx#,vy#,vz#,ux#,uy#,uz#
Normalize vectorx,vectory,vectorz
vectorw=Ax#*vectorx+Ay#*vectory+Az#*vectorz
End Function

Function PlanesIntersect(nx1#,ny1#,nz1#,d1#,nx2#,ny2#,nz2#,d2#,nx3#,ny3#,nz3#,d3#)
CrossProduct(nx2,ny2,nz2,nx3,ny3,nz3)
a#=vectorx*d1
CrossProduct(nx3,ny3,nz3,nx1,ny1,nz1)
b#=vectorX*d2
CrossProduct(nx1,ny1,nz1,nx2,ny2,nz2)
c#=vectorX*d3
numerator#=a+b+c
CrossProduct(nx2,ny2,nz2,nx3,ny3,nz3)
denominator#=DotProduct(nx1,ny1,nz1,vectorX,vectorY,vectorZ)
If denominator=0.0 Return False
IntersectedX#=numerator/denominator
CrossProduct(nx2,ny2,nz2,nx3,ny3,nz3)
a#=vectorY*d1
CrossProduct(nx3,ny3,nz3,nx1,ny1,nz1)
b#=vectorY*d2
CrossProduct(nx1,ny1,nz1,nx2,ny2,nz2)
c#=vectorY*d3
numerator#=a+b+c
CrossProduct(nx2,ny2,nz2,nx3,ny3,nz3)
denominator#=DotProduct(nx1,ny1,nz1,vectorX,vectorY,vectorZ)
If denominator=0.0 Return False
IntersectedY#=numerator/denominator
CrossProduct(nx2,ny2,nz2,nx3,ny3,nz3)
a#=vectorZ*d1
CrossProduct(nx3,ny3,nz3,nx1,ny1,nz1)
b#=vectorZ*d2
CrossProduct(nx1,ny1,nz1,nx2,ny2,nz2)
c#=vectorZ*d3
numerator#=a+b+c
CrossProduct(nx2,ny2,nz2,nx3,ny3,nz3)
denominator#=DotProduct(nx1,ny1,nz1,vectorX,vectorY,vectorZ)
If denominator=0.0 Return False
IntersectedZ#=numerator/denominator
vectorx=intersectedx
vectory=intersectedy
vectorz=intersectedz
Return True
End Function

Global TFORMEDVALUE_X#
Global TFORMEDVALUE_Y#
Global TFORMEDVALUE_Z#

Function TFormedX#()
Return TFORMEDVALUE_X
End Function

Function TFormedY#()
Return TFORMEDVALUE_Y
End Function

Function TFormedZ#()
Return TFORMEDVALUE_Z
End Function

Function TFormPoint(x#,y#,z#,src,dst)
TFORMEDVALUE_X=x
TFORMEDVALUE_Y=y
TFORMEDVALUE_Z=z
If src=dst Return
;Transform from source to parent
If src
	EulerAsQuat(ObjectPitch(src),ObjectYaw(src),ObjectRoll(src))
	qx#=vectorx()
	qy#=vectory()
	qz#=vectorz()
	qw#=vectorw()
	mat00#=(1.0-2.0*qx#*qx#-2.0*qz#*qz#)
	mat10#=-(2.0*qz#*qy#-2.0*qw#*qx#)
	mat20#=-(2.0*qx#*qy#+2.0*qw#*qz#)
	mat01#=-(2.0*qz#*qy#+2.0*qw#*qx#)
	mat11#=(1.0-2.0*qy#*qy#-2.0*qx#*qx#)
	mat21#=-(2.0*qw#*qy#-2.0*qx#*qz#)
	mat02#=2.0*qw#*qz#-2.0*qx#*qy#
	mat12#=2.0*qx#*qz#+2.0*qw#*qy#
	mat22#=1.0-2.0*qz#*qz#-2.0*qy#*qy#
	TFORMEDVALUE_X#=x#*mat00#+y*mat10#+z*mat20#
	TFORMEDVALUE_y#=x#*mat01#+y*mat11#+z*mat21#
	TFORMEDVALUE_z#=x#*mat02#+y*mat12#+z*mat22#
	TFORMEDVALUE_x#=TFORMEDVALUE_x#+objectx(src,0)
	TFORMEDVALUE_y#=TFORMEDVALUE_y#+objecty(src,0)
	TFORMEDVALUE_z#=TFORMEDVALUE_z#+objectz(src,0)
	parent=GetParent(src)
	If parent
		If ObjectClass(parent)<>CLASS_MODEL And ObjectClass(parent)<>CLASS_MODELREFERENCE
			tformpoint(TFORMEDVALUE_X,TFORMEDVALUE_Y,TFORMEDVALUE_Z,parent,0)
			EndIf
		EndIf
	EndIf
;Transform from world to destination
If dst
	parent=dst
	buffer=CreateBank(0)
	While parent
		If ObjectClass(parent)<>CLASS_MODEL And ObjectClass(parent)<>CLASS_MODELREFERENCE
			appendint buffer,parent
			parent=getparent(parent)
			entities=entities+1
			EndIf
		Wend
	For n=0 To entities-1
		dst=PeekInt(buffer,(entities-1-n)*4)
		x=TFORMEDVALUE_x-objectx(dst,0)
		y=TFORMEDVALUE_y-objecty(dst,0)
		z=TFORMEDVALUE_z-objectz(dst,0)
		EulerAsQuat ObjectPitch(dst),ObjectYaw(dst),ObjectRoll(dst)
		qx#=vectorx()
		qy#=vectory()
		qz#=vectorz()
		qw#=-vectorw()
		mat00#=(1.0-2.0*qx#*qx#-2.0*qz#*qz#)
		mat10#=-(2.0*qz#*qy#-2.0*qw#*qx#)
		mat20#=-(2.0*qx#*qy#+2.0*qw#*qz#)
		mat01#=-(2.0*qz#*qy#+2.0*qw#*qx#)
		mat11#=(1.0-2.0*qy#*qy#-2.0*qx#*qx#)
		mat21#=-(2.0*qw#*qy#-2.0*qx#*qz#)
		mat02#=2.0*qw#*qz#-2.0*qx#*qy#
		mat12#=2.0*qx#*qz#+2.0*qw#*qy#
		mat22#=1.0-2.0*qz#*qz#-2.0*qy#*qy#
		TFORMEDVALUE_X#=x#*mat00#+y*mat10#+z*mat20#
		TFORMEDVALUE_Y#=x#*mat01#+y*mat11#+z*mat21#
		TFORMEDVALUE_Z#=x#*mat02#+y*mat12#+z*mat22#
		Next
	FreeBank(buffer)
	EndIf
End Function

Function TFormVector(x#,y#,z#,src,dst)
TFORMEDVALUE_X=x
TFORMEDVALUE_Y=y
TFORMEDVALUE_Z=z
If src=dst Return
;Transform from source to parent
If src
	EulerAsQuat(ObjectPitch(src),ObjectYaw(src),ObjectRoll(src))
	qx#=vectorx()
	qy#=vectory()
	qz#=vectorz()
	qw#=vectorw()
	mat00#=(1.0-2.0*qx#*qx#-2.0*qz#*qz#)
	mat10#=-(2.0*qz#*qy#-2.0*qw#*qx#)
	mat20#=-(2.0*qx#*qy#+2.0*qw#*qz#)
	mat01#=-(2.0*qz#*qy#+2.0*qw#*qx#)
	mat11#=(1.0-2.0*qy#*qy#-2.0*qx#*qx#)
	mat21#=-(2.0*qw#*qy#-2.0*qx#*qz#)
	mat02#=2.0*qw#*qz#-2.0*qx#*qy#
	mat12#=2.0*qx#*qz#+2.0*qw#*qy#
	mat22#=1.0-2.0*qz#*qz#-2.0*qy#*qy#
	TFORMEDVALUE_X#=x#*mat00#+y*mat10#+z*mat20#
	TFORMEDVALUE_y#=x#*mat01#+y*mat11#+z*mat21#
	TFORMEDVALUE_z#=x#*mat02#+y*mat12#+z*mat22#
	parent=GetParent(src)
	If parent
		If ObjectClass(parent)<>CLASS_MODEL And ObjectClass(parent)<>CLASS_MODELREFERENCE
			tformpoint(TFORMEDVALUE_X,TFORMEDVALUE_Y,TFORMEDVALUE_Z,parent,0)
			EndIf
		EndIf
	EndIf
;Transform from world to destination
If dst
	parent=dst
	buffer=CreateBank(0)
	While parent
		If ObjectClass(parent)<>CLASS_MODEL And ObjectClass(parent)<>CLASS_MODELREFERENCE
			appendint buffer,parent
			parent=getparent(parent)
			entities=entities+1
			EndIf
		Wend
	For n=0 To entities-1
		dst=PeekInt(buffer,(entities-1-n)*4)
		x=TFORMEDVALUE_x
		y=TFORMEDVALUE_y
		z=TFORMEDVALUE_z
		EulerAsQuat ObjectPitch(dst),ObjectYaw(dst),ObjectRoll(dst)
		qx#=vectorx()
		qy#=vectory()
		qz#=vectorz()
		qw#=-vectorw()
		mat00#=(1.0-2.0*qx#*qx#-2.0*qz#*qz#)
		mat10#=-(2.0*qz#*qy#-2.0*qw#*qx#)
		mat20#=-(2.0*qx#*qy#+2.0*qw#*qz#)
		mat01#=-(2.0*qz#*qy#+2.0*qw#*qx#)
		mat11#=(1.0-2.0*qy#*qy#-2.0*qx#*qx#)
		mat21#=-(2.0*qw#*qy#-2.0*qx#*qz#)
		mat02#=2.0*qw#*qz#-2.0*qx#*qy#
		mat12#=2.0*qx#*qz#+2.0*qw#*qy#
		mat22#=1.0-2.0*qz#*qz#-2.0*qy#*qy#
		TFORMEDVALUE_X#=x#*mat00#+y*mat10#+z*mat20#
		TFORMEDVALUE_Y#=x#*mat01#+y*mat11#+z*mat21#
		TFORMEDVALUE_Z#=x#*mat02#+y*mat12#+z*mat22#
		Next
	FreeBank(buffer)
	EndIf
End Function

Function TFormQuat()

End Function

Function TFormEuler()

End Function

Function TranslateObject(obj,x#,y#,z#)
positionobject(obj,ObjectX(obj)+x#,ObjectY(obj)+y#,ObjectZ(obj)+z#)
End Function

Function MoveObject(obj,x#,y#,z#)
EulerasQuat(objectpitch(obj),objectyaw(obj),objectroll(obj))
qx#=vectorx()
qy#=vectory()
qz#=vectorz()
qw#=vectorw()
mat00#=(1.0-2.0*qx#*qx#-2.0*qz#*qz#)
mat10#=-(2.0*qz#*qy#-2.0*qw#*qx#)
mat20#=-(2.0*qx#*qy#+2.0*qw#*qz#)
mat01#=-(2.0*qz#*qy#+2.0*qw#*qx#)
mat11#=(1.0-2.0*qy#*qy#-2.0*qx#*qx#)
mat21#=-(2.0*qw#*qy#-2.0*qx#*qz#)
mat02#=2.0*qw#*qz#-2.0*qx#*qy#
mat12#=2.0*qx#*qz#+2.0*qw#*qy#
mat22#=1.0-2.0*qz#*qz#-2.0*qy#*qy#
mx#=x#*mat00#+y*mat10#+z*mat20#+objectx(obj)
my#=x#*mat01#+y*mat11#+z*mat21#+objecty(obj)
mz#=x#*mat02#+y*mat12#+z*mat22#+objectz(obj)
positionobject(obj,mx#,my#,mz#)
End Function

To set up a model matrix in OpenGL:

glPushMatrix()
glTranslatef x,y,z
glRotatef -yaw,0,1,0
glRotatef pitch,1,0,0
glRotatef roll,0,0,1
glScalef x,y,z
;render object
;render children
glPopMatrix()

To set up a camera matrix in OpenGL (changes handedness of coordinate system to match DirectX):

glRotatef pitch,1,0,0
glRotatef -yaw,0,1,0
glTranslatef -x,-y,z
glScalef 1,1,-1

Tom	(Posted 2005) [#2]

I was going to go with DX orientation when initialy starting on a max GL engine, but ended up tripping all over myself.

I'm not sure why you posted the GL stuff here :) Why not post in the OpenGL forum for more discussion & feedback?

Cheers
Tom