--
-- File:: rockstar/helpers/pathnodecheck.ms
-- Description:: Path Node Checker
--

RsCollisionList = #()
RsCheckDistance = 1.0
RsErrorList = #()

fn getCollisionList pos colList = (

	for obj in RsCollisionList do (

		if pos.x >= obj.min.x and pos.x <= obj.max.x and pos.y >= obj.min.y and pos.y <= obj.max.y do (
	
			append colList obj
		)
	)
)

fn doesNodeCollide checkNode = (

	colList = #()
	getCollisionList checkNode.pos colList

	retval = false

	for obj in colList do (
	
		col2mesh obj
		
		facecount = obj.numfaces			
		
		for i = 1 to facecount do (
		
			currface = getface obj i
			vertA = getvert obj currface[1]
			vertB = getvert obj currface[2]
			vertC = getvert obj currface[3]
			
			dist = raytriintersect checkNode.pos [0,0,-1] vertA vertB vertC
							
			if dist != undefined then (
			
				retval = true
			) else (
			
				dist = raytriintersect checkNode.pos [0,0,1] vertA vertC vertB

				if dist != undefined then (

					retval = true
				)				
			)
		)
		
		mesh2col obj
		
		if retval then return true
	)

	retval
)

fn checkPosAgainstCollision checkpos = (

	colList = #()
	getCollisionList checkpos colList
				
	minDist = undefined

	for obj in colList do (
	
		col2mesh obj
		
		facecount = obj.numfaces			
		
		for i = 1 to facecount do (
		
			currface = getface obj i
			vertA = getvert obj currface[1]
			vertB = getvert obj currface[2]
			vertC = getvert obj currface[3]
			
			dist = raytriintersect checkpos [0,0,-1] vertA vertB vertC
							
			if dist != undefined and (minDist == undefined or dist < (abs minDist)) then (
			
				minDist = dist
			) else (
			
				dist = raytriintersect checkpos [0,0,1] vertA vertC vertB

				if dist != undefined and (minDist == undefined or dist < (abs minDist)) then (

					minDist = -dist
				)			
			)
		)
		
		mesh2col obj
	)
	
	minDist 
)

fn checkNodePathAgainstCollision checkNode = (
		
	minDist = checkPosAgainstCollision checkNode.pos
		
	if minDist != undefined and ((abs minDist) < 1.0) then (
	
		checkNode.pos.z = checkNode.pos.z - minDist
	)
)

fn checkLinkAgainstCollision linkNode deviation = (

	nodeA = VehLinkGetNodeA linkNode
	nodeB = VehLinkGetNodeB linkNode
	
	error = false
	
	if doesNodeCollide nodeA and doesNodeCollide nodeB then (
	
		diff = nodeA.pos - nodeB.pos
		difflength = length diff
		normal = normalize diff
		checkPos = nodeB.pos
		
		difflength = difflength - RsCheckDistance
		
		while difflength > 0.0 do (
		
			checkPos = checkPos + (normal * RsCheckDistance)
			
			dist = checkPosAgainstCollision checkPos
			
			if ( ( dist != undefined ) and ( dist > deviation ) ) then return true
			
			difflength = difflength - RsCheckDistance
		)
	)
	
	false
)