' Updates:
' --------
' V0.41
' --------
' 30/01/07 - MeshCheckCollision2
' 07/01/07 - SphereToMesh2, SphereToMesh4, SlideSphereAgainstMesh, MeshCheckCollision, MeshCheckCollision2, MeshCheckCollision4, PolysCheckCollision, PolysCheckCollision3, PolysCheckCollision4
' --------
' V0.42
' --------
' 06/04/07 - ent_list field added
' 15/06/07 - UpdateCollisions

Type TCollision

	Global list:TList=New TList ' list containing collision setup info
	Global ent_list:TList=New TList ' list containing collision entities

	Field src_type
	Field des_type
	Field col_method=0
	Field response=0
	
	' Set this to match application scale..
	Const unitsPerMeter# = 100.0
	
	' MiniB3D functions
	
	' Internal - not recommended for general use
		
	Function UpdateCollisions()

		For Local ent:TEntity=EachIn TCollision.ent_list

			' reset collision impact info
			ent.no_collisions=0
			ent.collision=ent.collision[..0]

			If ent.collision_type<>0 And ent.EntityHidden()=False
		
				For Local col:TCollision=EachIn TCollision.list
				
					If col.src_type=ent.collision_type
					
						For Local ent2:TEntity=EachIn TCollision.ent_list
						
							If ent2.collision_type<>0 And ent2.EntityHidden()=False

								For Local col2:TCollision=EachIn TCollision.list
							
									If col2.des_type=ent2.collision_type
									
										If col.col_method=1
											
											Local t#=0.0
											Local q:TVector=New TVector
										
											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 Or col.col_method=4
										
											If QuickCheck(ent,ent2)=True
											
												Local ms=MilliSecs()
												If col.col_method=2
													SphereToMesh2(ent,ent2) ' dynamic ellipsoid to static mesh
												EndIf
											
												If col.col_method=4
													SphereToMesh4(ent,ent2) ' dynamic ellipsoid to static mesh (faster, limited ellipsoid support)
												EndIf
												'DebugLog "UpdateCollisions "+String(MilliSecs()-ms)
											
											EndIf
											
										EndIf
	
									EndIf	
								
								Next
								
							EndIf
							
						Next
					
					EndIf
				
				Next
				
			EndIf
			
			' update src ent 'old' position once all collisions performed
			ent.old_x=ent.EntityX(True)
			ent.old_y=ent.EntityY(True)
			ent.old_z=ent.EntityZ(True)
		
		Next
	
	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 src ent bounding box intersects dest ent bounding sphere
		Local a:TBox=GetBB(ent)
		Local b:TSphere=ent2.BoundingSphere()
		If TBox.SphereTBoxIntersection(b,a)=False Then Return False

		Return True
	
	End Function

	Function SphereToMesh2(ent:TEntity,ent2:TEntity)

		Local ms=MilliSecs()

		' ent = source entity (sphere)
		' ent2 = destination entity (mesh)

		' create new collision packet
		Local collisionPackage:TCollisionPacket=New TCollisionPacket
		collisionPackage.method_no=2
		collisionPackage.ent=ent
		collisionPackage.ent2=ent2
		
		' get moving sphere bounding box, used by octrees
		collisionPackage.bb=GetBB2(collisionPackage)
	
		' calculate pos, vel vectors, send to SlideSphereAgainstMesh function
		
		Local r:TVector=TVector.Create(ent.radius_x,ent.radius_y,ent.radius_z)
		
		' get src ent 'old' position
		Local pos:TVector=TVector.Create(ent.old_x#,ent.old_y#,ent.old_z#)

		' get src ent velocity by subtracting old position from current position
		Local vel:TVector=New TVector
		vel.x=ent.EntityX(True)-ent.old_x#
		vel.y=ent.EntityY(True)-ent.old_y#
		vel.z=ent.EntityZ(True)-ent.old_z#

		' Convert src ent position, velocity to ellipsoid space
		pos.x=pos.x/(r.x)
		pos.y=pos.y/(r.y)
		pos.z=pos.z/(r.z)
		
		vel.x=vel.x/(r.x)
		vel.y=vel.y/(r.y)
		vel.z=vel.z/(r.z)	

		' Slide src ent against dest mesh, get final position
		Local fpos:TVector = SlideSphereAgainstMesh(pos,vel,collisionPackage,True)
		
		' Convert final position back from ellipsoid space
		fpos.x=fpos.x*(r.x)
		fpos.y=fpos.y*(r.y)
		fpos.z=fpos.z*(r.z)	

		ent.PositionEntity(fpos.x,fpos.y,fpos.z,True)

		'DebugLog "SphereToMesh2 "+String(MilliSecs()-ms)

	End Function
	
	Function SphereToMesh4(ent:TEntity,ent2:TEntity)
	
		' ent = source entity (sphere)
		' ent2 = destination entity (mesh)
		
		' Create new collision 'packet' - used in SlideSphereAgainstMesh
		Local collisionPackage:TCollisionPacket=New TCollisionPacket
		collisionPackage.method_no=4
		collisionPackage.ent=ent
		collisionPackage.ent2=ent2
		
		' get moving sphere bounding box, used by octrees
		collisionPackage.bb=GetBB4(collisionPackage)
			
		' get src ent radius
		Local r:TVector=TVector.Create(ent.radius_x,ent.radius_y,ent.radius_z)
	
		' convert dest ent scale to src ent scale
		r.x=r.x/ent2.EntityScaleX(True)
		r.y=r.y/ent2.EntityScaleY(True)
		r.z=r.z/ent2.EntityScaleZ(True)	
				
		' get src ent 'old' position
		Local pos:TVector=TVector.Create(ent.old_x#,ent.old_y#,ent.old_z#)

		' get src ent velocity by subtracting old position from current position
		Local vel:TVector=TVector.Create(0.0,0.0,0.0)
		vel.x=ent.EntityX(True)-ent.old_x#
		vel.y=ent.EntityY(True)-ent.old_y#
		vel.z=ent.EntityZ(True)-ent.old_z#
	
		' convert src ent position to dest ent space
		TEntity.TFormPoint(pos.x,pos.y,pos.z,Null,ent2)
		pos.x=TEntity.TFormedX()
		pos.y=TEntity.TFormedY()
		pos.z=TEntity.TFormedZ()
	
		' convert src ent velocity to dest ent space
		TEntity.TFormVector(vel.x,vel.y,vel.z,Null,ent2)
		vel.x=TEntity.TFormedX()
		vel.y=TEntity.TFormedY()
		vel.z=TEntity.TFormedZ()
			
		' Convert src ent position, velocity to ellipsoid space
		pos.x=pos.x/(r.x)
		pos.y=pos.y/(r.y)
		pos.z=pos.z/(r.z)
		
		vel.x=vel.x/(r.x)
		vel.y=vel.y/(r.y)
		vel.z=vel.z/(r.z)	

		' Slide src ent against dest mesh, get final position
		Local fpos:TVector = SlideSphereAgainstMesh(pos,vel,collisionPackage,True)

		' Convert final position back from ellipsoid space
		fpos.x=fpos.x*(r.x)
		fpos.y=fpos.y*(r.y)
		fpos.z=fpos.z*(r.z)	

		' Convert final position back from dest ent space to src ent space
		TEntity.TFormPoint(fpos.x,fpos.y,fpos.z,ent2,Null)
		fpos.x=TEntity.TFormedX()
		fpos.y=TEntity.TFormedY()
		fpos.z=TEntity.TFormedZ()

		' Position entity
		ent.PositionEntity(fpos.x,fpos.y,fpos.z,True)
		
	End Function
	
	' get bounding box of moving sphere in world space (used in QuickCheck)
	Function GetBB:TBox(ent:TEntity)

		Local pos0:TVector=TVector.Create(ent.old_x#,ent.old_y#,ent.old_z#)
		Local pos:TVector=TVector.Create(ent.EntityX(True),ent.EntityY(True),ent.EntityZ(True))

		' get src ent radius
		Local r:TVector=TVector.Create(ent.radius_x,ent.radius_y,ent.radius_z)

		' calculate bounding box that encloses src entity old postion + new position
		Local bb:TBox=New TBox
		If pos0.x<pos.x
			bb.mini[0]=pos0.x-r.x
			bb.maxi[0]=pos.x+r.x
		Else
			bb.mini[0]=pos.x-r.x
			bb.maxi[0]=pos0.x+r.x
		EndIf
		If pos0.y<pos.y
			bb.mini[1]=pos0.y-r.y
			bb.maxi[1]=pos.y+r.y
		Else
			bb.mini[1]=pos.y-r.y
			bb.maxi[1]=pos0.y+r.y
		EndIf
		If pos0.z<pos.z
			bb.mini[2]=pos0.z-r.z
			bb.maxi[2]=pos.z+r.z
		Else
			bb.mini[2]=pos.z-r.z
			bb.maxi[2]=pos0.z+r.z
		EndIf
		
		Return bb
	
	End Function
	
	' get bounding box of moving sphere in dest ent space (method 2)
	Function GetBB2:TBox(collisionPackage:TCollisionPacket)
	
		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_z/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#)
		TEntity.TFormPoint pos0.x,pos0.y,pos0.z,Null,ent2
		pos0.x=TEntity.TFormedX()
		pos0.y=TEntity.TFormedY()
		pos0.z=TEntity.TFormedZ()

		Local pos:TVector=TVector.Create(ent.EntityX(True),ent.EntityY(True),ent.EntityZ(True))
		TEntity.TFormPoint(pos.x,pos.y,pos.z,Null,ent2)
		pos.x=TEntity.TFormedX()
		pos.y=TEntity.TFormedY()
		pos.z=TEntity.TFormedZ()
		
		' calculate bounding box that encloses src entity old postion + new position
		Local bb:TBox=New TBox
		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
	
	' get bounding box of moving sphere in dest ent space (method 4)
	Function GetBB4:TBox(collisionPackage:TCollisionPacket)
	
		Local ent:TEntity=collisionPackage.ent
		Local ent2:TEntity=collisionPackage.ent2

		' get src ent radius
		Local r:TVector=TVector.Create(ent.radius_x,ent.radius_y,ent.radius_z)
	
		' convert dest ent scale to src ent scale
		r.x=r.x/ent2.EntityScaleX(True)
		r.y=r.y/ent2.EntityScaleY(True)
		r.z=r.z/ent2.EntityScaleZ(True)
		
		Local pos0:TVector=TVector.Create(ent.old_x#,ent.old_y#,ent.old_z#)
		TEntity.TFormPoint pos0.x,pos0.y,pos0.z,Null,ent2
		pos0.x=TEntity.TFormedX()
		pos0.y=TEntity.TFormedY()
		pos0.z=TEntity.TFormedZ()
		
		' convert src ent new position to dest ent space
		Local pos:TVector=TVector.Create(ent.EntityX(True),ent.EntityY(True),ent.EntityZ(True))
		TEntity.TFormPoint pos.x,pos.y,pos.z,Null,ent2
		pos.x=TEntity.TFormedX()
		pos.y=TEntity.TFormedY()
		pos.z=TEntity.TFormedZ()
		
		' calculate bounding box that encloses src entity old postion + new position
		Local bb:TBox=New TBox
		If pos0.x<pos.x
			bb.mini[0]=pos0.x-r.x
			bb.maxi[0]=pos.x+r.x
		Else
			bb.mini[0]=pos.x-r.x
			bb.maxi[0]=pos0.x+r.x
		EndIf
		If pos0.y<pos.y
			bb.mini[1]=pos0.y-r.y
			bb.maxi[1]=pos.y+r.y
		Else
			bb.mini[1]=pos.y-r.y
			bb.maxi[1]=pos0.y+r.y
		EndIf
		If pos0.z<pos.z
			bb.mini[2]=pos0.z-r.z
			bb.maxi[2]=pos.z+r.z
		Else
			bb.mini[2]=pos.z-r.z
			bb.maxi[2]=pos0.z+r.z
		EndIf
		
		Return bb
	
	End Function

	Rem
	Function SlideSphereAgainstMesh0:TVector(pos:TVector,vel:TVector,collisionPackage:TCollisionPacket,reset=False)
			
		Local ms=MilliSecs()
			
		Global collisionRecursionDepth
		If reset=True Then collisionRecursionDepth=0
	
		' All hard-coded distances in this function is
		' scaled to fit the setting above..
		Local unitScale# = unitsPerMeter / 100.0
		Local veryCloseDistance# = 0.005 * unitScale
		
		' do we need to worry?
		If collisionRecursionDepth>5 Return pos
		
		' Ok, we need to worry:
		collisionPackage.velocity = vel.Copy()
		
		collisionPackage.normalizedVelocity = vel.Copy()
		collisionPackage.normalizedVelocity.normalize()
		collisionPackage.basePoint = pos.Copy()
		collisionPackage.foundCollision = False
		
		' Check for collision (calls the collision routines)
		' Application specific!!
		MeshCheckCollision(collisionPackage)
		
		' If no collision we just move along the velocity
		If collisionPackage.foundCollision = False Return pos.Add(vel)

		' *** Collision occured ***
		' The original destination point
		Local destinationPoint:TVector = pos.Add(vel)
		Local newBasePoint:TVector = pos.Copy()
		
		' only update if we are not already very close
		' and if so we only move very close to intersection..not
		' to the exact spot.
		If (collisionPackage.nearestDistance>=veryCloseDistance)
	
			Local V:TVector = vel.Copy()
			V.SetLength(collisionPackage.nearestDistance-veryCloseDistance)
			newBasePoint = collisionPackage.basePoint.Add(V)
			' Adjust polygon intersection point (so sliding
			' plane will be unaffected by the fact that we
			' move slightly less than collision tells us)
			V.normalize()
			collisionPackage.intersectionPoint = (collisionPackage.intersectionPoint.Subtract(V.Multiply(veryCloseDistance))).Copy()
		
		EndIf
		
		' Determine the sliding plane
		Local slidePlaneOrigin:TVector = collisionPackage.intersectionPoint.Copy()
		Local slidePlaneNormal:TVector = newBasePoint.Subtract(collisionPackage.intersectionPoint)
		slidePlaneNormal.normalize()
		Local slidingPlane:TPlane=TPlane.CreatePlane(slidePlaneOrigin,slidePlaneNormal)
		
		' Again, sorry about formatting.. but look carefully )
		Local newDestinationPoint:TVector = destinationPoint.Subtract(slidePlaneNormal.Multiply(slidingPlane.signedDistanceTo(destinationPoint)))
		
		' Generate the slide vector, which will become ournew
		' velocity vector for the Next iteration
		Local newVelocityVector:TVector = newDestinationPoint.Subtract(collisionPackage.intersectionPoint)
		
		' Recurse:
		' dont recurse if the new velocity is very small
		If newVelocityVector.length() < veryCloseDistance
			Return newBasePoint
		EndIf
		
		collisionRecursionDepth=collisionRecursionDepth+1

		Return SlideSphereAgainstMesh(newBasePoint,newVelocityVector,collisionPackage)

		'DebugLog "SlideSphereAgainstMesh "+String(MilliSecs()-ms)

	End Function
	End Rem

	Function SlideSphereAgainstMesh:TVector(pos:TVector,vel:TVector,collisionPackage:TCollisionPacket,reset=False)
			
		Local ms=MilliSecs()
			
		Global collisionRecursionDepth
		If reset=True Then collisionRecursionDepth=0
	
		' All hard-coded distances in this function is
		' scaled to fit the setting above..
		Local unitScale# = unitsPerMeter / 100.0
		Local veryCloseDistance# = 0.001 * unitScale '0.005f * unitScale
		
		' do we need to worry?
		If collisionRecursionDepth>5 Or vel.length() < veryCloseDistance*2 Return pos
		
		' Ok, we need to worry:
		collisionPackage.velocity = vel.Copy()
		collisionPackage.normalizedVelocity = vel.Copy()
		collisionPackage.normalizedVelocity.normalize()
		collisionPackage.basePoint = pos.Copy()
		collisionPackage.foundCollision = False
		collisionPackage.nearestDistance = 10000000
		
		' Check for collision (calls the collision routines)
		' Application specific!!
		MeshCheckCollision(collisionPackage)
		
		' If no collision we just move along the velocity
		If collisionPackage.foundCollision = False Return pos.Add(vel)

		' *** Collision occured ***
		' The original destination point
		Local destinationPoint:TVector = pos.Add(vel)
		Local newSourcePoint:TVector = pos.Copy()
		
		' only update if we are not already very close
		' and if so we only move very close to intersection..not
		' to the exact spot.
		Local V:TVector = vel.Copy()
		
		' If we are at the ideal point, just back off
		If(collisionPackage.nearestDistance=0.0) Return pos.Add(vel)

		V.SetLength( collisionPackage.nearestDistance)
		newSourcePoint = collisionPackage.basePoint.Add(V)

		' Determine the sliding plane
		Local slidePlaneNormal:TVector = newSourcePoint.Subtract(collisionPackage.intersectionPoint)
		slidePlaneNormal.normalize()

		' If this was the initial collision, ouput in CollisionTrace
		If collisionRecursionDepth=0
	
				Local ent:TEntity=collisionPackage.ent
				Local ent2:TEntity=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#=collisionPackage.intersectionPoint.x#
				ent.collision[i].y#=collisionPackage.intersectionPoint.y#
				ent.collision[i].z#=collisionPackage.intersectionPoint.z#
				ent.collision[i].nx#=slidePlaneNormal.x
				ent.collision[i].ny#=slidePlaneNormal.y
				ent.collision[i].nz#=slidePlaneNormal.z
				ent.collision[i].ent=ent2
				ent.collision[i].surf=collisionPackage.surf
				ent.collision[i].tri=collisionPackage.tri
		
				If collisionPackage.method_no=2
		
					' tform collision positions from model space into world space
					TEntity.TFormPoint(ent.collision[i].x#,ent.collision[i].y#,ent.collision[i].z#,ent2,Null)
					ent.collision[i].x#=TEntity.TFormedX#()
					ent.collision[i].y#=TEntity.TFormedY#()
					ent.collision[i].z#=TEntity.TFormedZ#()
				
				EndIf
				
				' tform collision positions from model space into world space
				TEntity.TFormNormal(ent.collision[i].nx#,ent.collision[i].ny#,ent.collision[i].nz#,ent2,Null)
				ent.collision[i].nx#=TEntity.TFormedX#()
				ent.collision[i].ny#=TEntity.TFormedY#()
				ent.collision[i].nz#=TEntity.TFormedZ#()
								
		EndIf
		
		' If no response needed, return the collision point
		'If collisionPackage.response=0
		
			'Return newSourcePoint
		
		'EndIf
	
		' Determine the displacement vector
		Local factor#
	
		V.SetLength(veryCloseDistance)
		If( V.dot(slidePlaneNormal) = 0.0)
			factor = 0.0;
		Else 
			factor = veryCloseDistance / ( V.dot(slidePlaneNormal) )
		EndIf
	
		Local displacementVector:TVector=V.Multiply(factor)

		' Move away
		newSourcePoint = newSourcePoint.Add(displacementVector)
		collisionPackage.intersectionPoint = collisionPackage.intersectionPoint.Add(displacementVector)
	
		Local slidePlaneOrigin:TVector = collisionPackage.intersectionPoint.Copy()	
		Local slidingPlane:TPlane=TPlane.CreatePlane(slidePlaneOrigin,slidePlaneNormal)
		
		' Again, sorry about formatting.. but look carefully )
		Local newDestinationPoint:TVector = destinationPoint.Subtract(slidePlaneNormal.Multiply(slidingPlane.signedDistanceTo(destinationPoint)))

		' Generate the slide vector, which will become our new
		' velocity vector for the next iteration
		Local newVelocityVector:TVector = newDestinationPoint.Subtract(collisionPackage.intersectionPoint)
	
		' Recurse:
		' dont recurse if the new velocity is very small	
		If (newVelocityVector.length() <= veryCloseDistance)
			Return newSourcePoint
		EndIf
	
		collisionRecursionDepth:+1
		Return SlideSphereAgainstMesh(newSourcePoint,newVelocityVector,collisionPackage)

	End Function

	Function MeshCheckCollision(collisionPackage:TCollisionPacket)
		
		Local col_method=collisionPackage.method_no
			
		If TMesh(collisionPackage.ent2).octree<>Null
		
			TMesh(collisionPackage.ent2).octree.no_polys_active=0
			TMesh(collisionPackage.ent2).octree.no_cells_active=0

			TMesh(collisionPackage.ent2).octree.OctCollision(collisionPackage)
			
		Else
		
			If col_method=2
				MeshCheckCollision2(collisionPackage)
			Else
				MeshCheckCollision4(collisionPackage)				
			EndIf
				
		EndIf
	
	End Function
	
	' check mesh for collision, used when no octree available (method 2)
	Function MeshCheckCollision2(collisionPackage:TCollisionPacket)
	
		'Local ms=MilliSecs()
	
		Local ent:TEntity=collisionPackage.ent
		Local ent2:TEntity=collisionPackage.ent2

		Local tform=True
		If ent2.px=0.0 And ent2.py=0.0 And ent2.pz=0.0 And ent2.sx=1.0 And ent2.sy=1.0 And ent2.sz=1.0 And ent2.rx=0.0 And ent2.ry=0.0 And ent2.rz=0.0 Then tform=False
			
		For Local s=1 To TMesh(ent2).CountSurfaces()
		
			Local surf:TSurface=TMesh(ent2).GetSurface(s)
			
			For Local tri=0 Until surf.CountTriangles()

				Local v0=surf.TriangleVertex(tri,0)
				Local v1=surf.TriangleVertex(tri,1)
				Local v2=surf.TriangleVertex(tri,2)
				
				Local a:TVector=New TVector
				Local b:TVector=New TVector
				Local c:TVector=New TVector
				
				a.x=surf.VertexX#(v0)
				a.y=surf.VertexY#(v0)
				a.z=surf.VertexZ#(v0)
				
				b.x=surf.VertexX#(v1)
				b.y=surf.VertexY#(v1)
				b.z=surf.VertexZ#(v1)
				
				c.x=surf.VertexX#(v2)
				c.y=surf.VertexY#(v2)
				c.z=surf.VertexZ#(v2)

				TEntity.TFormPoint(a.x,a.y,a.z,ent2,Null)
				a.x=TEntity.TFormedX#()
				a.y=TEntity.TFormedY#()
				a.z=TEntity.TFormedZ#()
					
				TEntity.TFormPoint(b.x,b.y,b.z,ent2,Null)
				b.x=TEntity.TFormedX#()
				b.y=TEntity.TFormedY#()
				b.z=TEntity.TFormedZ#()
					
				TEntity.TFormPoint(c.x,c.y,c.z,ent2,Null)
				c.x=TEntity.TFormedX#()
				c.y=TEntity.TFormedY#()
				c.z=TEntity.TFormedZ#()

				Local r:TVector=TVector.Create(0,0,0)
				r.x=ent.radius_x
				r.y=ent.radius_y
				r.z=ent.radius_z

				a.x:/r.x
				a.y:/r.y
				a.z:/r.z
				
				b.x:/r.x
				b.y:/r.y
				b.z:/r.z
				
				c.x:/r.x
				c.y:/r.y
				c.z:/r.z

				If CheckTriangle(collisionPackage,a,b,c)=True
	
					collisionPackage.collision=True	
					collisionPackage.surf=surf
					collisionPackage.tri=tri
		
				EndIf

			Next
		
		Next
		
		'DebugLog "MeshCheckCollision2 "+String(MilliSecs()-ms)
	
	End Function

	' check mesh for collision, used when no octree available (method 4)
	Function MeshCheckCollision4(collisionPackage:TCollisionPacket)
	
		Local ent:TEntity=collisionPackage.ent
		Local ent2:TEntity=collisionPackage.ent2
	
		For Local s=1 To TMesh(ent2).CountSurfaces()
		
			Local surf:TSurface=TMesh(ent2).GetSurface(s)
			
			For Local tri=0 Until surf.CountTriangles()

				Local v0=surf.TriangleVertex(tri,0)
				Local v1=surf.TriangleVertex(tri,1)
				Local v2=surf.TriangleVertex(tri,2)
				
				Local a:TVector=New TVector
				Local b:TVector=New TVector
				Local c:TVector=New TVector
				
				a.x=surf.VertexX#(v0)
				a.y=surf.VertexY#(v0)
				a.z=surf.VertexZ#(v0)
				
				b.x=surf.VertexX#(v1)
				b.y=surf.VertexY#(v1)
				b.z=surf.VertexZ#(v1)
				
				c.x=surf.VertexX#(v2)
				c.y=surf.VertexY#(v2)
				c.z=surf.VertexZ#(v2)

				Local r:TVector=TVector.Create(0,0,0)
				r.x=ent.radius_x/ent2.EntityScaleX(True)
				r.y=ent.radius_y/ent2.EntityScaleY(True)
				r.z=ent.radius_z/ent2.EntityScaleZ(True)
		
				a.x:/r.x
				a.y:/r.y
				a.z:/r.z
				
				b.x:/r.x
				b.y:/r.y
				b.z:/r.z
				
				c.x:/r.x
				c.y:/r.y
				c.z:/r.z

				If CheckTriangle(collisionPackage,a,b,c)=True
	
					collisionPackage.collision=True		
					collisionPackage.surf=surf
					collisionPackage.tri=tri
		
				EndIf

			Next
		
		Next
	
	End Function

	Function PolysCheckCollision(collisionPackage:TCollisionPacket,polys:TPolygon[])
		
		' called from TOctree.OctCollision
		
		Local col_method=collisionPackage.method_no

		If col_method=2
			PolysCheckCollision2(collisionPackage,polys)
		Else
			PolysCheckCollision4(collisionPackage,polys)
		EndIf

	End Function
	
	' check octree polys for collision (method 2)
	Function PolysCheckCollision2(collisionPackage:TCollisionPacket,polys:TPolygon[])

		Local ent:TEntity=collisionPackage.ent
		Local ent2:TEntity=collisionPackage.ent2
	
		For Local poly:TPolygon=EachIn polys

			If poly.no_verts=3

				Local a:TVector=poly.verts[0].Copy()
				Local b:TVector=poly.verts[1].Copy()
				Local c:TVector=poly.verts[2].Copy()

				TEntity.TFormPoint(a.x,a.y,a.z,ent2,Null)
				a.x=TEntity.TFormedX#()
				a.y=TEntity.TFormedY#()
				a.z=TEntity.TFormedZ#()
				
				TEntity.TFormPoint(b.x,b.y,b.z,ent2,Null)
				b.x=TEntity.TFormedX#()
				b.y=TEntity.TFormedY#()
				b.z=TEntity.TFormedZ#()
				
				TEntity.TFormPoint(c.x,c.y,c.z,ent2,Null)
				c.x=TEntity.TFormedX#()
				c.y=TEntity.TFormedY#()
				c.z=TEntity.TFormedZ#()
				
				a.x:/ent.radius_x
				a.y:/ent.radius_y
				a.z:/ent.radius_z
				
				b.x:/ent.radius_x
				b.y:/ent.radius_y
				b.z:/ent.radius_z
				
				c.x:/ent.radius_x
				c.y:/ent.radius_y
				c.z:/ent.radius_z

				If CheckTriangle(collisionPackage,a,b,c)=True

					collisionPackage.collision=True
					collisionPackage.surf=poly.surf
					collisionPackage.tri=poly.tri
	
				EndIf

			EndIf
			
		Next
		
	End Function
	
	' check octree polys for collision (method 4)
	Function PolysCheckCollision4(collisionPackage:TCollisionPacket,polys:TPolygon[])

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

		Local r:TVector=TVector.Create(0,0,0)
		r.x=ent.radius_x/ent2.EntityScaleX(True)
		r.y=ent.radius_y/ent2.EntityScaleY(True)
		r.z=ent.radius_z/ent2.EntityScaleZ(True)

		Local bb1:TBox=GetBB4(collisionPackage)

		For Local poly:TPolygon=EachIn polys

			If poly.no_verts=3

				Local a:TVector=poly.verts[0].Copy()
				Local b:TVector=poly.verts[1].Copy()
				Local c:TVector=poly.verts[2].Copy()

				'Local bb2:TBox=TBox.CreateFromTri(a,b,c)

				a.x:/r.x
				a.y:/r.y
				a.z:/r.z
				
				b.x:/r.x
				b.y:/r.y
				b.z:/r.z
				
				c.x:/r.x
				c.y:/r.y
				c.z:/r.z
				
				'If TBox.TBoxTBoxIntersection(bb1,bb2)=True

					If CheckTriangle(collisionPackage,a,b,c)=True
						
						collisionPackage.collision=True
						collisionPackage.surf=poly.surf
						collisionPackage.tri=poly.tri
						collisionPackage.no_tris=collisionPackage.no_tris+1
	
					EndIf
					
				'EndIf

			EndIf
			
		Next

	End Function
	
	' and below a function that’ll check a single triangle for collision:
	' Assumes: p1,p2 And p3 are given in ellipsoid space:
	
	Function CheckTriangle(colPackage:TCollisionPacket Var,p1:TVector,p2:TVector,p3:TVector)
		
		Local ms=MilliSecs()
		
		' Make the plane containing this triangle.
		Local trianglePlane:TPlane=TPlane.CreatePlaneFromTriangle(p1,p2,p3)
		
		' Is triangle front-facing to the velocity vector?
		' We only check front-facing triangles
		' (your choice of course)
		If (trianglePlane.isFrontFacingTo(colPackage.normalizedVelocity))
		
			' Get interval of plane intersection:
			Local t0#, t1#
			
			Local embeddedInPlane = False
			
			' Calculate the signed distance from sphere position to triangle plane
			Local signedDistToTrianglePlane# = trianglePlane.signedDistanceTo(colPackage.basePoint)
			
			' cache this as we’re going to use it a few times below:
			Local normalDotVelocity# = trianglePlane.normal.Dot(colPackage.velocity)
			
			' If sphere is travelling parrallel to the plane:
			If (normalDotVelocity = 0.0)
					
				If (Abs(signedDistToTrianglePlane) >= 1.0)
				
					' Sphere is not embedded in plane.
					' No collision possible:
					Return
					
				Else
		
					' sphere is embedded in plane.
					' It intersects in the whole range [0..1]
					embeddedInPlane = True
					t0 = 0.0
					t1 = 1.0
					
				EndIf
	
			Else
			
				' N dot D is Not 0. Calculate intersection interval:
				t0=(-1.0-signedDistToTrianglePlane)/normalDotVelocity
				t1=( 1.0-signedDistToTrianglePlane)/normalDotVelocity
		
				' Swap so t0 < t1
				If (t0 > t1)
					Local temp# = t1
					t1# = t0
					t0 = temp
				EndIf
				
				' Check that at least one result is within range:
				If (t0 > 1.0 Or t1 < 0.0)
				
					' Both t values are outside values [0,1]
					' No collision possible:
					Return
				
				EndIf
		
				' Clamp To [0,1]
				If (t0 < 0.0) t0 = 0.0
				If (t1 < 0.0) t1 = 0.0
				If (t0 > 1.0) t0 = 1.0
				If (t1 > 1.0) t1 = 1.0
				
			EndIf
	
			' OK, at this point we have two time values t0 and t1
			' between which the swept sphere intersects with the
			' triangle plane. If any collision is to occur it must
			' happen within this interval.
			Local collisionPoint:TVector
			Local foundCollison = False
			Local t# = 1.0
		
			' First we check for the easy case - collision inside
			' the triangle. If this happens it must be at time t0
			' as this is when the sphere rests on the front side
			' of the triangle plane. Note, this can only happen if
			' the sphere is Not embedded in the triangle plane.
			If (Not embeddedInPlane)
				Local planeIntersectionPoint:TVector = (colPackage.basePoint.Subtract(trianglePlane.normal)).Add(colPackage.velocity.Multiply(t0))
				If (checkPointInTriangle(planeIntersectionPoint,p1,p2,p3))
					foundCollison = True
					t = t0
					collisionPoint=planeIntersectionPoint.Copy()
				EndIf
			EndIf
				
			' If we haven’t found a collision already we’ll have to
			' sweep sphere against points and edges of the triangle.
			' Note: A collision inside the triangle (the check above)
			' will always happen before a vertex or edge collision!
			' This is why we can skip the swept test if the above
			' gives a collision!
			If foundCollison = False
			
				' some commonly used terms:
				Local velocity:TVector = colPackage.velocity.Copy()
				Local base:TVector = colPackage.basePoint.Copy()
				Local velocitySquaredLength# = velocity.squaredLength()
				Local a#,b#,c# ' params for equation
				Local newT#
							
				' For each vertex or edge a quadratic equation have to
				' be solved. We parameterize this equation as
				' a*t^2 + b*t + c = 0 and below we calculate the
				' parameters a,b And c for each test.
				
				' Check against points:
				
				a = velocitySquaredLength
				
				' P1
				b = 2.0*(velocity.Dot(base.Subtract(p1)))
				c = (p1.Subtract(base)).squaredLength() - 1.0
				If getLowestRoot(a,b,c, t, newT)
					t = newT
					foundCollison = True
					collisionPoint = p1.Copy()
				EndIf
				
				' P2
				b = 2.0*(velocity.dot(base.Subtract(p2)))
				c = (p2.Subtract(base)).squaredLength() - 1.0
				If getLowestRoot(a,b,c, t, newT)
					t = newT
					foundCollison = True
					collisionPoint = p2.Copy()
				EndIf
				
				' P3
				b = 2.0*(velocity.dot(base.Subtract(p3)))
				c = (p3.Subtract(base)).squaredLength() - 1.0
				If getLowestRoot(a,b,c, t, newT)
					t = newT
					foundCollison = True
					collisionPoint = p3.Copy()
				EndIf
				
				' Check against edges:
				
				' p1 . p2:
				Local edge:TVector = p2.Subtract(p1)
				Local baseToVertex:TVector = p1.Subtract(base)
				Local edgeSquaredLength# = edge.squaredLength()
				Local edgeDotVelocity# = edge.Dot(velocity)
				Local edgeDotBaseToVertex# = edge.Dot(baseToVertex)
				
				' Calculate parameters for equation
				a = edgeSquaredLength*-velocitySquaredLength +edgeDotVelocity*edgeDotVelocity
				b = edgeSquaredLength*(2*velocity.dot(baseToVertex))-2.0*edgeDotVelocity*edgeDotBaseToVertex
				c = edgeSquaredLength*(1-baseToVertex.squaredLength())+edgeDotBaseToVertex*edgeDotBaseToVertex
				
				' Does the swept sphere collide against infinite edge?
				If getLowestRoot(a,b,c, t, newT)
					' Check if intersection is within line segment:
					Local f#=(edgeDotVelocity*newT-edgeDotBaseToVertex)/edgeSquaredLength
					If f >= 0.0 And f <= 1.0
						' intersection took place within segment.
						t = newT
						foundCollison = True
						collisionPoint = p1.Add(edge.Multiply(f))
					EndIf
				EndIf
				
				' p2 . p3:
				edge:TVector = p3.Subtract(p2)
				baseToVertex:TVector = p2.Subtract(base)
				edgeSquaredLength# = edge.squaredLength()
				edgeDotVelocity# = edge.Dot(velocity)
				edgeDotBaseToVertex# = edge.Dot(baseToVertex)
				a = edgeSquaredLength*-velocitySquaredLength +edgeDotVelocity*edgeDotVelocity
				b = edgeSquaredLength*(2*velocity.dot(baseToVertex))-2.0*edgeDotVelocity*edgeDotBaseToVertex
				c = edgeSquaredLength*(1-baseToVertex.squaredLength())+edgeDotBaseToVertex*edgeDotBaseToVertex
				If getLowestRoot(a,b,c, t, newT)
					Local f#=(edgeDotVelocity*newT-edgeDotBaseToVertex)/edgeSquaredLength
					If (f >= 0.0 And f <= 1.0)
						t = newT
						foundCollison = True
						collisionPoint = p2.Add(edge.Multiply(f))
					EndIf
				EndIf
				
				' p3 . p1:
				edge:TVector = p1.Subtract(p3)
				baseToVertex:TVector = p3.Subtract(base)
				edgeSquaredLength# = edge.squaredLength()
				edgeDotVelocity# = edge.Dot(velocity)
				edgeDotBaseToVertex# = edge.Dot(baseToVertex)
				a = edgeSquaredLength*-velocitySquaredLength +edgeDotVelocity*edgeDotVelocity
				b = edgeSquaredLength*(2*velocity.dot(baseToVertex))-2.0*edgeDotVelocity*edgeDotBaseToVertex
				c = edgeSquaredLength*(1-baseToVertex.squaredLength())+edgeDotBaseToVertex*edgeDotBaseToVertex
				If getLowestRoot(a,b,c, t, newT)
					Local f#=(edgeDotVelocity*newT-edgeDotBaseToVertex)/edgeSquaredLength
					If (f >= 0.0 And f <= 1.0)
						t = newT
						foundCollison = True
						collisionPoint = p3.Add(edge.Multiply(f))
					EndIf
				EndIf
				
			EndIf
				
			' Set result:
			If (foundCollison = True)
	
				' distance To collision: ’t’ is time of collision
				Local distToCollision# = t*colPackage.velocity.length()
			
				' Does this triangle qualify for the closest hit?
				' it does if it’s the first hit or the closest
				If (colPackage.foundCollision = False Or distToCollision < colPackage.nearestDistance)
				
					' Collision information nessesary for sliding
					colPackage.nearestDistance = distToCollision
					colPackage.intersectionPoint=collisionPoint.Copy()
				
					colPackage.foundCollision = True
					
					Return True
				
				EndIf
			
			EndIf ' If Not backface
	
		EndIf
		
		'DebugLog "CheckTriangle "+String(MilliSecs()-ms)
		
	End Function

	Rem
	Utility functions
	Below is a Function that determines if a point is inside a triangle Or not. This
	is used in the collision detection step.
	End Rem

	Function CheckPointInTriangle(p:TVector,a:TVector,b:TVector,c:TVector)
	
		Local ms=MilliSecs()
	
		Local u#,v#,w#
		
		Barycentric(a,b,c,p,u#,v#,w#)
	
		'DebugLog "CheckPointInTriangle "+String(MilliSecs()-ms)
	
		If v>=0.0 And w>=0.0 And (v+w) <= 1.0 Return True Else Return False
	
	End Function

	Rem
	Function CheckPointInTriangle(point:TVector,pa:TVector,pb:TVector,pc:TVector)

		Local e10:TVector=pb.Subtract(pa)
		Local e20:TVector=pc.Subtract(pa)
		
		Local a:Float = e10.Dot(e10)
		Local b:Float = e10.Dot(e20)
		Local c:Float = e20.Dot(e20)
		Local ac_bb:Float=(a*c)-(b*b)
		
		Local vp:TVector=TVector.Create(point.x-pa.x, point.y-pa.y, point.z-pa.z)
		
		Local d:Float = vp.Dot(e10)
		Local e:Float = vp.Dot(e20)
		Local x:Float = (d*c)-(e*b)
		Local y:Float = (e*a)-(d*b)
		Local z:Float = x+y-ac_bb
		Return (( in(z)& ~(in(x)|in(y)) ) & 0x80000000)
		
	EndIf
	End Rem

	Function Barycentric(a:TVector,b:TVector,c:TVector,p:TVector,u# Var,v# Var,w# Var)
	
		Local v0:TVector=b.Subtract(a)
		Local v1:TVector=c.Subtract(a)
		Local v2:TVector=p.Subtract(a)
		
		Local d00#=v0.Dot(v0)
		Local d01#=v0.Dot(v1)
		Local d11#=v1.Dot(v1)
		Local d20#=v2.Dot(v0)
		Local d21#=v2.Dot(v1)
		Local denom#=d00*d11-d01*d01
		
		v=(d11*d20-d01*d21)/denom
		w=(d00*d21-d01*d20)/denom
		u=1.0-v-w
	
	End Function

	Rem
	Solving quadratic equations
	Below is a snippet of code that solves a quadratic equation And returns the
	lowest root, below a certain treshold (the maxR parameter):
	End Rem
	Function getLowestRoot(a#,b#,c#,maxR#,root# Var)
		
		' Check if a solution exists
		Local determinant# = b*b - 4.0*a*c
		
		' If determinant is negative it means no solutions.
		If (determinant < 0.0) Return False
		
		' calculate the two roots: (If determinant == 0 then
		' x1==x2 but let’s disregard that slight optimization)
		Local sqrtD# = Sqr(determinant)
		Local r1# = (-b - sqrtD#) / (2*a)
		Local r2# = (-b + sqrtD#) / (2*a)
		
		' Sort so x1 <= x2
		If (r1 > r2)
			Local temp# = r2
			r2 = r1
			r1 = temp
		EndIf
		
		' Get lowest root
		If (r1 > 0 And r1 < maxR)
			root = r1
			Return True
		EndIf
	
		' It is possible that we want x2 - this can happen
		' If x1 < 0
		If (r2 > 0 And r2 < maxR)
			root = r2
			Return True
		EndIf
		
		' No (valid) solutions
		Return False
		
	End Function
	
	Function SphereToSphere(ent:TEntity,ent2:TEntity,t#=0.0)

		Const EPSILON=0.001

		Local s0:TSphere=New TSphere
		s0.c.x=ent.old_x#
		s0.c.y=ent.old_y#
		s0.c.z=ent.old_z#
		s0.r=ent.radius_x
		
		Local s1:TSphere=New TSphere
		s1.c.x=ent2.old_x#
		s1.c.y=ent2.old_y#
		s1.c.z=ent2.old_z#
		s1.r=ent2.radius_x
										
		Local v0:TVector=New TVector
		v0.x=ent.EntityX(True)-ent.old_x#
		v0.y=ent.EntityY(True)-ent.old_y#
		v0.z=ent.EntityZ(True)-ent.old_z#
		
		Local v1:TVector=New TVector
		v1.x=ent2.EntityX(True)-ent2.old_x#
		v1.y=ent2.EntityY(True)-ent2.old_y#
		v1.z=ent2.EntityZ(True)-ent2.old_z#					
	
		Local s:TVector=New TVector
		s=s1.c.Subtract(s0.c)
		
		Local v:TVector=New TVector
		v=v1.Subtract(v0)
		
		Local r#=s1.r+s0.r
		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
	
	Rem
	Function SphereToSphere1(ent:TEntity,ent2:TEntity,t#=0.0)

		Const EPSILON=0.001

		Local s0:TSphere=New TSphere
		s0.c.x=ent.old_x#
		s0.c.y=ent.old_y#
		s0.c.z=ent.old_z#
		s0.r=1.0
		
		Local s1:TSphere=New TSphere
		s1.c.x=ent2.old_x#
		s1.c.y=ent2.old_y#
		s1.c.z=ent2.old_z#
		s1.r=1.0
										
		Local v0:TVector=New TVector
		v0.x=ent.EntityX(True)-ent.old_x#
		v0.y=ent.EntityY(True)-ent.old_y#
		v0.z=ent.EntityZ(True)-ent.old_z#
		
		Local v1:TVector=New TVector
		v1.x=ent2.EntityX(True)-ent2.old_x#
		v1.y=ent2.EntityY(True)-ent2.old_y#
		v1.z=ent2.EntityZ(True)-ent2.old_z#	

		s1.r=s1.r+s0.r
		
		Local v:TVector=v0.Subtract(v1)
		
		Local q:TVector=New TVector
		Local vlen#=v.Length()
		
		Local s2:TVector=s0.c.Add(v)
	
		If TPick.IntersectSegmentSphere(s0.c,s2,s1.c,s1.r,t#,q)
			Return t<=vlen
		EndIf
		
		Return 0
	
	End Function
	End Rem

End Type

Rem
Data about the move is coming in through the
CollisionPacket and this is also where we put the result of the check against
the triangle.
End Rem
Type TCollisionPacket
	
	' added

	Field method_no
	Field response

	Field ent:TEntity
	Field ent2:TEntity

	Field bb:TBox ' bounding box of moving sphere (ent)
	
	Field collision
	
	Field no_tris
	Field tri
	Field surf:TSurface
	
	' original

	' Information about the move being requested: (in R3)
	Field R3Velocity:TVector=New TVector
	Field R3Position:TVector=New TVector
	
	' Information about the move being requested: (in eSpace)
	Field velocity:TVector=New TVector
	Field normalizedVelocity:TVector=New TVector
	Field basePoint:TVector=New TVector
	
	' Hit information
	Field foundCollision
	Field nearestDistance#
	Field intersectionPoint:TVector=New TVector
			
End Type

Type TCollisionImpact

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

End Type
