Const COLLISION_EPSILON:Float=0.00001 ' 0.001 and 0.0001 reports non-existent collisions? (stop response more so)

Type TCollision

	Global col_response=2

	'collision methods
	Const COLLISION_METHOD_SPHERE:Int=1
	Const COLLISION_METHOD_POLYGON:Int=2
	Const COLLISION_METHOD_BOX:Int=3

	'collision actions
	Const COLLISION_RESPONSE_NONE:Int=0
	Const COLLISION_RESPONSE_STOP:Int=1
	Const COLLISION_RESPONSE_SLIDE:Int=2
	Const COLLISION_RESPONSE_SLIDEXZ:Int=3

	Global list:TList=New TList
	Global ent_lists:TList[100]

	Field src_type
	Field des_type
	Field col_method=0
	Field response=0
	
	Method New()
	
		If LOG_NEW
			DebugLog "New TCollision"
		EndIf
	
	End Method
	
	Method Delete()
	
		If LOG_DEL
			DebugLog "Del TCollision"
		EndIf
	
	End Method

	Function UpdateCollisions()

		For Local col:TCollision=EachIn TCollision.list
		
			If TCollision.ent_lists[col.src_type]=Null Or TCollision.ent_lists[col.des_type]=Null Then Continue
		
			For Local ent:TEntity=EachIn TCollision.ent_lists[col.src_type]
			
				ent.no_collisions=0
				ent.collision=ent.collision[..0]
				
				If ent.parent_hidden=True Or ent.hidden=True Then Continue
			
				For Local ent2:TEntity=EachIn TCollision.ent_lists[col.des_type]
				
					If ent2.parent_hidden=True Or ent2.hidden=True Then Continue

					If col.col_method=1
						
						If SphereToSphere(ent,ent2) ' dynamic sphere to sphere

							ent.no_collisions=ent.no_collisions+1			
							Local i=ent.no_collisions-1		
							ent.collision=ent.collision[..i+1]		
							ent.collision[i]=New TCollisionImpact
							ent.collision[i].ent=ent2
							
						EndIf
						
					EndIf
					
					If col.col_method=2
					
						If QuickCheck(ent,ent2)=True
										
							TMesh(ent2).TreeCheck() ' create collision tree for mesh if necessary
			
							Collide(ent,ent2) ' dynamic ellipsoid to static mesh (faster, limited ellipsoid support)
				
						EndIf
																
					EndIf
					
					' update src ent 'old' position
					ent2.old_x=ent2.EntityX(True)
					ent2.old_y=ent2.EntityY(True)
					ent2.old_z=ent2.EntityZ(True)
															
				Next
				
				' update src ent 'old' position
				ent.old_x=ent.EntityX(True)
				ent.old_y=ent.EntityY(True)
				ent.old_z=ent.EntityZ(True)
				
			Next
						
		Next
							
	End Function
	
	Function SphereToSphere(ent1:TEntity,ent2:TEntity,t#=0.0)

		Const EPSILON=0.001

		Local s1:TVector=TVector.Create(ent1.old_x#,ent1.old_y#,ent1.old_z#)
		Local sr1:Float=ent1.radius_x
		
		Local s2:TVector=TVector.Create(ent2.old_x#,ent2.old_y#,ent2.old_z#)
		Local sr2:Float=ent2.radius_x
										
		Local v1:TVector=New TVector
		v1.x=ent1.EntityX(True)-ent1.old_x#
		v1.y=ent1.EntityY(True)-ent1.old_y#
		v1.z=ent1.EntityZ(True)-ent1.old_z#
		
		Local v2:TVector=New TVector
		v2.x=ent2.EntityX(True)-ent2.old_x#
		v2.y=ent2.EntityY(True)-ent2.old_y#
		v2.z=ent2.EntityZ(True)-ent2.old_z#					
	
		Local s:TVector=New TVector
		s=s2.Subtract(s1)
		
		Local v:TVector=New TVector
		v=v2.Subtract(v1)
		
		Local r#=sr2+sr1
		Local c#=(s.Dot(s))-r*r
		
		If c<0.0
			' Spheres initially overlapping so exit directly
			t=0.0
			Return True
		EndIf
			
		Local a#=v.Dot(v)
		If a<EPSILON Return 0 ' Sphere not moving relative to each other
		
		Local b#=v.Dot(s)
		If b>=0.0 Return 0 ' Spheres not moving towards each other
		
		Local d#=(b*b)-(a*c)
		If d<0.0 Return 0 ' No real-value root, spheres do not interesect
		
		t=(-b-Sqr(d))/a

		If t>=0.0 And t<=1.0
			Return True 
		EndIf

	End Function

	' perform quick check to see whether it is possible that ent and ent 2 are intersecting
	Function QuickCheck(ent:TEntity,ent2:TEntity)
	
		' check to see if src ent has moved since last update - if not, no intersection
		Local ent_moved=False
		If ent.old_x<>ent.EntityX(True) Or ent.old_y<>ent.EntityY(True) Or ent.old_z<>ent.EntityZ(True) Then ent_moved=True
		If ent_moved=False Then Return False
		
		' check to see whether moving src ent intersects dest ent
		
		' get bounding box of moving src ent
		Local x1:Float,y1:Float,z1:Float,x2:Float,y2:Float,z2:Float
		
		BoxFromPoints(	ent.old_x#,ent.old_y#,ent.old_z#,..
						ent.EntityX(True),ent.EntityY(True),ent.EntityZ(True),..
						x1,y1,z1,x2,y2,z2)
						
		x1:-ent.radius_x
		y1:-ent.radius_y
		z1:-ent.radius_x
		x2:+ent.radius_x
		y2:+ent.radius_y
		z2:+ent.radius_x
		
		' get bounding sphere of dest ent
		Local sx:Float,sy:Float,sz:Float,sr:Float
		ent2.BoundingSphereNew(sx,sy,sz,sr)
		
		' check if box and sphere intersect
		If BoxSphereIntersection(x1,y1,z1,x2,y2,z2,sx,sy,sz,sr)=False Then Return False

		Return True
	
	End Function

	'
	' NEW VERSION
	'
	Function Collide(src:TEntity,dst:TEntity)
	
		Const MAX_HITS:Int=10
	
		'Local dv:TVec=src.getWorldTform().v
		'Local sv:TVec=src.getPrevWorldTform().v
		Local dv:TVec=TVec.Create(src.EntityX(True),src.EntityY(True),src.EntityZ(True))
		Local sv:TVec=TVec.Create(src.old_x,src.old_y,src.old_z)
	
		If sv.equals(dv)
			If dv.x<>sv.x Or dv.y<>sv.y Or dv.z<>sv.z
				'src.setWorldPosition( sv )
				src.PositionEntity sv.x,sv.y,sv.z
			EndIf
			Return
		EndIf
	
		Local panic:TVec=TVec.Create(sv.x,sv.y,sv.z)
	
		Global y_tform:TTransform=TTransform.Create0()
	
		'Const radii:TVec=src.getCollisionRadii()
		Local radii:TVec=TVec.Create(src.radius_x,src.radius_y,src.radius_x)
	
		Local radius:Float=radii.x,inv_y_scale:Float
		Local y_scale:Float=1;inv_y_scale=1;y_tform.m.j.y=1
	
		If radii.x<>radii.y
			y_scale=radius/radii.y;y_tform.m.j.y=radius/radii.y
			inv_y_scale=1/y_scale
			sv.y:*y_scale
			dv.y:*y_scale
		EndIf
	
		Local n_hit:Int=0
		Local planes:T_Plane[2]
		Local coll_line:TLine=TLine.Create( sv,dv.Subtract(sv) )
		Local dir:TVec=TVec.Create(coll_line.d.x,coll_line.d.y,coll_line.d.z)
	
		Local td:Float=coll_line.d.length()
		Local td_xz:Float=TVec.Create( coll_line.d.x,0,coll_line.d.z ).length()
		
		Local hits:Int=0

		'For Local x=0 To 0
		Repeat
		
			Local coll_obj:TEntity=Null
			Local coll_info=0

			'dst_tform:TTransform=dst.getPrevWorldTform()

			Local dst_tform:TTransform=TTransform.Create0()
			dst_tform.m=TMat.Create(..
			TVec.Create(dst.mat.grid[0,0],dst.mat.grid[0,1],-dst.mat.grid[0,2]),..
			TVec.Create(dst.mat.grid[1,0],dst.mat.grid[1,1],-dst.mat.grid[1,2]),..
			TVec.Create(-dst.mat.grid[2,0],-dst.mat.grid[2,1],dst.mat.grid[2,2]) )
			dst_tform.v=TVec.Create(dst.mat.grid[3,0],dst.mat.grid[3,1],-dst.mat.grid[3,2])

			Local coll:TCollisionTest=New TCollisionTest
			
			coll.bb=GetBB(src,dst)
		
			If y_scale=1
				'If hitTest(coll_line,radius,dst,dst_tform,coll_it.meth,coll)
				If hitTest(coll_line,radius,dst,dst_tform,2,coll)

					coll_obj=dst
					'coll_info=coll_it
					
				EndIf
			Else
				'If hitTest(coll_line,radius,dst,y_tform.MultiplyTransform(dst_tform),coll_it.meth,coll)
				If hitTest(coll_line,radius,dst,y_tform.MultiplyTransform(dst_tform),2,coll)

					coll_obj=dst
					'coll_info=coll_it
					
				EndIf
			EndIf
	
			If Not coll_obj Then Exit
	
			'register collision
			hits:+1
			If( hits=MAX_HITS )
	'			Exit(0);
				Exit
			EndIf
	
			'collided( src,coll_obj,coll_line,coll,inv_y_scale )
			collided( src,coll_obj,coll_line,coll,inv_y_scale )
	
			Local coll_plane:T_Plane=T_Plane.Create1( coll_line.Multiply(coll.time),coll.normal )
	
			coll_plane.d:-COLLISION_EPSILON
			coll.time=coll_plane.t_intersect( coll_line )
	
			If( coll.time>0.0 )' && fabs(coll.normal.dot( coll_line.d ))>EPSILON ){
				'update source position - ONLY IF AHEAD!
				sv=coll_line.Multiply(coll.time)
				td=td*(1-coll.time)
				td_xz=td_xz*(1-coll.time)
			EndIf
	
			If col_response=COLLISION_RESPONSE_STOP
				dv=sv.Copy()
				Exit
			EndIf

			' slide
	
			'find nearest point on plane to dest
			Local nv:TVec=coll_plane.Nearest( dv )
	
			If n_hit=0
				dv=nv.Copy()
			Else If n_hit=1
				If planes[0].Distance(nv)>=0
					dv=nv.Copy();n_hit=0
				Else If( Abs( planes[0].n.Dot( coll_plane.n ) )<1-EPSILON )
					dv=coll_plane.IntersecT_Plane( planes[0] ).Nearest( dv )
				Else
					DebugLog "SQUISHED!"
					'Exit(0);
					hits=MAX_HITS;Exit
				EndIf
			Else If planes[0].Distance(nv)>=0 And planes[1].Distance(nv)>=0
				dv=nv.Copy();n_hit=0
			Else
				dv=sv.Copy();Exit
			EndIf
	
			Local dd:TVec=dv.Subtract(sv)
	
			'going behind initial direction? really necessary?
			If dd.Dot( dir )<=0 Then dv=sv.Copy(); Exit
	
			If( col_response=COLLISION_RESPONSE_SLIDE )
				Local d:Float=dd.Length()
				If d<=EPSILON Then dv=sv.Copy();Exit
				If d>td Then dd=dd.Multiply(td/d)
			Else If( col_response=COLLISION_RESPONSE_SLIDEXZ )
				Local d:Float=TVec.Create( dd.x,0,dd.z ).Length()
				If d<=EPSILON Then dv=sv.Copy();Exit
				If d>td_xz Then dd=dd.Multiply(td_xz/d)
			EndIf
	
			coll_line.o=sv.Copy()
			coll_line.d=dd.Copy();dv=sv.Add(dd)
			planes[n_hit]=coll_plane.Copy()
			n_hit:+1

		'Next
		Forever

		If hits 
			If hits<MAX_HITS
				dv.y=dv.y*inv_y_scale
				'src.setWorldPosition( dv )
				PositionEntity src,dv.x,dv.y,dv.z,True
			Else
				'src.setWorldPosition( panic )
				PositionEntity src,panic.x,panic.y,panic.z,True
				'PositionEntity src,dv.x,dv.y,dv.z,True
			EndIf
		EndIf
		
	End Function
	
	Function HitTest(line:TLine,radius:Float,obj:TEntity,tf:TTransform,meth:Int,curr_coll:TCollisionTest)
	
		Select meth
		
		Case COLLISION_METHOD_POLYGON
		
			'create Local box
			Local box:TBox=TBox.CreateFromLine( line )
			box.Expand( radius )
			Local local_box:TBox=tf.Negate().MultiplyBox(box)
			
			Local polys:TPolygon[]=TMesh(obj).col_tree.TreePolysBox(local_box.a.x,local_box.a.y,local_box.a.z,local_box.b.x,local_box.b.y,local_box.b.z)
			
			Return HitTestCollide(local_box,line,radius,obj:TEntity,tf,curr_coll,polys)
			
		End Select
		
	End Function

	Function HitTestCollide( line_box:TBox,line:TLine,radius:Float,obj:TEntity,tform:TTransform,curr_coll:TCollisionTest,polys:TPolygon[] )

		Const X=0,Y=1,Z=2

		Global a:Float[3]
		Global b:Float[3]
		Global c:Float[3]

		Local hit=False
	
		For Local poly:TPolygon=EachIn polys

			If poly.no_verts=3
					
				a[X]=poly.verts[0].x; a[Y]=poly.verts[0].y; a[Z]=poly.verts[0].z
				b[X]=poly.verts[1].x; b[Y]=poly.verts[1].y; b[Z]=poly.verts[1].z
				c[X]=poly.verts[2].x; c[Y]=poly.verts[2].y; c[Z]=poly.verts[2].z

				Local t_v0:TVec=TVec.Create(a[X],a[Y],a[Z])
				Local t_v1:TVec=TVec.Create(b[X],b[Y],b[Z])
				Local t_v2:TVec=TVec.Create(c[X],c[Y],c[Z])

				' needs looking - causes sgb lev8 plate fall-through ' ***temp rem***
				
				'Local tri_box:TBox=TBox.CreateFromPoint( t_v0 )
				'tri_box.UpdateFromVector( t_v1 )
				'tri_box.UpdateFromVector( t_v2 )
				
				'Local box_col=tri_box.Overlaps( line_box )
				
				'If Not box_col Then Continue
	
				Local col=curr_coll.triangleCollide( line,radius,tform.MultiplyVector(t_v0),tform.MultiplyVector(t_v1),tform.MultiplyVector(t_v2 ) ) 'Then Continue
		
				'Local col=curr_coll.triangleCollide( line,radius,t_v0,t_v1,t_v2 )
				
				If Not col Then Continue

				curr_coll.surface=poly.surf
				curr_coll.index=poly.tri
				
				hit=True
				
			EndIf
			
		Next
		
		Return hit
	
	End Function
	
	Function collided(src:TEntity,dest:TEntity,line:TLine,coll:TCollisionTest,y_scale:Float )

		'ObjCollision *c
		'Local coords:TVec=line.Multiply(coll.time).Subtract(coll.normal.Negate().Multiply(src.radius_x)) negate shouldn't be there?
		Local coords:TVec=line.Multiply(coll.time).Subtract(coll.normal.Multiply(src.radius_x))
	
		'c=allocObjColl( dest,coords,coll )
		'c->coords.y*=y_scale
		'src->addCollision( c )
	
		'c=allocObjColl( src,coords,coll )
		'c->coords.y*=y_scale
		'dest->addCollision( c )

		Local ent:TEntity=src'collisionPackage.ent
		Local ent2:TEntity=dest'collisionPackage.ent2

		ent.no_collisions=ent.no_collisions+1
					
		Local i=ent.no_collisions-1
					
		ent.collision=ent.collision[..i+1]
					
		ent.collision[i]=New TCollisionImpact
		ent.collision[i].x#=coords.x#
		ent.collision[i].y#=coords.y#
		ent.collision[i].z#=coords.z#
		ent.collision[i].nx#=coll.normal.x'coll.surface.TriangleNX#(coll.index)
		ent.collision[i].ny#=coll.normal.y'coll.surface.TriangleNY#(coll.index)
		ent.collision[i].nz#=coll.normal.z'coll.surface.TriangleNZ#(coll.index)
		ent.collision[i].ent=dest
		ent.collision[i].surf=coll.surface
		ent.collision[i].tri=coll.index

		'If collisionPackage.method_no=2

			' tform collision positions from model space into world space
			TFormPoint ent.collision[i].x#,ent.collision[i].y#,ent.collision[i].z#,ent2,Null
			ent.collision[i].x#=TFormedX#()
			ent.collision[i].y#=TFormedY#()
			ent.collision[i].z#=TFormedZ#()
		
		'EndIf
		
		' tform collision positions from model space into world space
		TFormNormal ent.collision[i].nx#,ent.collision[i].ny#,ent.collision[i].nz#,ent2,Null
		ent.collision[i].nx#=TFormedX#()
		ent.collision[i].ny#=TFormedY#()
		ent.collision[i].nz#=TFormedZ#()

	End Function
	
End Type

Type TCollisionTest

	Field time:Float
	Field normal:TVec
	Field surface:TSurface
	Field index:Int
	
	Field bb:AABB

	Method New()
	
		time=1.0
	
		If LOG_NEW
			DebugLog "New TCollisionTest"
		EndIf
	
	End Method
	
	Method Delete()
	
		If LOG_DEL
			DebugLog "Del TCollisionTest"
		EndIf
	
	End Method

	Method update:Int(line:TLine,t:Float,n:TVec)

		'//If( t<0 Or t>time ) Return False
		'DebugStop
		If( t>time ) Return False
		Local p:T_Plane=T_Plane.Create1(line.Multiply(t),n)
		If( p.n.dot( line.d )>=0 ) Return False
		If( p.distance(line.o)<-COLLISION_EPSILON ) Return False
	
		time=t
		normal=n
		Return True
	
	End Method
	
	Method triangleCollide:Int(line:TLine,radius:Float,v0:TVec,v1:TVec,v2:TVec)

		'triangle plane
		Local p:T_Plane=T_Plane.CreateFromTri( v0,v1,v2 )
		If( p.n.dot( line.d )>=0 ) Return False
	
		'move plane out
		p.d:-radius
		Local t:Float=p.t_intersect( line )
		If( t>time ) Return False
	
		'edge planes
		Local p0:T_Plane=T_Plane.CreateFromTri( v0.Add(p.n),v1,v0 )
		Local p1:T_Plane=T_Plane.CreateFromTri( v1.Add(p.n),v2,v1 )
		Local p2:T_Plane=T_Plane.CreateFromTri( v2.Add(p.n),v0,v2 )
	
		'intersects triangle?
		Local i:TVec=line.Multiply(t)
		If p0.distance(i)>=0 And p1.distance(i)>=0 And p2.distance(i)>=0
			Return update( line,t,p.n )
		EndIf
	
		If( radius<=0 ) Return False
			
		Local e1=edgeTest( v0,v1,p.n,p0.n,line,radius,Self )
		Local e2=edgeTest( v1,v2,p.n,p1.n,line,radius,Self )
		Local e3=edgeTest( v2,v0,p.n,p2.n,line,radius,Self )
		
		Local x=e1|e2|e3
			
		Return x
		
	End Method

End Type

'v0,v1 = edge verts
'pn = poly normal
'en = edge normal
Function edgeTest:Int(v0:TVec,v1:TVec,pn:TVec,en:TVec,line:TLine,radius:Float,curr_coll:TCollisionTest)
	'DebugStop
	Local tm:TMat=TMat.Create( en,(v1.Subtract(v0)).normalized(),pn ).Inverse()
	Local sv:TVec=tm.MultiplyVector(line.o.Subtract(v0))
	Local dv:TVec=tm.MultiplyVector(line.o.Add(line.d).Subtract(v0)) ' *order?*
	Local l:TLine=TLine.Create( sv,dv.Subtract(sv) )
	'do cylinder test...
	Local a:Float,b:Float,c:Float,d:Float,t1:Float,t2:Float,t:Float
	a=(l.d.x*l.d.x+l.d.z*l.d.z)
	If( Not a ) Return False				'ray parallel to cylinder
	b=(l.o.x*l.d.x+l.o.z*l.d.z)*2
	c=(l.o.x*l.o.x+l.o.z*l.o.z)-radius*radius
	d=b*b-4*a*c
	If( d<0 ) Return False					'ray misses cylinder
	t1=(-b+Sqr(d))/(2*a)
	t2=(-b-Sqr(d))/(2*a)
	If t1<t2 Then t=t1 Else t=t2
	If( t>curr_coll.time ) Return False		'intersects too far away
	Local i:TVec=l.Multiply(t)
	Local p:TVec=TVec.Create(0.0,0.0,0.0)
	If( i.y>v0.distance(v1) ) Return False	'intersection above cylinder
	If( i.y>=0 )
		p.y=i.y
	Else
		'below bottom of cylinder...do sphere test...
		a=l.d.dot(l.d)
		If( Not a ) Return False			'ray parallel to sphere
		b=l.o.dot(l.d)*2
		c=l.o.dot(l.o)-radius*radius
		d=b*b-4*a*c
		If( d<0 ) Return False				'ray misses sphere
		t1=(-b+Sqr(d))/(2*a)
		t2=(-b-Sqr(d))/(2*a)
		If t1<t2 Then t=t1 Else t=t2
		If( t>curr_coll.time ) Return False
		i=l.Multiply(t)
	EndIf

	Return curr_coll.update( line,t,(tm.Inverse().MultiplyVector(i.Subtract(p))).normalized() )
	
End Function

Type TCollisionImpact

	Method New()
	
		If LOG_NEW
			DebugLog "New TCollisionImpact"
		EndIf
	
	End Method
	
	Method Delete()
	
		If LOG_DEL
			DebugLog "Del TCollisionImpact"
		EndIf
	
	End Method

	Field x#,y#,z#
	Field nx#,ny#,nz#
	Field time#
	Field ent:TEntity
	Field surf:TSurface
	Field tri

End Type

' get bounding box of moving sphere in dest ent space (method 2)
Function GetBB:AABB(ent:TEntity,ent2:TEntity)

	'Local ent:TEntity=collisionPackage.ent
	'Local ent2:TEntity=collisionPackage.ent2

	Local rx#=ent.radius_x/ent2.EntityScaleX(True) ' must divide before testing against r_max below, then multiply after
	Local ry#=ent.radius_y/ent2.EntityScaleY(True) ' "         "
	Local rz#=ent.radius_x/ent2.EntityScaleZ(True) ' "         "

	Local r_max#=0.0
	If rx>r_max# Then r_max#=rx*ent2.EntityScaleX(True) ' must multiply after testing against r_max
	If ry>r_max# Then r_max#=ry*ent2.EntityScaleY(True) ' "         "
	If rz>r_max# Then r_max#=rz*ent2.EntityScaleZ(True) ' "         "

	Local pos0:TVector=TVector.Create(ent.old_x#,ent.old_y#,ent.old_z#)
	TFormPoint pos0.x,pos0.y,pos0.z,Null,ent2
	pos0.x=TFormedX()
	pos0.y=TFormedY()
	pos0.z=TFormedZ()

	Local pos:TVector=TVector.Create(ent.EntityX(True),ent.EntityY(True),ent.EntityZ(True))
	TFormPoint pos.x,pos.y,pos.z,Null,ent2
	pos.x=TFormedX()
	pos.y=TFormedY()
	pos.z=TFormedZ()
	
	' calculate bounding box that encloses src entity old postion + new position
	Local bb:AABB=New AABB
	If pos0.x<pos.x
		bb.mini[0]=pos0.x-r_max
		bb.maxi[0]=pos.x+r_max
	Else
		bb.mini[0]=pos.x-r_max
		bb.maxi[0]=pos0.x+r_max
	EndIf
	If pos0.y<pos.y
		bb.mini[1]=pos0.y-r_max
		bb.maxi[1]=pos.y+r_max
	Else
		bb.mini[1]=pos.y-r_max
		bb.maxi[1]=pos0.y+r_max
	EndIf
	If pos0.z<pos.z
		bb.mini[2]=pos0.z-r_max
		bb.maxi[2]=pos.z+r_max
	Else
		bb.mini[2]=pos.z-r_max
		bb.maxi[2]=pos0.z+r_max
	EndIf
	
	Return bb

End Function