;vividgl-3d lite.
;
;(c)antony wells 2004
;
;--[ maths & 3d feedback module ]-[core]
;
;-[ these mostly wrap mark's ever popular tform funcs.(i know i need 'em!)
;why reinvent the wheel when it already works!?
;that said, vivid pro does it all in C++.
;Which may change depending on how great bmax is of course :) (Which it will be.)

Function entity_alignvector(entity,xvector#,yvector#,zvector#,axis,rate#=1)
	ent.ent=Object.ent(entity)
	AlignToVector ent\vpiv,xvector,yvector,zvector,axis,rate
End Function

Function TForm_Point(x#,y#,z#,from=0,go=0)
If from<>0
	fromt.ent=Object.ent(from)
	fromentity=fromt\vpiv
EndIf
If go<>0
	got.ent=Object.ent(go)
	goentity=got\vpiv
EndIf
	TFormPoint x,y,z,fromentity,goentity
End Function

Function TForm_Vector(x#,y#,z#,from=0,go=0)
If from<>0
	fromt.ent=Object.ent(from)
	fromentity=fromt\vpiv
EndIf
If go<>0
	got.ent=Object.ent(go)
	goentity=got\vpiv
EndIf
	TFormVector x,y,z,fromentity,goentity
End Function

Function tform_normal(x#,y#,z#,from=0,go=0)

	If from<>0
	fromt.ent=Object.ent(from)
	fromentity=fromt\vpiv
EndIf
If go<>0
	got.ent=Object.ent(go)
	goentity=got\vpiv
EndIf
	TFormNormal x,y,z,fromentity,goentity
End Function

;Entity_UpdateTangs & Update Norms
;were converted from code originally by halo.
;The C++ version(Which uses texture tangents
;rather than tri tangents) is coming in update 1.
Function entity_updatetangentsHalo(mesh,x#=-1,y#=-1,z#=1,range#=1500)

	temp=Entity_Pivot()
	For s=0 To Entity_CountSurfaces(mesh)-1
		surf=Entity_GetSurface(mesh,s)
		Surface_Lock(surf,0)
		For v=0 To Surface_CountVertices()-1
			TForm_Point Surface_VertexX(v),Surface_VertexY(v),Surface_VertexZ(v),mesh,0
			tx#=TFormedX()
			ty#=TFormedY()
			tz#=TFormedZ()
			xd#=tx-x
			yd#=ty-y
			zd#=tz-z
			Distance#=Sqr(xd*xd+yd*yd+zd*zd)
		;	if distance<range 
			intensity#=(1.0-(Distance#/range#))
			vx#=(tx-x)/Distance
			vy#=(ty-y)/Distance
			vz#=(tz-z)/Distance
			glentityparent temp,mesh
			entity_alignvector temp,Surface_VertexNX(v),Surface_VertexNY(v),Surface_VertexNZ(v),3,1
			Entity_Rotate temp,EntPi(temp,True),EntYa(temp,True),0
			tform_normal vx,vy,vz,0,temp
			r#=255-(1.0-TFormedX())*127.5
			g#=255-(1.0-TFormedY())*127.5
			b#=(1.0-TFormedZ())*127.5*intensity
			Surface_VertexColor v,r,g,b,1
		entity_noparent(temp)
		;	endif
		Next
		surface_updatecolors(surf)
	Next			

	Entity_Free temp
End Function

Function Entity_UpdateNormalsHalo(mesh)
	For s=0 To Entity_CountSurfaces(mesh)-1
		surf=Entity_GetSurface(mesh,s)
		Surface_Lock(surf,0)
		For v=0 To Surface_CountVertices()-1 
			nx#=0.0 
			ny#=0.0 
			nz#=0.0 
			triscount=0 
	For t=0 To Surface_CountTriangles()-1 
		a=Surface_TriangleVertex(t,0)
		b=Surface_TriangleVertex(t,1) 
		c=Surface_TriangleVertex(t,2) 
		If a=v Or b=v Or c=v
			ax#=Surface_VertexX(a) 
			ay#=Surface_VertexY(a) 
			az#=Surface_VertexZ(a) 
			TForm_Point ax,ay,az,mesh,0 
			ax=TFormedX() 
			ay=TFormedY() 
			az=TFormedZ() 	
			bx#=Surface_VertexX(b) 
			by#=Surface_VertexY(b) 
			bz#=Surface_VertexZ(b) 
			TForm_Point bx,by,bz,mesh,0 
			bx=TFormedX()
			by=TFormedY() 
			bz=TFormedZ() 
			cx#=Surface_VertexX(c) 
			cy#=Surface_VertexY(c) 
			cz#=Surface_VertexZ(c) 
			TForm_Point cx,cy,cz,mesh,0 	
			cx=TFormedX()
			cy=TFormedY() 
			cz=TFormedZ() 
			ux#=bx-ax 
			uy#=by-ay
			uz#=bz-az 
			vx#=cx-ax
			vy#=cy-ay
			vz#=cz-az 
			x#=(uy*vz)-(uz*vy) 
			y#=(uz*vx)-(ux*vz) 
			z#=(ux*vy)-(uy*vx) 
			nx=nx+x 
			ny=ny+y 
			nz=nz+z 
			triscount=triscount+1 
		EndIf
	Next 
	l#=Sqr(nx^2+ny^2+nz^2) 
	nx=nx/l 
	ny=ny/l 
	nz=nz/l 
	TForm_Vector nx,ny,nz,0,mesh
	Surface_VertexNormals v,TFormedX(),TFormedY(),TFormedZ() 
	Next
	surface_updatenormals(surf)
Next 
End Function 


Function tForm_X#() ;faster to just call the original commands
	Return TFormedX()
End Function

Function tForm_Y#()
	Return TFormedY()
End Function

Function tForm_Z#()
	Return TFormedZ()
End Function

Global glvectorx#,glvectory#,glvectorz#
Function Entity_Vector(fromentity,toentity,normalize=True)
	in1.ent=Object.ent(fromentity)
	in2.ent=Object.ent(fromentity)
	glvectorx=in2\x-in1\x
	glvectory=in2\y-in1\y
	glvectorz=in2\z-in2\z
	If normalize 
		td# =Sqr(glvectorx*glvectorx+glvectory*glvectory+glvectorz*glvectorz)
		glvectorx=glvectorx/td
		glvectory=glvectory/td
		glvectorz=glvectorz/td
	EndIf
End Function

Function Vector_X#()
	Return glvectorx
End Function
Function Vector_Y#()
	Return glvectory
End Function
Function Vector_Z#()
	Return glvectorz
End Function

; change these constants if you notice slips in accuracy
Const quattoeuleraccuracy# = 0.001
Const quatslerpaccuracy#   = 0.0001

Function rotation.rotation(pitch#,yaw#,roll#)
	r.rotation=New rotation
	r\pitch=pitch
	r\yaw=yaw
	r\roll=roll
	Return r
End Function

Function copyquat.quat(in.quat)
	out.quat=New quat
	out\w=in\w
	out\x=in\x
	out\y=in\y
	out\z=in\z
	Return out
End Function


Function quat.quat(w#,x#,y#,z#)
	q.quat=New quat
	q\w=w
	q\x=x
	q\y=y
	q\z=z
	Return q
End Function
Function setquat(q.quat,w#,x#,y#,z#)
	q\w=w
	q\x=x
	q\y=y
	q\z=z
End Function

Function setrotation(r.rotation,pitch#,yaw#,roll#)
	r\pitch=pitch
	r\yaw=yaw
	r\roll=roll
End Function

;Calculate normals is a public domain function wrote
;Vivid-pro does this in C++ :)

Dim Face_NX#(32768)
Dim Face_NY#(32768)
Dim Face_NZ#(32768)
		
Dim Vertex_ConnectedTris(32768)
Dim Vertex_TriList(32768, 32)

Function Entity_GenNormals(ThisMesh)
	Surfaces = Entity_CountSurfaces(ThisMesh)
	For LOOP_Surface = 0 To Surfaces-1

		Surface_Handle = Entity_GetSurface(ThisMesh, LOOP_Surface)

		For LoopV = 0 To 32767	
			Vertex_ConnectedTris(LoopV) = 0
		Next	

		Surface_Lock(Surface_Handle)
			Tris = Surface_CountTriangles()
		For LOOP_Tris = 0 To Tris-1
				Vertex_0 = Surface_TriangleVertex( LOOP_Tris, 0)
				Vertex_1 = Surface_TriangleVertex( LOOP_Tris, 1)
				Vertex_2 = Surface_TriangleVertex( LOOP_Tris, 2)
				ConnectedTris = Vertex_ConnectedTris(Vertex_0)
				Vertex_TriList(Vertex_0, ConnectedTris) = LOOP_Tris
				Vertex_ConnectedTris(Vertex_0) = ConnectedTris + 1

				ConnectedTris = Vertex_ConnectedTris(Vertex_1)
				Vertex_TriList(Vertex_1, ConnectedTris) = LOOP_Tris
				Vertex_ConnectedTris(Vertex_1) = ConnectedTris + 1

				ConnectedTris = Vertex_ConnectedTris(Vertex_2)
				Vertex_TriList(Vertex_2, ConnectedTris) = LOOP_Tris
				Vertex_ConnectedTris(Vertex_2) = ConnectedTris + 1
				Ax# = Surface_VertexX#( Vertex_0)
				Ay# = Surface_VertexY#( Vertex_0)
				Az# = Surface_VertexZ#( Vertex_0)
				Bx# = Surface_VertexX#( Vertex_1)
				By# = Surface_VertexY#( Vertex_1)
				Bz# = Surface_VertexZ#( Vertex_1)
				Cx# = Surface_VertexX#(Vertex_2)
				Cy# =Surface_VertexY#(Vertex_2)
				Cz# = Surface_VertexZ#( Vertex_2)
				Px# = Ax#-Bx#
				Py# = Ay#-By#
				Pz# = Az#-Bz#
				Qx# = Bx#-Cx#
				Qy# = By#-Cy#
				Qz# = Bz#-Cz#
				Nx# = Py#*Qz# - Pz#*Qy#
				Ny# = Pz#*Qx# - Px#*Qz#
				Nz# = Px#*Qy# - Py#*Qx#
				Face_NX#(LOOP_Tris) = Nx#
				Face_NY#(LOOP_Tris) = Ny#
				Face_NZ#(LOOP_Tris) = Nz#
		Next
		Vertices = Surface_CountVertices()
		For LOOP_Vertices = 0 To Vertices-1
			Nx# = 0
			Ny# = 0
			Nz# = 0
			Polys = Vertex_ConnectedTris(LOOP_Vertices)
			For LOOP_Polys = 0 To Polys-1
				ThisPoly = Vertex_TriList(LOOP_Vertices, LOOP_Polys)
				Nx# = Nx# + Face_NX#(ThisPoly)
				Ny# = Ny# + Face_NY#(ThisPoly)
				Nz# = Nz# + Face_NZ#(ThisPoly)			
			Next	
				Nl# = Sqr(Nx#^2 + Ny#^2 + Nz#^2)
				Nx# = Nx# / Nl#
				Ny# = Ny# / Nl#
				Nz# = Nz# / Nl#
				Surface_VertexNormals LOOP_Vertices, Nx#, Ny#, Nz#
		Next
	Next
End Function

;--Legacy 3rd party public domain functions. These are no longer used, they are useful
;functions however.
Function eulertoquat(out.quat, src.rotation)
; nb roll is inverted due to change in handedness of coordinate systems
Local cr# = Cos(-src\roll/2)
Local cp# = Cos(src\pitch/2)
Local cy# = Cos(src\yaw/2)

Local sr# = Sin(-src\roll/2)
Local sp# = Sin(src\pitch/2)
Local sy# = Sin(src\yaw/2)

; these variables are only here to cut down on the number of multiplications
Local cpcy# = cp * cy
Local spsy# = sp * sy
Local spcy# = sp * cy
Local cpsy# = cp * sy

; generate the output quat
out\w = cr * cpcy + sr * spsy
out\x = sr * cpcy - cr * spsy
out\y = cr * spcy + sr * cpsy
out\z = cr * cpsy - sr * spcy
End Function

Function quattoeuler(out.rotation, src.quat)
Local sint#, cost#, sinv#, cosv#, sinf#, cosf#
Local cost_temp#

sint = (2 * src\w * src\y) - (2 * src\x * src\z)
cost_temp = 1.0 - (sint * sint)

If Abs(cost_temp) > quattoeuleraccuracy
cost = Sqr(cost_temp)
Else
cost = 0
EndIf

If Abs(cost) > quattoeuleraccuracy
sinv = ((2. * src\y * src\z) + (2 * src\w * src\x)) / cost
cosv = (1. - (2. * src\x * src\x) - (2 * src\y * src\y)) / cost
sinf = ((2. * src\x * src\y) + (2 * src\w * src\z)) / cost
cosf = (1. - (2. * src\y * src\y) - (2 * src\z * src\z)) / cost
Else
sinv = (2 * src\w * src\x) - (2 * src\y * src\z)
cosv = 1 - (2 * src\x * src\x) - (2 * src\z * src\z)
sinf = 0
cosf = 1
EndIf

; generate the output rotation
out\roll = -ATan2(sinv, cosv) ;  inverted due to change in handedness of coordinate system
out\pitch = ATan2(sint, cost)
out\yaw = ATan2(sinf, cosf)
End Function

Function quatslerp(res.quat, start.quat, fin.quat, t#)
Local scaler_w#, scaler_x#, scaler_y#, scaler_z#
Local omega#, cosom#, sinom#, scale0#, scale1#

cosom = start\x * fin\x + start\y * fin\y + start\z * fin\z + start\w * fin\w

If cosom <= 0.0
cosom = -cosom
scaler_w = -fin\w
scaler_x = -fin\x
scaler_y = -fin\y
scaler_z = -fin\z
Else
scaler_w = fin\w
scaler_x = fin\x
scaler_y = fin\y
scaler_z = fin\z
EndIf

If (1.0 - cosom) > quatslerpaccuracy
omega = ACos(cosom)
sinom = Sin(omega)
scale0 = Sin((1 - t) * omega) / sinom
scale1 = Sin(t * omega) / sinom
Else
; angle too small: use linear interpolation instead
scale0 = 1 - t
scale1 = t
EndIf

res\x = scale0 * start\x + scale1 * scaler_x
res\y = scale0 * start\y + scale1 * scaler_y
res\z = scale0 * start\z + scale1 * scaler_z
res\w = scale0 * start\w + scale1 * scaler_w
End Function

Function multiplyquat(q1.quat, q2.quat)
Local a#, b#, c#, d#, e#, f#, g#, h#

a = (q1\w + q1\x) * (q2\w + q2\x)
b = (q1\z - q1\y) * (q2\y - q2\z)
c = (q1\w - q1\x) * (q2\y + q2\z)
d = (q1\y + q1\z) * (q2\w - q2\x)
e = (q1\x + q1\z) * (q2\x + q2\y)
f = (q1\x - q1\z) * (q2\x - q2\y)
g = (q1\w + q1\y) * (q2\w - q2\z)
h = (q1\w - q1\y) * (q2\w + q2\z)

q1\w = b + (-e - f + g + h) / 2.
q1\x = a - ( e + f + g + h) / 2.
q1\y = c + ( e - f + g - h) / 2.
q1\z = d + ( e - f - g + h) / 2.
End Function

Function fillrotation(r.rotation, pitch#, yaw#, roll#)
r\pitch = pitch
r\yaw = yaw
r\roll = roll
End Function


Type rotation
Field pitch#, yaw#, roll#
End Type

Type quat
Field w#, x#, y#, z#
End Type