Upload

lua/niknaks/modules/sh_pathfind_module.lua

@@ -0,0 +1,438 @@
+--[[
+| This file was obtained through the combined efforts
+| of Madbluntz & Plymouth Antiquarian Society.
+|
+| Credits: lifestorm, Gregory Wayne Rossel JR.,
+| 	Maloy, DrPepper10 @ RIP, Atle!
+|
+| Visit for more: https://plymouth.thetwilightzone.ru/
+--]]
+
+-- Copyright © 2022-2072, Nak, https://steamcommunity.com/id/Nak2/
+-- All Rights Reserved. Not allowed to be reuploaded.
+local NikNaks = NikNaks
+local CurTime, setmetatable, IsValid = CurTime, setmetatable, IsValid
+local min, max, cos = math.min, math.max, math.cos
+
+--- @class LPathFollower
+--- @field _segments LPathFollowerSegment[]
+local meta = {}
+meta.__index = meta
+meta.__tostring = function( self )
+	return "LPathFollower Age: " .. self:GetAge()
+end
+NikNaks.__metatables["LPathFollower"] = meta
+
+--[[
+	t._segments = {}
+	t._entity = nil		-- Overrides the last position in the segments
+	t._age = CurTime()
+	
+]]
+--- Creates an empty path-follower
+--- @param start_pos  Vector
+--- @return LPathFollower
+function meta.CreatePathFollower( start_pos )
+	--- @class LPathFollower
+	local t = {}
+	t._segments = {}
+	t._start = start_pos
+	t._length = 0
+	t._age = CurTime()
+	t._valid = true
+	t._cursor = 1
+	t._cursor_dis = 0
+	t._cursor_lef = 0
+
+	return setmetatable( t, meta )
+end
+
+--- Adds a segment.
+--- @param from Vector
+--- @param to Vector
+--- @param curvature number
+--- @param move_type number
+--- @return LPathFollowerSegment
+function meta:AddSegment( from, to, curvature, move_type )
+	--- @class LPathFollowerSegment
+	local seg = {}
+		seg.curvature = curvature or 0
+		seg.move_type = move_type or 1
+		seg.distanceFromStart = from:Distance( to )
+
+		local n = ( to - from ):GetNormalized()
+		seg.forward = n
+		seg.yaw = ( -n ):Angle().y
+
+		if n.z > 0.7 or n.z < -0.7 then
+			self.how = 9
+		else
+			if n.y < -0.5 then -- North
+				seg.how = 0
+			elseif n.y > 0.5 then -- South
+				seg.how = 2
+			elseif n.x > 0.5 then -- East
+				seg.how = 1
+			else				-- West
+				seg.how = 3
+			end
+		end
+
+		seg.length = from:Distance( to )
+		seg.s_lengh = self._length
+		seg.pos = to
+		--seg.ladder
+		--seg.node
+		--seg.area
+		--seg.nna
+
+	self._length = self._length + seg.length
+	self._segments[#self._segments + 1] = seg
+	return seg
+end
+
+-- Default easy functions
+do
+	--- Returns the length of the path.
+	function meta:GetLength()
+		return self._length or 0
+	end
+
+	--- Returns the first segment.
+	function meta:FirstSegment()
+		return self._segments[1]
+	end
+
+	--- Returns the last segment.
+	function meta:LastSegment()
+		return self._segments[#self._segments]
+	end
+
+	--- Returns all segments.
+	function meta:GetAllSegments()
+		return self._segments
+	end
+
+	--- Returns the age of the path.
+	function meta:GetAge()
+		return CurTime() - self._age
+	end
+
+	--- Resets the age of the path.
+	function meta:ResetAge()
+		self._age = CurTime()
+	end
+
+	--- Returns true if the path is valid.
+	function meta:IsValid()
+		return self._valid or false
+	end
+
+	--- Invalidates the path.
+	function meta:Invalidate()
+		self._valid = false
+	end
+
+	--- Returns the starting position of the path.
+	function meta:GetStart()
+		return self._start
+	end
+
+	--- Returns the ending position of the path (Note, will update if the target is an entity).
+	function meta:GetEnd()
+		if IsValid( self.target_ent ) then
+			return self.target_ent:GetPos()
+		end
+
+		return self._segments[#self._segments].pos
+	end
+end
+
+-- Cursor
+local findClosestSeg
+do
+	--- Returns the cursor position.
+	--- @return number
+	--- @return number
+	local function findCursor( self, distance )
+		local q = self._segments
+		distance = min( self._length, distance )
+
+		for i = 1, #q do
+			if distance <= q[i].s_lengh + q[i].length then
+				return i, distance - ( q[i].s_lengh + q[i].length )
+			end
+		end
+
+		return 1, distance
+	end
+
+	--- Returns the closest segment and its index.
+	--- @param position Vector
+	--- @return LPathFollowerSegment
+	--- @return number
+	findClosestSeg = function( self, position )
+		local c, d, q
+
+		for i, seg in pairs( self._segments ) do
+			local dis = seg.pos:DistToSqr( position )
+			if not c or c > dis then
+				c = dis
+				d = seg
+				q = i
+			end
+		end
+
+		return d, q
+	end
+
+	--- Returns the closest position along the path to said position.
+	--- @param position Vector
+	--- @return Vector
+	local function findClosestBetween( A, dir, position, maxLength )
+		local v = position - A
+		local d = v:Dot( dir )
+		return A + dir * max( 0, min( d, maxLength ) )
+	end
+
+	--- Returns the position on the path by given distance
+	--- @param distance number
+	--- @return Vector
+	function meta:GetPositionOnPath( distance )
+		local seg_id, lef = findCursor( self, distance )
+		local t = self._segments
+		local seg = t[seg_id]
+		return seg.pos + seg.forward * lef
+	end
+
+	--- Returns the closest position along the path to said position.
+	--- @param position Vector
+	--- @return Vector
+	function meta:GetClosestPosition( position )
+		-- Locate the closest points
+		local seg, seg_id = findClosestSeg( self, position )
+		if not seg then return self:GetStart() end -- Fallback to the start pos
+
+		local max_seg = #self._segments
+
+		if seg_id <= 1 then
+			local n_seg = self._segments[seg_id + 1]
+			local v1 = findClosestBetween( self:GetStart(), seg.forward, position, seg.length )
+			local v2 = findClosestBetween( seg.pos, n_seg.forward, position, n_seg.length )
+			if v1:DistToSqr( position ) > v2:DistToSqr( position ) then
+				return v2
+			else
+				return v1
+			end
+		elseif seg_id >= max_seg then
+			local s_seg = self._segments[max_seg - 1]
+			return findClosestBetween( s_seg.pos, seg.forward, position, seg.length )
+		else
+			local p_seg = self._segments[seg_id - 1]
+			local n_seg = self._segments[seg_id + 1]
+			local v1 = findClosestBetween( p_seg.pos, seg.forward, position, seg.length )
+			local v2 = findClosestBetween( seg.pos, n_seg.forward, position, n_seg.length )
+			if v1:DistToSqr( position ) > v2:DistToSqr( position ) then
+				return v2
+			else
+				return v1
+			end
+		end
+	end
+
+	--- Moves the cursor to the start of the path.
+	function meta:MoveCursorToStart()
+		self._cursor_dis = 0
+		self._cursor_lef = 0
+		self._cursor = 1
+	end
+
+	--- Moves the cursor to the end of the path.
+	function meta:MoveCursorToEnd()
+		self._cursor_dis = self:GetLength()
+		self._cursor = #self._segments
+		self._cursor_lef = self._segments[self._cursor].length
+	end
+
+	--- Returns the cursor progress along the path
+	--- @return number
+	function meta:GetCursorPosition()
+		return self._cursor_dis
+	end
+
+	--- Moves the cursor to said distance.
+	--- @param distance number
+	function meta:MoveCursorTo( distance )
+		self._cursor_dis = distance
+		local seg_id, lef = findCursor( self, distance )
+		self._cursor = seg_id
+		self._cursor_lef = lef
+	end
+
+	--- Moves the cursor said distance
+	--- @param distance number
+	function meta:MoveCursor( distance )
+		self._cursor_dis = self._cursor_dis + distance
+		local nd = self._cursor_lef + distance
+		local seg = self._segments[self._cursor]
+		if nd < 0 or nd > seg.length then -- New segment
+			local seg_id, lef = findCursor( self, self._cursor_dis )
+			self._cursor = seg_id
+			self._cursor_lef = lef
+		else
+			self._cursor_lef = nd
+		end
+	end
+
+	--- Returns the closest sequence.
+	--- @param position Vector
+	--- @return LPathFollowerSegment
+	--- @return number
+	function meta:FindClosestSeg( position )
+		return findClosestSeg( self, position )
+	end
+
+	--- Returns the distance from the path.
+	--- @param position Vector
+	--- @return number
+	function meta:FindDistanceFromPath( position )
+		return self:GetClosestPosition( position ):Distance( position )
+	end
+
+end
+
+-- NPC stuff
+do
+	--[[
+		By default in Gmod, you're reuired to create a pathfind object, and then compute it.
+		However I've found many situations where I could reuse a path. I.e a group of NPC's.
+		So to stick to the closest "Gmod way", path:Update can be called on multiple entities and got a goal argument.
+
+		C function here: https://github.com/Joshua-Ashton/Source-PlusPlus/blob/4056819cea889d73626a1cbc09518b2f8ba5dda4/src/game/shared/cstrike/bot/nav_path.cpp#L472 
+	]]
+	function meta:Update( ent, toleranceSqrt )
+		-- Make sure it is a valid entity and it has loco
+		if not IsValid( ent ) or not ent.loco then return true end
+
+		local entPos = ent:GetPos()
+		local seq, id = nil, ent._cursor
+
+		-- Located the closest segment, and use that
+		if not id then
+			seq, id = findClosestSeg( self, entPos )
+			ent._cursor = id
+		else
+			seq = self._segments[id]
+		end
+
+		if not seq then return true end -- No segment, we must have reached the end
+
+		local goal = seq.pos
+		if goal:DistToSqr( entPos ) < toleranceSqrt then
+			if id >= #self._segments then return true end -- Reached the end
+			id = id + 1
+			seq = self._segments[id]
+			goal = seq.pos
+			ent._cursor = ent._cursor + 1
+		end
+
+		ent.loco:Approach( goal, 1 )
+		ent.loco:FaceTowards( goal )
+		ent:SetAngles( Angle( 0, ( ent:GetPos() - goal ):Angle().y, 0 ) )
+	end
+
+end
+
+-- NET
+do
+	function NikNaks.net.WritePath( path )
+		local n = #path._segments
+		net.WriteUInt( n, 16 )
+		net.WriteFloat( path._age )
+		net.WriteVector( path._start )
+
+		for i = 1, n do
+			local seg = path._segments[i]
+			net.WriteVector( seg.pos - seg.length * seg.forward )
+			net.WriteVector( seg.pos )
+			net.WriteFloat( seg.curvature )
+			net.WriteUInt( seg.move_type, 8 )
+		end
+	end
+
+	function NikNaks.net.ReadPath()
+		local n = net.ReadUInt( 16 )
+		local age = net.ReadFloat()
+		local path = meta.CreatePathFollower( net.ReadVector() )
+		path._age = age
+
+		for _ = 1, n do
+			path:AddSegment( net.ReadVector(), net.ReadVector(), net.ReadFloat(), net.ReadUInt( 8 ) )
+		end
+
+		return path
+	end
+end
+
+-- Debug
+do
+	local mat_goal = Material("editor/assault_rally")
+	local mat_start = Material("effects/powerup_agility_hud")
+
+	local tMat = Material("effects/bluelaser1")
+	local m = Material("hud/arrow_big")
+
+	local point = Material("hud/freezecam_callout_arrow")
+	local cir = Material("hud/cart_point_neutral_opaque")
+	local mat_arrow = Material("vgui/glyph_expand")
+
+	local mat_solider = Material("hud/bomb_carried")
+	local jump_man = Material("hud/death_wheel_1")
+
+	local col_walk = color_white
+	local col_climb = Color(155,55,155)
+	local col_fly = Color(55,55,255)
+	local col_jump = Color(125, 55, 255)
+
+	local mov = bit.bor(NikNaks.CAP_MOVE_GROUND, NikNaks.CAP_MOVE_CLIMB, NikNaks.CAP_MOVE_JUMP)
+	local point_b = Material("hud/cart_point_blue")
+	local point_c = Material("hud/expanding_vert_middle_blue_bg")
+
+	function meta:DebugRender()
+		local l
+		for i, seg in ipairs(self:GetAllSegments()) do
+			render.SetMaterial(cir)
+			render.DrawSprite( seg.pos, 16, 16 )
+			if l then
+				local col = color_white
+				if seg.move_type == NikNaks.CAP_MOVE_CLIMB then
+					col = col_climb
+				elseif seg.move_type == NikNaks.CAP_MOVE_FLY then
+					col = col_fly
+				elseif seg.move_type == NikNaks.CAP_MOVE_JUMP then
+					col = col_jump
+				end
+				local d = seg.pos:Distance(l)
+				local n = (SysTime() * 2) % 1
+				m:SetVector("$color",Vector(col.r,col.g,col.b) / 255)
+				render.SetMaterial(m)
+				render.DrawBeam( seg.pos, l, 30, n, d / 40 + n, col )
+				render.SetMaterial(point)
+				render.DrawBeam( l + seg.forward * 10,l + seg.forward * 30, 20, 0, 1)
+			end
+			l = seg.pos
+		end
+
+		local num = max(1, self:GetLength() / 800)
+		for i = 1, num do
+			local dis = (CurTime() * 100 + i * 800) % self:GetLength()
+			local pos = self:GetPositionOnPath( dis )
+			local q = 1 + cos(SysTime() * 10 + i) * 0.1
+			render.SetMaterial(mat_arrow)
+			render.DrawBeam( pos + Vector(0,0,16), pos, 16, q - 1, q, color_white )
+			render.SetMaterial(mat_solider)
+			render.DrawSprite( pos + Vector(0,0,20), 16, 16 )
+		end
+	end
+end
+