local O={}O.__index=O local m={}m.__index=m local N={0}local P={0}local g={0}local A={0}function lerp(O,m,N)return O+(m-O)*N end function O.new(N,P,g,A,s,E,S)local j=setmetatable({},O)j.start=m.new(N[1],N[2],N[3])j.endpoint=m.new(P[1],P[2],P[3])j.Xrand=g[1]j.Yrand=g[2]j.Zrand=g[3]j.expandDis=A j.goalSampleRate=s j.MaxNode=E j.MaxIter=S return j end function O._Start(E,s)E.nodeList={[1]=E.start}for s=1,E.MaxIter,1 do if A[1]==0 then E.endpoint=m.new(workspace.End.Position.X*3,workspace.End.Position.Y*3,workspace.End.Position.Z*3)A[1]=workspace.End.Position*3 elseif A[1]~=workspace.End.Position*3 then E.endpoint=m.new(workspace.End.Position.X*3,workspace.End.Position.Y*3,workspace.End.Position.Z*3)A[1]=workspace.End.Position*3 E.expandDis=15 N={0}P={0}g={0}end local S if N[1]~=0 then S=E:_heuristic_random_point()else S=E:_get_random_point()end local j=E:_GetNearestListIndex(S)local x=E:_steer(S,j)local H=E.nodeList[j]local e=x.x-H.x local Y=x.y-H.y local B=x.z-H.z local R=math.sqrt((e^2+Y^2)+B^2)if E:_check_collision_extend(H.x,H.y,H.z,x.x,x.y,x.z,R)then local m=E:_find_near_nodes(x,5)local N=E:_choose_parent(x,m)E.nodeList[s+99]=N E:_rewire(s+99,N,m)table.insert(E.nodeList[N.parent].children,O:_len(E.nodeList[N.parent].children)+1,s+99)if O:_len(E.nodeList)>E.MaxNode then local m={}for N,P in pairs(E.nodeList)do if O:_len(P.children)==0 and O:_len(E.nodeList[P.parent].children)>1 then m[N]=N end end if#m>1 then local O=m[math.random(1,#m-1)]table.remove(E.nodeList[E.nodeList[O].parent].children,table.find(E.nodeList[E.nodeList[O].parent].children,O))E.nodeList[O]=nil else local m={}for N,P in pairs(E.nodeList)do if O:_len(P.children)==0 then table.insert(m,#m+1,N)end end local N=m[math.random(1,#m-1)]table.remove(E.nodeList[E.nodeList[N].parent].children,table.find(E.nodeList[E.nodeList[N].parent].children,N))E.nodeList[N]=nil end end end if s%10==0 then O:_DrawGraph(E.nodeList,E.start,E.endpoint,E.expandDis)if s%10==0 then for O,m in pairs(workspace.Nodes2:GetChildren())do m:Destroy()end for O,m in pairs(E.nodeList)do local N=Instance.new("\080\097\114\116")N.Parent=workspace.Nodes2 N.Size=Vector3.new(1.5,1.5,1.5)N.Position=Vector3.new(m.x,m.y,m.z)/3 N.Anchored=true N.Locked=true N.CanCollide=false N.Shape=Enum.PartType.Ball N.Material=Enum.Material.SmoothPlastic N.Color=Color3.fromRGB(190,0,3)if m.parent~=nil then local O=(Vector3.new(m.x,m.y,m.z)-Vector3.new(E.nodeList[m.parent].x,E.nodeList[m.parent].y,E.nodeList[m.parent].z)).magnitude local N=Instance.new("\080\097\114\116")N.Parent=workspace.Nodes2 N.Locked=true N.Size=Vector3.new(.8,.8,O/3)N.Position=Vector3.new(lerp(m.x,E.nodeList[m.parent].x,.5),lerp(m.y,E.nodeList[m.parent].y,.5),lerp(m.z,E.nodeList[m.parent].z,.5))/3 N.Anchored=true N.CanCollide=false N.CFrame=CFrame.new(N.Position,Vector3.new(E.nodeList[m.parent].x,E.nodeList[m.parent].y,E.nodeList[m.parent].z)/3)N.Material=Enum.Material.SmoothPlastic N.Color=Color3.fromRGB(255,65,68)end end if g[1]~=0 then E.expandDis=30 local m=g[1]local N=O:_len(g[1])while N>1 do local O=Vector3.new(m[N][1]/3,m[N][2]/3,m[N][3]/3)local P=Vector3.new(m[N-1][1]/3,m[N-1][2]/3,m[N-1][3]/3)local g=(O-P).Magnitude local A=Instance.new("\080\097\114\116",workspace.Heuristic)A.Anchored=true A.Locked=true A.CanCollide=false A.Position=O+(P-O)*.5 A.CFrame=CFrame.new(A.Position,P)A.Size=Vector3.new(2,2,g)A.Material=Enum.Material.SmoothPlastic A.Color=Color3.fromRGB(89,255,0)N-=1 end end wait()end for O,m in pairs(workspace.Heuristic:GetChildren())do m:Destroy()end end end for O,m in pairs(workspace.Nodes2:GetChildren())do m:Destroy()end if g[1]~=0 then local m=g[1]local N=O:_len(g[1])while N>1 do local O=Vector3.new(m[N][1]/3,m[N][2]/3,m[N][3]/3)local P=Vector3.new(m[N-1][1]/3,m[N-1][2]/3,m[N-1][3]/3)local g=(O-P).Magnitude local A=Instance.new("\080\097\114\116",workspace.Detect)A.Anchored=true A.Position=O+(P-O)*.5 A.CFrame=CFrame.new(A.Position,P)A.Size=Vector3.new(2,2,g)A.CanCollide=false A.Material=Enum.Material.SmoothPlastic A.Color=Color3.fromRGB(89,255,0)N-=1 wait(.05)end end return E.nodeList end function O._DrawGraph(S,m,A,s,E)local j=S:_get_best_last_index(m,s,E)if j~=nil then local E=S:_gen_final_course(j,A,s,m)local x=O:_len(E)local H=0 local e=(workspace.Start.Position-workspace.End.Position).magnitude while x>1 do local O=(Vector3.new(E[x-1][1],E[x-1][2],E[x-1][3])-Vector3.new(E[x][1],E[x][2],E[x][3])).magnitude H=H+O x-=1 end local Y=Vector3.new(A.x,A.y,A.z)local B=Vector3.new(s.x,s.y,s.z)local R=N[1]local l=math.sqrt(((Y.X-B.X)^2+(Y.Y-B.Y)^2)+(Y.Z-B.Z)^2)/2 local v=math.sqrt((R/2)^2-l^2)local h=Y+(B-Y)*.5 local M=script.Parent.Heuristic:Clone()M.Parent=workspace.Heuristic M.Anchored=true M.Locked=true M.CanCollide=false M.Size=Vector3.new(v,v,R)/3 M.Position=h/3 M.CFrame=CFrame.new(M.Position,B/3)M.Color=Color3.fromRGB(255,0,4)M.Material=Enum.Material.ForceField if H<P[1]then g[1]=E P[1]=H elseif P[1]==0 then g[1]=E P[1]=H end if e*3+1~=N[1]and e*3+1<N[1]then if N[1]>H then M.Color=Color3.fromRGB(255,255,0)N[1]=H else N[1]=N[1]-.05 end elseif N[1]==0 then N[1]=H end end end function O._gen_final_course(A,m,N,P,g)local s={{P.x,P.y;P.z}}while g[m].parent~=nil do local N=g[m]table.insert(s,O:_len(s)+1,{N.x;N.y,N.z})m=N.parent end table.insert(s,O:_len(s)+1,{N.x,N.y,N.z})return s end function O._path_validation(g,O,m,N,P)local A=g:_get_best_last_index(O,N,P)if A~=nil then while O[A].parent~=nil do local m=A A=O[A].parent local N=O[m].x-O[A].x local P=O[m].y-O[A].y local s=O[m].z-O[A].z local E=math.sqrt((N^2+P^2)+s^2)if not g:_check_collision_extend(O[A].x,O[A].y,O[A].z,O[m].x,O[m].y,O[m].z,E)then table.remove(O.children,m)g:_remove_branch(m)end end end end function O._remove_branch(m,O)for O in m.nodelist[O].children do m:_remove_branch(O)end table.remove(m.nodeList,O)end function O._choose_parent(N,O,m)if#m==0 then return O end local P={}for m,g in pairs(m)do local A=O.x-N.nodeList[g].x local s=O.y-N.nodeList[g].y local E=O.z-N.nodeList[g].z local S=math.sqrt((A^2+s^2)+E^2)if N:_check_collision_extend(N.nodeList[g].x,N.nodeList[g].y,N.nodeList[g].z,O.x,O.y,O.z,S)then table.insert(P,#P+1,N.nodeList[g].cost+S)else table.insert(P,#P+1,"\105\110\102")end end local g=math.min(table.unpack(P))local A=m[table.find(P,g)]if g==math.huge then print("\109\105\110\099\111\115\116 \105\115 \105\110\102")return O end O.cost=g O.parent=A return O end function O._heuristic_random_point(O)local m if math.random(0,100)>O.goalSampleRate then local P=Vector3.new(O.start.x,O.start.y,O.start.z)local g=Vector3.new(O.endpoint.x,O.endpoint.y,O.endpoint.z)local A=N[1]local s=math.sqrt(((P.X-g.X)^2+(P.Y-g.Y)^2)+(P.Z-g.Z)^2)/2 local E=math.sqrt((A/2)^2-s^2)local S=P+(g-P)*.5 local j=math.random(-(A/2),A/2)local x=(math.abs(E*2)/math.abs(A))*math.sqrt((A/2)^2-j^2)local H=math.random(-x/2,x/2)local e=math.abs(E/2)*math.sqrt((1-j^2/(A/2)^2)-H^2/(E/2)^2)local Y=math.random(-e,e)local B=(CFrame.new(S,P)*CFrame.new(Y,H,j)).Position m={B.X,B.Y,B.Z}else m={O.endpoint.x,O.endpoint.y,O.endpoint.z}end return m end function O._len(m,O)local N=0 for O,m in pairs(O)do N=N+1 end return N end function O._steer(P,O,N)local g=P.nodeList[N]local A=O[1]-g.x local s=O[2]-g.y local E=O[3]-g.z local S=math.sqrt((A^2+s^2)+E^2)local j=m.new(g.x,g.y,g.z)j.x=g.x+(O[1]-g.x)*(P.expandDis/S)j.y=g.y+(O[2]-g.y)*(P.expandDis/S)j.z=g.z+(O[3]-g.z)*(P.expandDis/S)j.cost=g.cost+P.expandDis j.parent=N return j end function O._get_random_point(O)local m if math.random(0,100)>O.goalSampleRate then m={math.random(0,O.Xrand),math.random(0,O.Yrand),math.random(0,O.Zrand)}else m={O.endpoint.x;O.endpoint.y,O.endpoint.z}end return m end function O._get_best_last_index(g,m,N,P)local A={}for O,m in pairs(m)do local g=math.sqrt(((m.x-N.x)^2+(m.y-N.y)^2)+(m.z-N.z)^2)if g<=P then table.insert(A,#A+1,O)end end if O:_len(A)==0 then return nil end for O,N in pairs(A)do if m[N].cost==math.min(m[N].cost)then return N end end return nil end function O._calc_dist_to_goal(N,O,m)return math.sqrt(O^2+m^2)end function O._find_near_nodes(N,O,m)local P=N.expandDis*m local g={}local A={}for m,N in pairs(N.nodeList)do if((N.x-O.x)^2+(N.y-O.y)^2)+(N.z-O.z)^2<=P^2 then table.insert(g,m,N)end end for O,m in pairs(N.nodeList)do if g[O]~=nil then table.insert(A,#A+1,O)end end return A end function O._rewire(g,m,N,P)local A=O:_len(g.nodeList)for O,P in pairs(P)do local A=g.nodeList[P]local s=N.x-A.x local E=N.y-A.y local S=N.z-A.z local j=math.sqrt((s^2+E^2)+S^2)local x=N.cost+j if A.cost>x then if g:_check_collision_extend(A.x,A.y,A.z,N.x,N.y,N.z,j)then table.remove(g.nodeList[A.parent].children,table.find(g.nodeList[A.parent].children,P))A.parent=m A.cost=x table.insert(N.children,#N.children+1,P)end end end end function O._check_collision_extend(E,O,m,N,P,g,A,s)local S=Vector3.new(O,m,N)/3 local j=Vector3.new(P,g,A)/3 local x=(CFrame.lookAt(S,j)).LookVector*(s/3)local H=RaycastParams.new()H.FilterDescendantsInstances={workspace.Collisions}H.FilterType=Enum.RaycastFilterType.Whitelist local e=workspace:Raycast(S,x,H)if e~=nil then return false else return true end end function O._GetNearestListIndex(m,O)local N={}local P={}local g={}for m,P in pairs(m.nodeList)do table.insert(N,#N+1,{(P.x-O[1])^2;(P.y-O[2])^2,(P.z-O[3])^2})end for O,m in pairs(N)do table.insert(P,O,(m[1]+m[2])+m[3])end for O,m in pairs(m.nodeList)do table.insert(g,#g+1,O)end return g[table.find(P,math.min(table.unpack(P)))]end function m.new(O,N,P)local g=setmetatable({},m)g.x=O g.y=N g.z=P g.cost=.0 g.parent=nil g.children={}return g end return O
--Nicolai & Perpelea, Edited on March 19th 2022, 19:54 GMT+1
local rrt = {}
rrt.__index = rrt

local Node = {}
Node.__index = Node

local ShortestPath = {0}
local OldLength = {0}
local CurrentIteration = {0}
local CurrentEndPoint = {0}

function lerp(a,b,t)
	return a+(b-a)*t
end

function rrt.new(start, goal, randArea, expandDis, goalSampleRate, MaxNode, MaxIter)
	local self = setmetatable({}, rrt)
	self.start = Node.new(start[1], start[2], start[3])
	self.endpoint = Node.new(goal[1], goal[2], goal[3])
	self.Xrand = randArea[1]
	self.Yrand = randArea[2]
	self.Zrand = randArea[3]
	self.expandDis = expandDis
	self.goalSampleRate = goalSampleRate
	self.MaxNode = MaxNode
	self.MaxIter = MaxIter
	return self
end

function rrt:_Start(info)
	self.nodeList = {[1]= self.start}	

	for i = 1,self.MaxIter do
		if CurrentEndPoint[1] == 0 then
			self.endpoint = Node.new(workspace.End.Position.X*3,workspace.End.Position.Y*3,workspace.End.Position.Z*3)
			CurrentEndPoint[1] = workspace.End.Position*3
		elseif CurrentEndPoint[1] ~= workspace.End.Position*3 then
			self.endpoint = Node.new(workspace.End.Position.X*3,workspace.End.Position.Y*3,workspace.End.Position.Z*3)
			CurrentEndPoint[1] = workspace.End.Position*3
			self.expandDis = 15
			ShortestPath = {0}
			OldLength = {0}
			CurrentIteration = {0}
		end

		local rnd
		if ShortestPath[1] ~= 0 then
			rnd = self:_heuristic_random_point()
		else
			rnd = self:_get_random_point()
		end

		local nind = self:_GetNearestListIndex(rnd)
		local newNode = self:_steer(rnd, nind)


		--Intersection
		local nearestNode = self.nodeList[nind]

		local dx = newNode.x - nearestNode.x
		local dy = newNode.y - nearestNode.y
		local dz = newNode.z - nearestNode.z
		local d = math.sqrt(dx^2 + dy^2 + dz^2)


		if self:_check_collision_extend(nearestNode.x, nearestNode.y, nearestNode.z, newNode.x, newNode.y, newNode.z, d) then
			local nearinds = self:_find_near_nodes(newNode,5) 
			local newNode = self:_choose_parent(newNode, nearinds)
			self.nodeList[i+99] = newNode

			self:_rewire(i+99, newNode, nearinds) 
			table.insert(self.nodeList[newNode.parent].children,(rrt:_len(self.nodeList[newNode.parent].children))+1,i+99)

			if rrt:_len(self.nodeList) > self.MaxNode then
				local leaves = {}

				for key, node in pairs(self.nodeList) do
					if rrt:_len(node.children) == 0 and rrt:_len(self.nodeList[node.parent].children) > 1 then 
						leaves[key] = key
					end
				end

				if #leaves > 1 then
					local ind = leaves[math.random(1,(#leaves)-1)]
					table.remove(self.nodeList[self.nodeList[ind].parent].children, table.find(self.nodeList[self.nodeList[ind].parent].children,ind))	
					self.nodeList[ind] = nil
				else
					local leaves2 = {}
					for key, node in pairs(self.nodeList) do
						if rrt:_len(node.children) == 0 then
							table.insert(leaves2,(#leaves2)+1,key)
						end
					end

					local ind = leaves2[math.random(1,(#leaves2)-1)]
					table.remove(self.nodeList[self.nodeList[ind].parent].children, table.find(self.nodeList[self.nodeList[ind].parent].children,ind))	
					self.nodeList[ind] = nil
				end
			end
		end


		if i%10 == 0 then
			rrt:_DrawGraph(self.nodeList,self.start,self.endpoint,self.expandDis)
			if i%10 == 0 then 
				for _,v in pairs(workspace.Nodes2:GetChildren()) do
					v:Destroy()
				end

				for _,node in pairs(self.nodeList) do
					local prt = Instance.new("Part")
					prt.Parent = workspace.Nodes2
					prt.Size = Vector3.new(1.5,1.5,1.5)
					prt.Position = Vector3.new(node.x,node.y,node.z)/3
					prt.Anchored = true
					prt.Locked = true
					prt.CanCollide = false


					prt.Shape = Enum.PartType.Ball
					prt.Material = Enum.Material.SmoothPlastic
					prt.Color = Color3.fromRGB(190, 0, 3)
					if node.parent ~= nil then
						local Magnitude = (Vector3.new(node.x,node.y,node.z)-Vector3.new(self.nodeList[node.parent].x,self.nodeList[node.parent].y,self.nodeList[node.parent].z)).magnitude
						local prt = Instance.new("Part")
						prt.Parent = workspace.Nodes2
						prt.Locked = true
						prt.Size = Vector3.new(.8,.8,Magnitude/3)
						prt.Position = Vector3.new(lerp(node.x,self.nodeList[node.parent].x,.5),lerp(node.y,self.nodeList[node.parent].y,.5),lerp(node.z,self.nodeList[node.parent].z,.5))/3
						prt.Anchored = true
						prt.CanCollide = false
						prt.CFrame = CFrame.new(prt.Position,Vector3.new(self.nodeList[node.parent].x,self.nodeList[node.parent].y,self.nodeList[node.parent].z)/3)
						prt.Material = Enum.Material.SmoothPlastic
						prt.Color = Color3.fromRGB(255, 65, 68)
					end
				end
				if CurrentIteration[1] ~= 0 then
					self.expandDis = 30
					local path = CurrentIteration[1]
					local ind = rrt:_len(CurrentIteration[1])

					while ind > 1 do
						local p1 = Vector3.new(path[ind][1]/3,path[ind][2]/3,path[ind][3]/3)
						local p2 = Vector3.new(path[ind-1][1]/3,path[ind-1][2]/3,path[ind-1][3]/3)

						local Magnitude = (p1-p2).Magnitude

						local Segment = Instance.new("Part",workspace.Heuristic)
						Segment.Anchored = true
						Segment.Locked = true
						Segment.CanCollide = false
						Segment.Position = p1+(p2-p1)*.5
						Segment.CFrame = CFrame.new(Segment.Position,p2) 
						Segment.Size = Vector3.new(2,2,Magnitude)
						Segment.Material = Enum.Material.SmoothPlastic
						Segment.Color = Color3.fromRGB(89, 255, 0)

						ind-=1
					end
				end

				wait()
			end
			for _,v in pairs(workspace.Heuristic:GetChildren()) do
				v:Destroy()
			end
		end
	end
	for _,v in pairs(workspace.Nodes2:GetChildren()) do
		v:Destroy()
	end

	if CurrentIteration[1] ~= 0 then
		local path = CurrentIteration[1]
		local ind = rrt:_len(CurrentIteration[1])

		while ind > 1 do
			local p1 = Vector3.new(path[ind][1]/3,path[ind][2]/3,path[ind][3]/3)
			local p2 = Vector3.new(path[ind-1][1]/3,path[ind-1][2]/3,path[ind-1][3]/3)

			local Magnitude = (p1-p2).Magnitude

			local Segment = Instance.new("Part",workspace.Detect)
			Segment.Anchored = true
			Segment.Position = p1+(p2-p1)*.5
			Segment.CFrame = CFrame.new(Segment.Position,p2) 
			Segment.Size = Vector3.new(2,2,Magnitude)
			Segment.CanCollide = false
			Segment.Material = Enum.Material.SmoothPlastic
			Segment.Color = Color3.fromRGB(89, 255, 0)

			ind-=1
			wait(.05)
		end
	end
	return self.nodeList
end

function rrt:_DrawGraph(nodeList,start,endpoint,expandDis)
	local lastIndex = self:_get_best_last_index(nodeList,endpoint,expandDis)


	if lastIndex ~= nil then
		local path = self:_gen_final_course(lastIndex,start,endpoint,nodeList)
		local ind = rrt:_len(path)
		local pathlength = 0
		local PointMag = (workspace.Start.Position-workspace.End.Position).magnitude

		while ind > 1 do
			local distance = ((Vector3.new(path[ind-1][1], path[ind-1][2], path[ind-1][3]))-(Vector3.new(path[ind][1], path[ind][2], path[ind][3]))).magnitude
			pathlength = pathlength + distance

			ind-=1
		end

		local start = Vector3.new(start.x,start.y,start.z)
		local endpoint = Vector3.new(endpoint.x,endpoint.y,endpoint.z)

		local MajorAxis = ShortestPath[1]
		local FocalLength = (math.sqrt(((start.X-endpoint.X)^2)+((start.Y-endpoint.Y)^2)+((start.Z-endpoint.Z)^2))/2)
		local MinorAxis = math.sqrt(((MajorAxis/2)^2)-FocalLength^2)
		local Center = start+(endpoint-start)*.5

		local prt = script.Parent.Heuristic:Clone()
		prt.Parent = workspace.Heuristic
		prt.Anchored = true
		prt.Locked = true
		prt.CanCollide = false
		prt.Size = Vector3.new(MinorAxis,MinorAxis,MajorAxis)/3
		prt.Position = Center/3
		prt.CFrame = CFrame.new(prt.Position,endpoint/3)
		prt.Color = Color3.fromRGB(255, 0, 4) 
		prt.Material = Enum.Material.ForceField


		if pathlength < OldLength[1] then
			CurrentIteration[1] = path
			OldLength[1] = pathlength
		elseif OldLength[1] == 0 then
			CurrentIteration[1] = path
			OldLength[1] = pathlength
		end


		if (PointMag*3+1  ~= ShortestPath[1] and PointMag*3+1 < ShortestPath[1]) then
			if ShortestPath[1] > pathlength then
				prt.Color = Color3.fromRGB(255, 255, 0) 
				ShortestPath[1] = pathlength
			else
				ShortestPath[1] = ShortestPath[1]  - .05
			end
		elseif ShortestPath[1] == 0 then
			ShortestPath[1] = pathlength
		end
	end
end

function rrt:_gen_final_course(goalind,start,endpoint,nodeList)
	local path = {{endpoint.x, endpoint.y, endpoint.z}}

	while nodeList[goalind].parent ~= nil do
		local node = nodeList[goalind]
		table.insert(path,rrt:_len(path) + 1,{node.x, node.y, node.z})
		goalind = node.parent
	end
	table.insert(path,rrt:_len(path) + 1,{start.x, start.y, start.z})
	return path
end


function rrt:_path_validation(nodeList,startpoint,endpoint,expandDis)
	local lastIndex = self:_get_best_last_index(nodeList,endpoint,expandDis)
	if lastIndex ~= nil then
		while nodeList[lastIndex].parent ~= nil do
			local nodeInd = lastIndex
			lastIndex = nodeList[lastIndex].parent

			local dx = nodeList[nodeInd].x - nodeList[lastIndex].x
			local dy = nodeList[nodeInd].y - nodeList[lastIndex].y
			local dz = nodeList[nodeInd].z - nodeList[lastIndex].z
			local d = math.sqrt(dx^2 + dy^2 + dz^2)

			if not self:_check_collision_extend(nodeList[lastIndex].x, nodeList[lastIndex].y,nodeList[lastIndex].z, nodeList[nodeInd].x,nodeList[nodeInd].y,nodeList[nodeInd].z, d) then
				table.remove(nodeList.children, nodeInd)					
				self:_remove_branch(nodeInd) 
			end
		end
	end
end

function rrt:_remove_branch(nodeInd)
	for ix in self.nodelist[nodeInd].children do
		self:_remove_branch(ix)
	end

	table.remove(self.nodeList, nodeInd)
end

function rrt:_choose_parent(newNode, nearinds)
	if #nearinds == 0 then
		return newNode
	end

	local dlist = {}

	for _,i in pairs(nearinds) do
		local dx = newNode.x - self.nodeList[i].x
		local dy = newNode.y - self.nodeList[i].y
		local dz = newNode.z - self.nodeList[i].z
		local d = math.sqrt(dx^2 + dy^2 + dz^2)

		if self:_check_collision_extend(self.nodeList[i].x, self.nodeList[i].y, self.nodeList[i].z, newNode.x, newNode.y, newNode.z, d) then
			table.insert(dlist,(#dlist) + 1,self.nodeList[i].cost + d)
		else
			table.insert(dlist,(#dlist) + 1,"inf")
		end
	end

	local mincost = math.min(table.unpack(dlist))
	local minind = nearinds[table.find(dlist,mincost)]

	if mincost == math.huge then
		print("mincost is inf")

		return newNode
	end

	newNode.cost = mincost
	newNode.parent = minind

	return newNode
end

function rrt:_heuristic_random_point()
	local rnd
	if math.random(0, 100) > self.goalSampleRate then
		local start = Vector3.new(self.start.x,self.start.y,self.start.z)
		local endpoint = Vector3.new(self.endpoint.x,self.endpoint.y,self.endpoint.z)

		local MajorAxis = ShortestPath[1]
		local FocalLength = (math.sqrt(((start.X-endpoint.X)^2)+((start.Y-endpoint.Y)^2)+((start.Z-endpoint.Z)^2))/2)
		local MinorAxis = math.sqrt(((MajorAxis/2)^2)-FocalLength^2)
		local Center = start+(endpoint-start)*.5

		local xa = math.random(-(MajorAxis/2),MajorAxis/2)
		local intersept = (math.abs(MinorAxis*2)/math.abs(MajorAxis))*math.sqrt(((MajorAxis/2)^2)-xa^2)
		local randomintersepty = math.random(-intersept/2,intersept/2)
		local Z1 = math.abs(MinorAxis/2)*math.sqrt(1-xa^2/(MajorAxis/2)^2-randomintersepty^2/(MinorAxis/2)^2)
		local randominterseptz = math.random(-Z1,Z1)

		local randomposition = (CFrame.new(Center,start) * CFrame.new(randominterseptz,randomintersepty,xa)).Position


		--[[local prt = Instance.new("Part")
		prt.Parent = workspace.Heuristic
		prt.Locked = true
		prt.Anchored = true
		prt.CanCollide = false
		prt.Size = Vector3.new(2,2,2)
		prt.Color = Color3.fromRGB(234, 0, 255)
		prt.Position = Vector3.new(randomposition.X,randomposition.Y,randomposition.Z)/3
		prt.Shape = Enum.PartType.Ball]]
		rnd = {randomposition.X,randomposition.Y,randomposition.Z}
	else
		rnd = {self.endpoint.x, self.endpoint.y, self.endpoint.z}
	end
	return rnd
end


function rrt:_len(t)
	local n = 0

	for _,v in pairs(t) do
		n = n + 1
	end
	return n
end

function rrt:_steer(rnd, nind)
	local nearestNode = self.nodeList[nind]

	local dx = rnd[1] - nearestNode.x
	local dy = rnd[2] - nearestNode.y
	local dz = rnd[3] - nearestNode.z
	local d = math.sqrt(dx^2 + dy^2 + dz^2)

	local newNode = Node.new(nearestNode.x, nearestNode.y, nearestNode.z)
	newNode.x = nearestNode.x + (rnd[1]-nearestNode.x) * (self.expandDis/d)
	newNode.y = nearestNode.y + (rnd[2]-nearestNode.y) * (self.expandDis/d)
	newNode.z = nearestNode.z + (rnd[3]-nearestNode.z) * (self.expandDis/d)

	newNode.cost = nearestNode.cost + self.expandDis
	newNode.parent = nind
	return newNode
end


function rrt:_get_random_point()
	local rnd
	if math.random(0,100) > self.goalSampleRate then
		rnd = {math.random(0, self.Xrand), math.random(0, self.Yrand), math.random(0, self.Zrand)}
	else
		rnd = {self.endpoint.x, self.endpoint.y, self.endpoint.z}
	end

	return rnd
end

function rrt:_get_best_last_index(nodelist,endpoint,expandDis)
	local goalinds = {}

	for key, node in pairs(nodelist) do
		local equation = math.sqrt(((node.x-endpoint.x)^2) + ((node.y-endpoint.y) ^2) + ((node.z-endpoint.z)^2))
		if equation <= expandDis then
			table.insert(goalinds,(#goalinds)+1,key)
		end
	end

	if rrt:_len(goalinds) == 0 then
		return nil
	end

	for _,i in pairs(goalinds) do
		if nodelist[i].cost == math.min(nodelist[i].cost) then
			return i
		end
	end

	return nil
end

function rrt:_calc_dist_to_goal(a,b)
	return math.sqrt(a^2 + b^2)
end


function rrt:_find_near_nodes(newNode, value)
	local r = self.expandDis * value

	local dlist = {}
	local nearinds = {}

	for Index,Child in pairs(self.nodeList) do
		if (((Child.x - newNode.x)^2)+((Child.y - newNode.y)^2)+((Child.z - newNode.z)^2)) <= r^2 then
			table.insert(dlist,Index,Child)
		end
	end

	for Index,_ in pairs(self.nodeList) do
		if dlist[Index] ~= nil then
			table.insert(nearinds,(#nearinds)+1,Index)
		end
	end

	return nearinds
end

function rrt:_rewire(newNodeInd, newNode, nearinds)
	local nnode = rrt:_len(self.nodeList)
	for set,element in pairs(nearinds) do
		local nearNode = self.nodeList[element]

		local dx = newNode.x - nearNode.x
		local dy = newNode.y - nearNode.y
		local dz = newNode.z - nearNode.z
		local d = math.sqrt(dx^2 + dy^2 + dz^2)

		local scost = newNode.cost + d

		if nearNode.cost > scost then
			if self:_check_collision_extend(nearNode.x, nearNode.y, nearNode.z, newNode.x, newNode.y, newNode.z, d) then
				table.remove(self.nodeList[nearNode.parent].children,table.find(self.nodeList[nearNode.parent].children,element))

				nearNode.parent = newNodeInd
				nearNode.cost = scost

				table.insert(newNode.children,(#newNode.children) +1,element)

			end
		end
	end
end

--This needs to be rewritten

function rrt:_check_collision_extend(nix, niy, niz, ix, iy, iz, d)
	local rayOrigin = Vector3.new(nix,niy,niz)/3
	local second = Vector3.new(ix,iy,iz)/3
	local rayDirection = CFrame.lookAt(rayOrigin,second).LookVector*(d/3)

	-- Build a "RaycastParams" object and cast the ray
	local raycastParams = RaycastParams.new()
	raycastParams.FilterDescendantsInstances = {workspace.Collisions}
	raycastParams.FilterType = Enum.RaycastFilterType.Whitelist
	local raycastResult = workspace:Raycast(rayOrigin, rayDirection, raycastParams)

	if raycastResult ~= nil then
		return false
	else
		return true
	end
end

function rrt:_GetNearestListIndex(rnd) 
	local dlist = {}
	local dlist2 = {}
	local nearinds = {}

	for Index,Child in pairs(self.nodeList) do
		table.insert(dlist,(#dlist)+1,{(Child.x - rnd[1])^2,(Child.y - rnd[2])^2,(Child.z - rnd[3])^2})
	end

	for Index,Child in pairs(dlist) do
		table.insert(dlist2,Index,Child[1]+Child[2]+Child[3])
	end

	for Index,Child in pairs(self.nodeList) do
		table.insert(nearinds,(#nearinds)+1,Index)
	end

	return nearinds[table.find(dlist2,math.min(table.unpack(dlist2)))]
end


function Node.new(x,y,z)
	local self = setmetatable({}, Node)

	self.x = x
	self.y = y 
	self.z = z
	self.cost = 0.0
	self.parent = nil
	self.children = {}

	return self 
end

return rrt
