-- This Source Code Form is subject to the terms of the bCDDL, v. 1.1.
-- If a copy of the bCDDL was not distributed with this
-- file, You can obtain one at http://beamng.com/bCDDL-1.1.txt
local M = {}
local E = setmetatable({}, {__newindex = function(t, key, val) log('E', 'ai.lua', 'Tried to insert new elements into token empty table') end})
M.mode = 'disabled' -- this is the main mode
M.manualTargetName = nil
M.debugMode = 'off'
M.speedMode = nil
M.routeSpeed = nil
M.extAggression = 0.3
M.cutOffDrivability = 0
M.driveInLaneFlag = 'off'
M.extAvoidCars = 'auto'
-- [[ STORE FREQUENTLY USED FUNCTIONS IN UPVALUES ]] --
local buffer = require("string.buffer")
local max = math.max
local min = math.min
local sin = math.sin
local asin = math.asin
local pi = math.pi
local abs = math.abs
local sqrt = math.sqrt
local floor = math.floor
local tableInsert = table.insert
local tableRemove = table.remove
local tableConcat = table.concat
local strFormat = string.format
-- [[ HEAVY DEBUG MODE ]] --
local visDebug = {trajecRec = {last = 0}, routeRec = {last = 0}, labelRenderDistance = 10, debugSpots = {}, candidatePaths = nil}
--local newPositionsDebug = {} -- for debug purposes
-- [[ Simulation time step]] --
local dt
-- [[ ENVIRONMENT VARIABLES ]] --
local g = obj:getGravity() -- standard gravity is negative
local gravityDir = vec3(0, 0, sign2(g))
g = max(1e-30, abs(g)) -- prevent divivion by 0 gravity
-- [[ PERFORMANCE RELATED ]] --
local aggression = 1
-- [[ AI DATA: POSITION, CONTROL, STATE ]] --
local ego = {
pos = obj:getFrontPosition(),
dirVec = obj:getDirectionVector(),
prevDirVec = obj:getDirectionVector(),
upVec = obj:getDirectionVectorUp(),
rightVec = vec3(),
width = nil,
length = nil,
wheelBase = nil,
currentSegment = {},
vel = vec3(obj:getSmoothRefVelocityXYZ()),
speed = vec3(obj:getSmoothRefVelocityXYZ()):length(),
staticFrictionCoef = 1,
ghostR = nil,
ghostL = nil,
}
ego.race = {
dT_min = 0.1,
dT_max = 0.3,
Td = 0.1,
Tp = 0.01,
d = 4, --distance thredshold, unless my distance from vehicle ahead is greater than this, I'll want to go faster than him
kp = 0.6
}
local targetSpeedDifSmoother = newTemporalSmoothingNonLinear(1e300, 4, 0)
local targetSpeedSmoother = newTemporalSmoothingNonLinear(10, 10, ego.speed)
local egoDeviationSmoother = newTemporalSmoothing(1)
local smoothTcs = newTemporalSmoothingNonLinear(0.1, 0.9)
local throttleSmoother = newTemporalSmoothing(1e30, 0.2)
local internalState = {
changePlanTimer = 0,
egoCannotMoveTime = 0,
egoForceGoFrontTime = 0,
road = 'onroad',
crash = {time = 0, manoeuvre = 0, dir = nil, pos = nil},
chaseData = {playerState = nil, playerStoppedTimer = 0, playerRoad = nil, driveAhead = false, targetSpeed = nil}
}
local twt = {
state = 0,
dirState = {0, 0}, -- direction, steer
posTable = {vec3(), vec3(), vec3(), vec3()},
dirTable = {vec3(), vec3(), vec3(), vec3()},
minRay = "",
rayMins = {math.huge, math.huge, math.huge, math.huge, math.huge, math.huge, math.huge, math.huge}, --clockwise beginning from corner front left
minRayCoefs = {0, 0, 0, 0, 0, 0, 0, 0}, -- clockwise as above
blueNoiseCoefs = {0, 0, 0, 0, 0, 0, 0, 0},
biasedCoefs = {0, 0, 0, 0, 0, 0, 0, 0},
blueNoiseRR = 0,
sampleCounter = 0, -- for starting scan, to give some time for precision
targetSpeed = 0,
OBBinRange = {},
RRT = {1,1,1,2,1,2,2,2,2,2,2,3,2,3,3,3,3,3,3,4,3,4,4,4,4,4,4,1,4,1,1,1},
speedSmoother = newTemporalSmoothingNonLinear(1000, 0.25),
steerSmoother = newTemporalSmoothingNonLinear(2)
}
twt.reset = function()
twt.state = 0
twt.dirState[1], twt.dirState[2] = 0, 0
twt.minRay = nil
for i = 1, #twt.rayMins do twt.rayMins[i] = math.huge end
for i = 1, #twt.blueNoiseCoefs do twt.blueNoiseCoefs[i] = 0 end
twt.sampleCounter = 0
twt.targetSpeed = 0
twt.speedSmoother:set(0)
twt.steerSmoother:set(0)
end
local forces = {}
local velocities = {}
local lastCommand = {steering = 0, throttle = 0, brake = 0, parkingbrake = 0}
local opt = {driveInLaneFlag = false, racing = false, trajMethod = nil, aggressionMode = nil, avoidCars = 'on'}
-- [[ TRAFFIC ]] --
local trafficStates = {}
local trafficPathState = {}
local traffic = {
trafficTable = {},
trafficFilterQue = {array = {}, dict = {}},
trafficFilterFromQue = {},
frameQue = 0,
rateQue = 0, -- 1 means adding a que vehicle every second
filtered = true, -- TrafficFilter on or off (true/false)
intersection = true, -- check for intersection between ego and other vehicle
vdraw = false, -- draw debuf spheres to show traffic vehicles in trafficTable
Rfl = 60, -- larger radius for traffic filter queue
Rfl_fun = function(v) return clamp((v+10)*3, 60, 200) end, --
Rfs = 60, -- smaller radius for traffic table
Rfs_fun = function(v) return clamp(v*3, 60, 100) end,
distAhead = 40 -- minimum Ahead distance for searching traffic vehicle
}
-- [[ OPPONENT DATA ]] --
local player
-- [[ SETTINGS, PARAMETERS, AUXILIARY DATA ]] --
local mapData -- map data including node connections (edges and edge data), node positions and node radii
local signalsData -- traffic intersection and signals data
local currentRoute
local MIN_PLAN_COUNT = 3
local targetWPName
local edgeDict
local wpList
local manualPath
local validateInput = nop
local speedProfile
local loopPath, noOfLaps
local parameters
local restoreGearboxMode = false
local targetObjectSelectionMode
local scriptData
local scriptai = nil
local recover = {recoverOnCrash = false, recoverTimer = 0, _recoverOnCrash = false}
------------------------------
-- For Debugging (useful function to print a numbered list of upvalues of a specific function)
local function inspect_upvalues(func)
local i = 1
while true do
local name, value = debug.getupvalue(func, i)
if not name then break end
print(string.format("Upvalue %d: %s = %s", i, name, tostring(value)))
i = i + 1
end
end
local dataLogger = {logData = nop}
local function logDataTocsv()
if not dataLogger.csvFile then
print('Started Logging Data')
dataLogger.csvFile = require('csvlib').newCSV("time", "posX", "posY", "posZ", "speed", "targetSpeed", "ax", "ay", "az", "vx", "vy", "vz", "throttle", "brake", "steering", "staticFrictionCoef")
dataLogger.time = 0
else
dataLogger.time = dataLogger.time + dt
end
dataLogger.csvFile:add(
dataLogger.time,
ego.pos.x,
ego.pos.y,
ego.pos.z,
ego.speed,
((currentRoute or E).plan or E).targetSpeed,
-sensors.gy,
sensors.gx,
sensors.gz,
ego.vel:dot(ego.dirVec),
-(ego.vel:dot(ego.rightVec)),
ego.vel:dot(ego.upVec),
lastCommand.throttle * 100,
lastCommand.brake * 100,
lastCommand.steering * 100,
ego.staticFrictionCoef)
end
local function writeCsvFile(name)
if dataLogger.csvFile then
print('Writing Data to CSV.')
dataLogger.csvFile:write(name)
dataLogger.csvFile = nil
dataLogger.time = nil
print('Done')
else
print('No data to write')
end
end
local function startStopDataLog(name)
if dataLogger.logData == nop then
print('Initialized Data Log')
dataLogger.logData = logDataTocsv
dataLogger.name = name
else
print('Stopped Logging Data')
dataLogger.logData = nop
writeCsvFile(dataLogger.name or name)
dataLogger.name = nil
end
end
local function getObjectBoundingBox(id)
local x = obj:getObjectDirectionVector(id)
local z = obj:getObjectDirectionVectorUp(id)
local y = z:cross(x)
y:setScaled(obj:getObjectInitialWidth(id) * 0.5 / max(y:length(), 1e-30))
x:setScaled(obj:getObjectInitialLength(id) * 0.5)
z:setScaled(obj:getObjectInitialHeight(id) * 0.5)
return obj:getObjectCenterPosition(id), x, y, z
end
local function drawOBB(c, x, y, z, col)
-- c: center point
-- x: front half extent vec
-- y: left half extent vec
-- z: up half extent vec
local debugDrawer = obj.debugDrawProxy
col = col or color(255, 0, 0, 255)
local p1 = c - x + y - z -- RLD
local p2 = c - x - y - z -- RRD
local p3 = c - x - y + z -- RRU
local p4 = c - x + y + z -- RLU
local p5 = c + x + y - z -- FLD
local p6 = c + x - y - z -- FRD
local p7 = c + x - y + z -- FRU
local p8 = c + x + y + z -- FLU
-- rear face
debugDrawer:drawCylinder(p1, p2, 0.02, col)
debugDrawer:drawCylinder(p2, p3, 0.02, col)
debugDrawer:drawCylinder(p3, p4, 0.02, col)
debugDrawer:drawCylinder(p4, p1, 0.02, col)
-- front face
debugDrawer:drawCylinder(p5, p6, 0.02, col)
debugDrawer:drawCylinder(p6, p7, 0.02, col)
debugDrawer:drawCylinder(p7, p8, 0.02, col)
debugDrawer:drawCylinder(p8, p5, 0.02, col)
-- left face
debugDrawer:drawCylinder(p1, p5, 0.02, col)
debugDrawer:drawCylinder(p4, p8, 0.02, col)
-- right face
debugDrawer:drawCylinder(p2, p6, 0.02, col)
debugDrawer:drawCylinder(p3, p7, 0.02, col)
end
local function drawVehicleBoundingBox(vid, col)
local c, vx, vy, vz = getObjectBoundingBox(vid)
drawOBB(c, vx, vy, vz, col)
end
local function aistatus(status, category)
guihooks.trigger("AIStatusChange", {status=status, category=category})
end
local function getState()
return M
end
local function stateChanged()
if playerInfo.anyPlayerSeated then
guihooks.trigger("AIStateChange", getState())
end
end
local function setSpeed(speed)
if type(speed) ~= 'number' then M.routeSpeed = nil else M.routeSpeed = speed end
end
local function setSpeedMode(speedMode)
if speedMode == 'set' or speedMode == 'limit' or speedMode == 'legal' or speedMode == 'off' then
M.speedMode = speedMode
else
M.speedMode = nil
end
end
local function resetSpeedModeAndValue()
M.speedMode = nil -- maybe this should be 'off'
M.routeSpeed = nil
end
local function setAggressionInternal(v)
aggression = v or M.extAggression
end
local function setAggressionExternal(v)
M.extAggression = v or M.extAggression
setAggressionInternal()
stateChanged()
end
local function setAggressionMode(aggrmode)
if aggrmode == 'rubberBand' then
opt.aggressionMode = aggrmode
else
opt.aggressionMode = nil
setAggressionInternal()
end
end
local function resetAggression()
setAggressionInternal()
end
local function resetInternalStates()
trafficStates.block = {timer = 0, coef = 0, timerLimit = 6, block = false}
trafficStates.side = {timer = 0, cTimer = 0, side = 1, displacement = 0, timerLimit = 6}
trafficStates.action = {hornTimer = -1, hornTimerLimit = 1, forcedStop = false}
trafficStates.intersection = {timer = 0, turn = 0, block = false}
internalState.chaseData.playerState = nil
internalState.chaseData.playerStoppedTimer = 0
internalState.chaseData.playerRoad = nil
internalState.chaseData.driveAhead = false
internalState.chaseData.targetSpeed = nil
internalState.crash.time = 0
internalState.crash.manoeuvre = 0
internalState.crash.dir = nil
internalState.crash.pos = nil
if electrics.values.horn == 1 then electrics.horn(false) end
if electrics.values.signal_left_input or electrics.values.signal_right_input then electrics.set_warn_signal(false) end
trafficPathState = {}
end
resetInternalStates()
local function resetParameters()
-- parameters are used for finer AI control
parameters = {
turnForceCoef = 2, -- coefficient for curve spring forces
awarenessForceCoef = 0.25, -- coefficient for vehicle awareness displacement
edgeDist = 0, -- minimum distance from the edge of the road
trafficWaitTime = 2, -- traffic delay after stopping at intersection
delayFactorWaitTime = {0.55 + (0.95 - 0.55) * math.random() , 0.05 + (0.45 - 0.05) * math.random()},
enableElectrics = true, -- use electrics such as hazard lights (especially for traffic)
driveStyle = 'default',
staticFrictionCoefMult = 0.95,
lookAheadKv = 0.6,
applyWidthMarginOffset = true,
planErrorSmoothing = true,
springForceIntegratorDispLim = 0.1, -- node displacement force magnitude limit
understeerThrottleControl = 'on', -- anything other than an 'off' value will keep this active
oversteerThrottleControl = 'on', -- anything other than an 'off' value will keep this active
throttleTcs = 'on', -- anything other than an 'off' value will keep this active
abBrakeControl = 'on',
underSteerBrakeControl = 'on',
throttleKp = 0.5,
targetSpeedSmootherRate = 10,
}
end
resetParameters()
local function setParameters(data)
tableMerge(parameters, data)
end
local function dumpParameters()
dump(parameters)
dump('aggression = ', aggression)
end
M.dumpParameters = dumpParameters
local function setTargetObjectID(id)
M.targetObjectID = M.targetObjectID ~= objectId and id or -1
if M.targetObjectID ~= -1 then targetObjectSelectionMode = 'manual' end
end
local function calculateWheelBase()
local avgWheelNodePos, numOfWheels = vec3(), 0
for _, wheel in pairs(wheels.wheels) do
-- obj:getNodePosition is the pos vector of query node (wheel.node1) relative to ref node in world coordinates
avgWheelNodePos:setAdd(obj:getNodePosition(wheel.node1))
numOfWheels = numOfWheels + 1
end
if numOfWheels == 0 then return 0 end
avgWheelNodePos:setScaled(1 / numOfWheels)
local dirVec = obj:getDirectionVector()
local avgFrontWheelPos, frontWheelCount = vec3(), 0
local avgBackWheelPos, backWheelCount = vec3(), 0
for _, wheel in pairs(wheels.wheels) do
local wheelPos = obj:getNodePosition(wheel.node1)
if wheelPos:dot(dirVec) > avgWheelNodePos:dot(dirVec) then
avgFrontWheelPos:setAdd(wheelPos)
frontWheelCount = frontWheelCount + 1
else
avgBackWheelPos:setAdd(wheelPos)
backWheelCount = backWheelCount + 1
end
end
avgFrontWheelPos:setScaled(1 / frontWheelCount)
avgBackWheelPos:setScaled(1 / backWheelCount)
return avgFrontWheelPos:distance(avgBackWheelPos)
end
local function updatePlayerData()
mapmgr.getObjects()
if not next(mapmgr.objects) then return end -- prevents changes if table is empty
if mapmgr.objects[M.targetObjectID] and targetObjectSelectionMode == 'manual' then
player = mapmgr.objects[M.targetObjectID]
elseif tableSize(mapmgr.objects) == 2 then
if player ~= nil then
player = mapmgr.objects[player.id]
else
for k, v in pairs(mapmgr.objects) do
if k ~= objectId then
M.targetObjectID = k
player = v
break
end
end
targetObjectSelectionMode = 'auto'
end
else
if player ~= nil and player.active == true then
player = mapmgr.objects[player.id]
else
for k, v in pairs(mapmgr.objects) do
if k ~= objectId and v.active == true then
M.targetObjectID = k
player = v
break
end
end
targetObjectSelectionMode = 'targetActive'
end
end
end
local function driveCar(steering, throttle, brake, parkingbrake)
input.event("steering", steering, "FILTER_AI", nil, nil, nil, "ai")
input.event("throttle", throttle, "FILTER_AI", nil, nil, nil, "ai")
input.event("brake", brake, "FILTER_AI", nil, nil, nil, "ai")
input.event("parkingbrake", parkingbrake, "FILTER_AI", nil, nil, nil, "ai")
lastCommand.steering = steering
lastCommand.throttle = throttle
lastCommand.brake = brake
lastCommand.parkingbrake = parkingbrake
end
local function populateOBBinRange(range)
range = range * range
local i = 0
for id in pairs(mapmgr.getObjects()) do
if id ~= objectId then
local pos = obj:getObjectCenterPosition(id)
if pos:squaredDistance(ego.pos) < range then
twt.OBBinRange[i+1] = pos
-- Get bounding box direction vectors
twt.OBBinRange[i+2] = obj:getObjectDirectionVector(id) -- x
twt.OBBinRange[i+4] = obj:getObjectDirectionVectorUp(id) -- z
twt.OBBinRange[i+3] = twt.OBBinRange[i+3] or vec3()
twt.OBBinRange[i+3]:setCross(twt.OBBinRange[i+4], twt.OBBinRange[i+2]); twt.OBBinRange[i+3]:normalize() -- y (left)
-- Scale bounding box direction vectors to vehicle diamensions
twt.OBBinRange[i+3]:setScaled(obj:getObjectInitialWidth(id) * 0.5)
twt.OBBinRange[i+2]:setScaled(obj:getObjectInitialLength(id) * 0.5)
twt.OBBinRange[i+4]:setScaled(obj:getObjectInitialHeight(id) * 0.5)
i = i + 4
end
end
end
twt.OBBinRange.n = i
end
local function castRay(rpos, rdir, rayDist)
for i = 1, twt.OBBinRange.n, 4 do
local minHit, maxHit = intersectsRay_OBB(rpos, rdir, twt.OBBinRange[i], twt.OBBinRange[i+1], twt.OBBinRange[i+2], twt.OBBinRange[i+3])
if maxHit > 0 then
rayDist = min(rayDist, max(minHit, 0))
end
end
return obj:castRayStatic(rpos, rdir, rayDist)
end
local function driveToTarget(targetPos, throttle, brake, targetSpeed)
if not targetPos then return end
local plan = currentRoute and currentRoute.plan
targetSpeed = targetSpeed or plan and plan.targetSpeed
if not targetSpeed then return end
local targetVec = targetPos - ego.pos; targetVec:normalize()
local dirAngle = asin(ego.rightVec:dot(targetVec))
-- oversteer
local throttleOverCoef = 1
if ego.speed > 1 then
local rightVel = ego.rightVec:dot(ego.vel)
if rightVel * ego.rightVec:dot(targetPos - ego.pos) > 0 then
local rotVel = min(1, (ego.prevDirVec:projectToOriginPlane(ego.upVec):normalized()):distance(ego.dirVec) * dt * 10000)
throttleOverCoef = max(0, 1 - abs(rightVel * ego.speed * 0.05) * min(1, dirAngle * dirAngle * ego.speed * 6) * rotVel)
end
end
local dirVel = ego.vel:dot(ego.dirVec)
local absegoSpeed = abs(dirVel)
local throttleUnderCoef = 1
local brakeUnderCoef = 1
if plan and plan[3] and dirVel > 3 then
local p1, p2 = plan[1].pos, plan[2].pos
local p2p1DirVec = p2 - p1; p2p1DirVec:normalize()
local tp2 = (plan.targetSeg or 0) > 1 and targetPos or plan[3].pos
local targetSide = (tp2 - p2):dot(p2p1DirVec:cross(ego.upVec))
local outDeviation = egoDeviationSmoother:value() - plan.egoDeviation * sign(targetSide)
outDeviation = sign(outDeviation) * min(1, abs(outDeviation))
egoDeviationSmoother:set(outDeviation)
egoDeviationSmoother:getUncapped(0, dt)
if outDeviation > 0 and absegoSpeed > 3 then
local steerCoef = outDeviation * absegoSpeed * absegoSpeed * min(1, dirAngle * dirAngle * 4)
local understeerCoef = max(0, steerCoef) * min(1, abs(ego.vel:dot(p2p1DirVec) * 3))
local noUndersteerCoef = max(0, 1 - understeerCoef)
throttleUnderCoef = noUndersteerCoef
brakeUnderCoef = min(brakeUnderCoef, max(0, 1 - understeerCoef * understeerCoef))
end
else
egoDeviationSmoother:set(0)
end
-- wheel speed
local throttleTcsCoef = 1
local brakeABSCoef = 1
if absegoSpeed > 0.05 then
if sensors.gz <= 0.1 then
local totalSlip = 0
local propSlip = 0
local totalDownForce = 0
local lwheels = wheels.wheels
for i = 0, tableSizeC(lwheels) - 1 do
local wd = lwheels[i]
if not wd.isBroken then
local lastSlip = wd.lastSlip
local downForce = wd.downForceRaw
totalSlip = totalSlip + lastSlip * downForce
totalDownForce = totalDownForce + downForce
if wd.isPropulsed then
propSlip = max(propSlip, lastSlip)
end
end
end
absegoSpeed = max(absegoSpeed, 3)
totalSlip = totalSlip / (totalDownForce + 1e-25)
-- abs
brakeABSCoef = min(1, 1.5 * square(square(square(max(0, absegoSpeed - totalSlip) / absegoSpeed))))
-- tcs
propSlip = propSlip * (parameters.driveStyle == 'offRoad' and 0.8 or 1)
local tcsCoef = max(0, absegoSpeed - propSlip * propSlip) / absegoSpeed
throttleTcsCoef = min(tcsCoef, smoothTcs:get(tcsCoef, dt))
else
brakeABSCoef = 0
throttleTcsCoef = 0
end
end
local brakeCoef = 1
if parameters.underSteerBrakeControl ~= 'off' then
brakeCoef = min(brakeCoef, brakeUnderCoef)
end
if parameters.abBrakeControl ~= 'off' then
brakeCoef = min(brakeCoef, brakeABSCoef)
end
local throttleCoef = 1
if parameters.oversteerThrottleControl ~= 'off' then
throttleCoef = min(throttleCoef, throttleOverCoef)
end
if parameters.understeerThrottleControl ~= 'off' then
throttleCoef = min(throttleCoef, throttleUnderCoef)
end
if parameters.throttleTcs ~= 'off' then
throttleCoef = min(throttleCoef, throttleTcsCoef)
end
local dirTarget = ego.dirVec:dot(targetVec)
local dirTargetAxis = ego.rightVec:dot(targetVec)
if internalState.crash.manoeuvre == 1 and dirTarget < ego.dirVec:dot(internalState.crash.dir) then
driveCar(-sign(dirAngle), brake * brakeCoef, throttle * throttleCoef, 0)
return
else
internalState.crash.manoeuvre = 0
end
if parameters.driveStyle == 'offRoad' then
brakeCoef = 1
throttleCoef = sqrt(throttleCoef)
end
internalState.egoForceGoFrontTime = max(0, internalState.egoForceGoFrontTime - dt)
if twt.state == 1 and internalState.egoCannotMoveTime > 1 and internalState.egoForceGoFrontTime == 0 then
twt.state = 0
internalState.egoCannotMoveTime = 0
internalState.egoForceGoFrontTime = 2
end
if internalState.egoForceGoFrontTime > 0 and dirTarget < 0 then
dirTarget = -dirTarget
dirAngle = -dirAngle
end
if currentRoute and (dirTarget < 0 or (twt.state == 1 and dirTarget < 0.866)) then -- TODO: Improve entry condition when twt.state == 1
local helperVec = 0.35 * ego.upVec -- auxiliary vector
twt.posTable[2]:setAdd2(ego.pos, helperVec)
helperVec:setScaled2(ego.dirVec, -0.05 * ego.length)
twt.posTable[2]:setAdd(helperVec)
helperVec:setScaled2(ego.rightVec, 0.5 * (0.7 * ego.width)) -- lateral translation
twt.posTable[2]:setAdd(helperVec) -- front right corner pos
helperVec:setScaled(-2)
twt.posTable[1]:setAdd2(twt.posTable[2], helperVec) -- front left corner pos
helperVec:setScaled2(ego.dirVec, -0.9 * ego.length) -- longitudinal translation
twt.posTable[3]:setAdd2(twt.posTable[2], helperVec) -- back right corner pos
twt.posTable[4]:setAdd2(twt.posTable[1], helperVec) -- back left corner pos
twt.dirTable[1]:setScaled2(ego.rightVec, -1)
twt.dirTable[2]:set(ego.dirVec)
twt.dirTable[3]:set(ego.rightVec)
twt.dirTable[4]:setScaled2(ego.dirVec, -1)
local sizeRatio = ego.length/ego.width
local blueNoiseRange = 6 + 2 * sizeRatio
local rayDist = 4 * ego.wheelBase -- TODO: Optimize rayDist. Higher is better for more open spaces but w performance hit
populateOBBinRange(rayDist)
local tmpVec = vec3()
local RRPicks = max(1, min(floor(150 * dt + 0.5), 8)) -- x * dt where x/fps is the numer of samples
for k = 1, RRPicks do --the ray cast loop: each iteration scans one area and casts on the minimum
local i = 0
local blueIndex = twt.blueNoiseRR * blueNoiseRange
if blueIndex < 3 then
i = floor(1 + blueIndex)
elseif blueIndex < 6 then
i = floor(1 + blueIndex) + 1
elseif blueIndex < 6 + sizeRatio then
i = 4
else
i = 8
end
local j = i * 4
tmpVec:setLerp(twt.posTable[twt.RRT[j-3]], twt.posTable[twt.RRT[j-2]], twt.biasedCoefs[i])
helperVec:setLerp(twt.dirTable[twt.RRT[j-1]], twt.dirTable[twt.RRT[j]], twt.biasedCoefs[i]) -- LERP corner direction
helperVec:normalize()
local rayLen = castRay(tmpVec, helperVec, min(twt.rayMins[i], rayDist))
if twt.rayMins[i] > rayLen or rayLen == rayDist then
twt.minRayCoefs[i] = twt.biasedCoefs[i]
twt.rayMins[i] = rayLen
else
tmpVec:setLerp(twt.posTable[twt.RRT[j-3]], twt.posTable[twt.RRT[j-2]], twt.minRayCoefs[i])
helperVec:setLerp(twt.dirTable[twt.RRT[j-1]], twt.dirTable[twt.RRT[j]], twt.minRayCoefs[i]) -- LERP corner direction
helperVec:normalize()
twt.rayMins[i] = castRay(tmpVec, helperVec, min(max(2 * twt.rayMins[i], 0.02), rayDist))
end
twt.blueNoiseRR = getBlueNoise1d(twt.blueNoiseRR)
twt.blueNoiseCoefs[i] = getBlueNoise1d(twt.blueNoiseCoefs[i])
local criticality = 2 * twt.rayMins[i] / (twt.rayMins[i] + rayDist) + 0.25
twt.biasedCoefs[i] = biasGainFun(twt.blueNoiseCoefs[i], twt.minRayCoefs[i], criticality)
end
local minRayDist = min(twt.rayMins[1], twt.rayMins[2], twt.rayMins[3], twt.rayMins[4], twt.rayMins[5], twt.rayMins[6], twt.rayMins[7], twt.rayMins[8])
twt.sampleCounter = twt.sampleCounter + 1
if twt.state == 0 then
if twt.sampleCounter * dt > max(0.5, min(50 * dt, 2.5)) and ego.speed <= 0.5 then -- TODO: Optimize sample count condition
twt.state = 1
else
local speed = max(0, ego.speed - 0.25)
local throttle = max(0, -sign(speed * dirVel)) * 0.3
local brake = max(0, sign(speed * dirVel)) * 0.3
driveCar(0, throttle, brake, 0)
end
end
if twt.state == 1 then -- start driving
if twt.targetSpeed < 0.66 or twt.minRay == "S" then -- TODO: Some branches may be consolidated
if minRayDist == twt.rayMins[7] then
twt.dirState[1] = 1
twt.dirState[2] = sign2(dirTargetAxis)
twt.minRay = "BL"
elseif minRayDist == twt.rayMins[5] then
twt.dirState[1] = 1
twt.dirState[2] = sign2(dirTargetAxis)
twt.minRay = "BR"
elseif minRayDist == twt.rayMins[1] then
twt.dirState[1] = -1
twt.dirState[2] = -sign2(dirTargetAxis)
twt.minRay = "FL"
if twt.dirState[2] == 1 and twt.rayMins[8] < 0.4 and twt.targetSpeed < 0.6 then twt.dirState[2] = 0 end
elseif minRayDist == twt.rayMins[3] then
twt.dirState[1] = -1
twt.dirState[2] = -sign2(dirTargetAxis)
if twt.dirState[2] == -1 and twt.rayMins[4] < 0.4 and twt.targetSpeed < 0.6 then twt.dirState[2] = 0 end
twt.minRay = "FR"
elseif minRayDist == twt.rayMins[6] then
twt.dirState[1] = 1
twt.dirState[2] = sign2(dirTargetAxis)
twt.minRay = "B"
elseif minRayDist == min(twt.rayMins[4], twt.rayMins[8]) then
local sideRuler = 0
local minSide = minRayDist == twt.rayMins[8] and "L" or "R"
if minSide == "L" then
sideRuler = ego.length * (1 - twt.minRayCoefs[8])
else
sideRuler = ego.length * twt.minRayCoefs[4]
end
twt.dirState[2] = 0
if sideRuler < twt.rayMins[6] and dirTarget < 0 then --TODO : test this
twt.dirState[1] = -1
elseif (ego.length - sideRuler) < twt.rayMins[2] then
twt.dirState[1] = 1
if minSide == "L" then
twt.dirState[2] = 1
else
twt.dirState[2] = -1
end
end
twt.minRay = "S"
else
twt.dirState[1] = -1
twt.dirState[2] = -sign2(dirTargetAxis)
twt.minRay = "F"
end
end
local threshold = min(ego.speed - 0.1, 0.66)
local dirCoef, minDist = 1, nil --TODO: what is car is trapped/sandwiched?
if twt.dirState[1] == -1 then -- reverse
dirCoef = min(1.2 - dirTarget, 1) -- dirTarget -> targetSpeed modulation
if twt.dirState[2] == -1 then -- steer left
minDist = min(twt.rayMins[4], twt.rayMins[6], twt.rayMins[7])
elseif twt.dirState[2] == 1 then -- steer right
minDist = min(twt.rayMins[5], twt.rayMins[6], twt.rayMins[8])
else -- no steering
minDist = twt.rayMins[6]
if twt.minRay ~= "S" then
threshold = twt.rayMins[2] -- possibly max(threshold, twt.rayMins[2])
end
end
else -- forward
if twt.dirState[2] == -1 then
minDist = min(twt.rayMins[1], twt.rayMins[2], twt.rayMins[8])
elseif twt.dirState[2] == 1 then
minDist = min(twt.rayMins[2], twt.rayMins[3], twt.rayMins[4])
else
minDist = twt.rayMins[2]
end
end
local targetSpeed = sqrt(2 * g * min(aggression, ego.staticFrictionCoef) * max(0, minDist - threshold) * dirCoef)
twt.targetSpeed = max(min(twt.speedSmoother:get(targetSpeed, dt), min(6, aggression * 6)), 0.3)
local speedDif = twt.targetSpeed - twt.dirState[1] * sign2(dirVel) * ego.speed
local steering = twt.steerSmoother:get(twt.dirState[2], dt)
local pbrake = 0 -- * clamp(sign2(0.83 + ego.upVec:dot(gravityDir)), 0, 1) -- >= 10 deg
local throttle, brake = 0, 0
if twt.dirState[1] == -1 then
throttle = clamp(-speedDif * 0.4, 0, 0.4)
brake = clamp(speedDif * 0.2, 0, 1)
elseif twt.dirState[1] == 1 then
throttle = clamp(speedDif * 0.2, 0, 1)
brake = clamp(-speedDif * 0.4, 0, 0.4)
end
driveCar(steering, throttle, brake, pbrake)
end
else
twt.reset()
local pbrake
if ego.vel:dot(ego.dirVec) < 0 and ego.speed > 0.1 then
-- TODO: handbrake when stoped or moving backward
-- apply brake (in arcade mode throttle input when moving backwards activates the brakes) and or parkingbrake
-- when vehicle is moving backwards but the target speed is effectively zero
if ego.speed < 0.15 and targetSpeed <= 1e-5 then
pbrake = 1
else
pbrake = 0
end
throttle = 0.5 * throttleCoef
brake = 0
else
if (ego.speed > 4 and ego.speed < 30 and abs(dirAngle) > 0.97 and brake == 0) or (ego.speed < 0.15 and targetSpeed <= 1e-5) then
-- The first condition was supposed to help the vehicles turn in tight corners by applying handbrake. This is very old logic. It might
-- no longer be needed. Revise.
-- The second condition is supposed to apply handbrake when the vehicle is at a stop but it looks to be unstable
-- TODO: handbrake when turning
pbrake = 1
else
pbrake = 0
end
throttle = throttle * throttleCoef
brake = brake * brakeCoef
end
local aggSq = square(aggression + max(0, -(ego.dirVec:dot(gravityDir))))
local rate = max(throttleSmoother[throttleSmoother:value() < throttle], 10 * aggSq * aggSq)
throttle = throttleSmoother:getWithRateUncapped(throttle, dt, rate)
driveCar(dirAngle, throttle, brake, pbrake)
end
end
local function posOnPlan(pos, plan, dist)
if not plan then return end
dist = dist or 4
dist = dist * dist
local bestSeg, bestXnorm
for i = 1, #plan-2 do
local p0, p1 = plan[i].pos, plan[i+1].pos
local xnorm1 = pos:xnormOnLine(p0, p1)
if xnorm1 > 0 then
local p2 = plan[i+2].pos
local xnorm2 = pos:xnormOnLine(p1, p2)
if xnorm1 < 1 then -- contained in segment i
if xnorm2 > 0 then -- also partly contained in segment i+1
local sqDistFromP1 = pos:squaredDistance(p1)
if sqDistFromP1 <= dist then
bestSeg = i
bestXnorm = 1
break -- break inside conditional
end
else
local sqDistFromLine = pos:squaredDistance(p0 + (p1 - p0) * xnorm1)
if sqDistFromLine <= dist then
bestSeg = i
bestXnorm = xnorm1
end
break -- break should be outside above conditional
end
elseif xnorm2 < 0 then
local sqDistFromP1 = pos:squaredDistance(p1)
if sqDistFromP1 <= dist then
bestSeg = i
bestXnorm = 1
end
break -- break outside conditional
end
else
break
end
end
return bestSeg, bestXnorm
end
local function egoPosOnPlan(plan)
local planCount = plan.planCount
local egoSeg = 1
local egoXnormOnSeg = 0
for i = 1, planCount-1 do
local p0Pos, p1Pos = plan[i].pos, plan[i+1].pos
local xnorm = ego.pos:xnormOnLine(p0Pos, p1Pos)
if xnorm < 1 then
if i < planCount - 2 then
local nextXnorm = ego.pos:xnormOnLine(p1Pos, plan[i+2].pos)
if nextXnorm >= 0 then
local p1Radius = plan[i+1].radiusOrig
if ego.pos:squaredDistance(linePointFromXnorm(p1Pos, plan[i+2].pos, nextXnorm)) <
square(ego.width + lerp(p1Radius, plan[i+2].radiusOrig, min(1, nextXnorm))) then
egoXnormOnSeg = nextXnorm
egoSeg = i + 1
break
end
end
end
egoXnormOnSeg = xnorm
egoSeg = i
break
end
end
local disp = 0
if egoSeg > 1 then
local sumLen = 0
disp = egoSeg - 1
for i = 1, disp do
sumLen = sumLen + plan[i].length
end
for i = 1, plan.planCount do
plan[i] = plan[i+disp]
if disp > 0 and i + disp > plan.planCount then
velocities[i] = vec3(0,0,0)
else
velocities[i] = velocities[i+disp]
end
end
plan.planCount = plan.planCount - disp
plan.planLen = max(0, plan.planLen - sumLen)
plan.stopSeg = max(1, plan.stopSeg - disp)
end
plan.egoXnormOnSeg = egoXnormOnSeg
plan.egoSeg = egoSeg - disp
end
-- returns the node index
local function getLastNodeWithinDistance(plan, dist)
dist = dist - plan[1].length * (1 - plan.egoXnormOnSeg)
if dist < 0 then
return 1
end
local planSeg = plan.planCount
for i = 2, plan.planCount-1 do
dist = dist - plan[i].length
if dist < 0 then
planSeg = i
break
end
end
return planSeg
end
local function calculateTarget(plan)
egoPosOnPlan(plan)
local targetLength = max(ego.speed * parameters.lookAheadKv, 4.5)
if plan.planCount >= 3 then
local xnorm = clamp(plan.egoXnormOnSeg, 0, 1)
targetLength = max(targetLength, plan[1].length * (1 - xnorm), plan[2].length * xnorm)
end
local remainder = targetLength
local targetPos = vec3(plan[plan.planCount].pos)
local targetSeg = max(1, plan.planCount-1)
local prevPos = linePointFromXnorm(plan[1].pos, plan[2].pos, plan.egoXnormOnSeg) -- ego.pos
local segVec, segLen = vec3(), nil
for i = 2, plan.planCount do
local pos = plan[i].pos
segVec:setSub2(pos, prevPos)
segLen = segLen or segVec:length()
if remainder <= segLen then
targetSeg = i - 1
targetPos:setScaled2(segVec, remainder / (segLen + 1e-30)); targetPos:setAdd(prevPos)
-- smooth target
local xnorm = clamp(targetPos:xnormOnLine(prevPos, pos), 0, 1)
local lp_n1n2 = linePointFromXnorm(prevPos, pos, xnorm * 0.5 + 0.25)
if xnorm <= 0.5 then
if i >= 3 then
targetPos = linePointFromXnorm(linePointFromXnorm(plan[i-2].pos, prevPos, xnorm * 0.5 + 0.75), lp_n1n2, xnorm + 0.5)
end
else
if i <= plan.planCount - 2 then
targetPos = linePointFromXnorm(lp_n1n2, linePointFromXnorm(pos, plan[i+1].pos, xnorm * 0.5 - 0.25), xnorm - 0.5)
end
end
break
end
prevPos = pos
remainder = remainder - segLen
segLen = plan[i].length
end
plan.targetPos = targetPos
plan.targetSeg = targetSeg
end
local function targetsCompatible(baseRoute, newRoute)
local baseTvec = baseRoute.plan.targetPos - ego.pos
local newTvec = newRoute.plan.targetPos - ego.pos
if ego.speed < 2 then return true end
if newTvec:dot(ego.dirVec) * baseTvec:dot(ego.dirVec) <= 0 then return false end
local baseTargetRight = baseTvec:cross(ego.upVec); baseTargetRight:normalize()
return abs(newTvec:normalized():dot(baseTargetRight)) * ego.speed < 2
end
local function getMinPlanLen(limLow, speed, accelg)
-- given current speed, distance required to come to a stop if I can decelerate at 0.2g
limLow = limLow or 150
speed = speed or ego.speed
accelg = max(0.2, accelg or 0.2)
return min(550, max(limLow, 0.5 * speed * speed / (accelg * g)))
end
local function pickAiWp(wp1, wp2, dirVec)
dirVec = dirVec or ego.dirVec
local vec1 = mapData.positions[wp1] - ego.pos
local vec2 = mapData.positions[wp2] - ego.pos
local dot1 = vec1:dot(dirVec)
local dot2 = vec2:dot(dirVec)
if (dot1 * dot2) <= 0 then
if dot1 < 0 then
return wp2, wp1
end
else
if vec2:squaredLength() < vec1:squaredLength() then
return wp2, wp1
end
end
return wp1, wp2
end
local function pathExtend(path, newPath)
if newPath == nil then return end
local pathCount = #path
if path[pathCount] ~= newPath[1] then return end
pathCount = pathCount - 1
for i = 2, #newPath do
path[pathCount+i] = newPath[i]
end
end
-- http://cnx.org/contents/--TzKjCB@8/Projectile-motion-on-an-inclin
local function projectileSqSpeedToRangeRatio(pos1, pos2, pos3)
local sinTheta = (pos2.z - pos1.z) / pos1:distance(pos2)
local sinAlpha = (pos3.z - pos2.z) / pos2:distance(pos3)
local cosAlphaSquared = max(1 - sinAlpha * sinAlpha, 0)
local cosTheta = sqrt(max(1 - sinTheta * sinTheta, 0)) -- in the interval theta = {-pi/2, pi/2} cosTheta is always positive
return 0.5 * g * cosAlphaSquared / max(cosTheta * (sinTheta*sqrt(cosAlphaSquared) - cosTheta*sinAlpha), 0)
end
local function getPathLen(path, startIdx, stopIdx)
if not path then return end
startIdx = startIdx or 1
stopIdx = stopIdx or #path
local positions = mapData.positions
local pathLen = 0
for i = startIdx+1, stopIdx do
pathLen = pathLen + positions[path[i-1]]:distance(positions[path[i]])
end
return pathLen
end
local function getPathBBox(path, startIdx, stopIdx)
if not path then return end
startIdx = startIdx or 1
stopIdx = stopIdx or #path
local positions = mapData.positions
local p = positions[path[startIdx]]
local v = positions[path[stopIdx]] + p; v:setScaled(0.5)
local r = getPathLen(path, startIdx, stopIdx) * 0.5
return {v.x - r, v.y - r, v.x + r, v.y + r}, v, r
end
local function waypointInPath(path, waypoint, startIdx, stopIdx)
if not path or not waypoint then return end
startIdx = startIdx or 1
stopIdx = stopIdx or #path
for i = startIdx, stopIdx do
if path[i] == waypoint then
return i
end
end
end
local function getPlanLen(plan, from, to)
from = max(1, from or 1)
to = min(plan.planCount-1, to or math.huge)
local planLen = 0
for i = from, to do
planLen = planLen + plan[i].length
end
return planLen
end
local function abortUpcommingLaneChange()
if not currentRoute then return end
if currentRoute.laneChanges[1] then
currentRoute.laneChanges[1].side = 0
end
local lastPlanPidx = currentRoute.plan[currentRoute.plan.planCount].pathidx
for _, lc in ipairs(currentRoute.laneChanges) do
if lc.pathIdx <= lastPlanPidx then
lc.side = 0
end
end
end
local function updatePlanLen(plan, j, k)
--profilerPushEvent("ai_update_planLen")
-- bulk recalculation of plan edge lengths and length of entire plan
-- j: index of earliest node position that has changed
-- k: index of latest node position that has changed
j = max((j or 1) - 1, 1)
k = min(k or plan.planCount, plan.planCount-1)
local planLen = plan.planLen
for i = j, k do
local edgeLen = plan[i+1].pos:distance(plan[i].pos)
planLen = planLen - plan[i].length + edgeLen
plan[i].length = edgeLen
end
plan.planLen = planLen
--profilerPopEvent("ai_update_planLen")
end
-- expand lane lateral limits of this plan node to the lane range limits
local function openLaneToLaneRange(planNode)
local roadHalfWidth = planNode.halfWidth
planNode.laneLimLeft = linearScale(planNode.rangeLeft, 0, 1, -roadHalfWidth, roadHalfWidth)
planNode.laneLimRight = linearScale(planNode.rangeRight, 0, 1, -roadHalfWidth, roadHalfWidth)
planNode.lanesOpen = true
end
-- expand lane lateral limits to the lane range for all nodes up to and including idx
local function openPlanLanesToLaneRange(plan, idx)
plan = plan or (currentRoute and currentRoute.plan)
if not plan then return end
for i = 1, min(plan.planCount, idx) do
openLaneToLaneRange(plan[i])
end
end
local function laneChange(plan, dist, signedDisp)
if not plan and currentRoute then plan = currentRoute.plan end
if not plan then return end
-- Apply node displacement
local invDist = 1 / (dist + 1e-30)
local curDist, normalDispVec = 0, vec3()
for i = 2, plan.planCount do
openLaneToLaneRange(plan[i])
curDist = curDist + plan[i-1].length
plan[i].lateralXnorm = clamp(plan[i].lateralXnorm + signedDisp * min(curDist * invDist, 1), plan[i].laneLimLeft, plan[i].laneLimRight)
normalDispVec:setScaled2(plan[i].normal, plan[i].lateralXnorm)
plan[i].pos:setAdd2(plan[i].posOrig, normalDispVec)
-- Recalculate vec and dirVec
plan[i].vec:setSub2(plan[i-1].pos, plan[i].pos); plan[i].vec.z = 0
plan[i].dirVec:set(plan[i].vec)
plan[i].dirVec:normalize()
end
updatePlanLen(plan)
--[[ For debugging
table.clear(newPositionsDebug)
for i = 1, #newPositions do
newPositionsDebug[i] = vec3(newPositions[i])
end
--]]
end
local function setStopPoint(plan, dist, args)
if not plan and currentRoute then
plan = currentRoute.plan
end
if not plan then return end
if not dist then -- reset stop segment
plan.stopSeg = math.huge
return
end
if (args or E).avoidJunction and currentRoute.path then -- prevents stopping directly in junctions
-- this code is temporary, and inefficient...
local seg
while true do
seg = getLastNodeWithinDistance(plan, dist)
local nid = currentRoute.path[plan[seg].pathidx]
if seg < plan.planCount - 1 and tableSize(mapData.graph[nid]) > 2 and plan[seg].pos:squaredDistance(mapData.positions[nid]) <= square(mapData.radius[nid]) then
dist = dist + 20
else
break
end
end
plan.stopSeg = seg
else
plan.stopSeg = getLastNodeWithinDistance(plan, dist)
end
end
local function numOfLanesFromRadius(rad1, rad2)
return max(1, math.floor(min(rad1, rad2 or math.huge) * 2 / 3.61 + 0.5)) -- math.floor(min(rad1, rad2) / 2.7) + 1
end
local laneStringBuffer = buffer.new()
local function flipLanes(lanes)
-- ex. '--+++' becomes '---++'
for i = #lanes, 1, -1 do
laneStringBuffer:put(lanes:byte(i) == 43 and '-' or lanes:byte(i) == 45 and '+' or '0')
end
return laneStringBuffer:get()
end
-- returns the number of lanes in a given direction
local function numOfLanesInDirection(lanes, dir)
-- lanes: a lane string
-- dir: '+' or '-'
dir = dir == '+' and 43 or dir == '-' and 45
local lanesN = 0
for i = 1, #lanes, 1 do
if lanes:byte(i) == dir then
lanesN = lanesN + 1
end
end
return lanesN
end
-- Returns the lane configuration of an edge as traversed in the fromNode -> toNode direction
-- if an edge does not have lane data they are deduced from the node radii
local function getEdgeLaneConfig(fromNode, toNode)
local lanes
local edge = mapData.graph[fromNode][toNode]
if edge.lanes then
lanes = edge.lanes
else -- make up some lane data in case they don't exist
if edge.oneWay then
local numOfLanes = numOfLanesFromRadius(mapData.radius[fromNode], mapData.radius[toNode])
lanes = string.rep("+", numOfLanes)
else
local numOfLanes = max(1, math.floor(numOfLanesFromRadius(mapData.radius[fromNode], mapData.radius[toNode]) * 0.5))
if mapmgr.rules.rightHandDrive then
lanes = string.rep("+", numOfLanes)..string.rep("-", numOfLanes)
else
lanes = string.rep("-", numOfLanes)..string.rep("+", numOfLanes)
end
end
end
return edge.inNode == fromNode and lanes or flipLanes(lanes) -- flip lanes string based on inNode data
end
-- Calculate the edge incident on wp2 which is most similar to the edge wp1->wp2
local function roadNaturalContinuation(wp1, wp2)
local inLaneConfig = getEdgeLaneConfig(wp1, wp2)
local inRadiuswp2, inRadiuswp1 = mapData:getEdgeRadii(wp2, wp1)
local inEdgeDir = vec3(); inEdgeDir:setSub2(mapData:getEdgePositions(wp2, wp1)); inEdgeDir:normalize()
local laneFlow = mapData.graph[wp1][wp2].drivability * 4 * min(inRadiuswp2, inRadiuswp1) / #inLaneConfig
local inLaneCount = numOfLanesInDirection(inLaneConfig, '+')
local inFwdFlow = inLaneCount * laneFlow
local inLaneCountOpposite = (#inLaneConfig - inLaneCount)
local inBackFlow = inLaneCountOpposite * laneFlow
local outEdgeDir, maxOutflow, minNode = vec3(), 0, nil
for k, v in pairs(mapData.graph[wp2]) do
if k ~= wp1 then
local outLaneConfig = getEdgeLaneConfig(wp2, k)
local numOfOutLanes = numOfLanesInDirection(outLaneConfig, '+')
outEdgeDir:setSub2(mapData:getEdgePositions(k, wp2)); outEdgeDir:normalize()
local dirCoef = 0.5 * max(0, 1 + outEdgeDir:dot(inEdgeDir))
local outLaneCountOpposite = (#outLaneConfig - numOfOutLanes)
local outRadiuswp2, outRadiusk = mapData:getEdgeRadii(wp2, k)
if numOfOutLanes == inLaneCount and outLaneCountOpposite == inLaneCountOpposite then
laneFlow = mapData.graph[wp2][k].drivability * 4 * 0.5 * (inRadiuswp2 + outRadiuswp2) / #outLaneConfig
else
laneFlow = mapData.graph[wp2][k].drivability * 4 * min(outRadiuswp2, outRadiusk)/ #outLaneConfig
end
local outFwdFlow = min(inFwdFlow, numOfOutLanes * laneFlow)
local outBackFlow = min(inBackFlow, outLaneCountOpposite * laneFlow)
local outflow = outFwdFlow * (1 + outBackFlow) * dirCoef
if outflow > maxOutflow then
maxOutflow = outflow
minNode = k
end
end
end
return minNode
end
-- returns the lane indices of the left most and right most lanes in the direction of travel
local function laneRangeIdx(laneConfig)
local numOfLanes = #laneConfig
local leftIdx, rightIdx = 1, numOfLanes
for i = 1, numOfLanes do
if laneConfig:byte(i) == 43 then -- "+"
leftIdx = i
break
end
end
for i = numOfLanes, 1, -1 do
if laneConfig:byte(i) == 43 then -- "+"
rightIdx = i
break
end
end
return leftIdx, rightIdx, numOfLanes
end
-- returns the lane lateral xnorm limits (boundaries in [0, 1] range 0 being the left boundary)
-- of the left most and right most lanes in the direction of travel and the number of lanes in that range.
-- This range might include lanes in the opposite direction if they are interleaved with lanes in the direction of travel
-- ex. '--++-++' the left most lane is lane 3, the right most lane is lane 7. The range is from lane 3 to lane 7 inclusive.
local function laneRange(laneConfig)
local leftLim, rightLim, numOfLanes = laneRangeIdx(laneConfig)
return (leftLim - 1) / numOfLanes, rightLim / numOfLanes, rightLim - leftLim + 1
end
-- Splits the lane string into three parts 1:leftIdx-1, leftIdx:rightIdx, rightIdx+1:numOfLanes and returns the three strings
-- leftIdx is the indice of the left most lane in the direction of travel
-- rightIdx is the indice of the right most lane in the direction of travel
-- therefore leftIdx:rightIdx is the lane string from the left most to the right most lane in the direction of travel
local function splitLaneRange(laneConfig)
local leftIdx, rightIdx, numOfLanes = laneRangeIdx(laneConfig)
--leftIdx = leftIdx
--rightIdx = rightIdx
return laneConfig:sub(1, leftIdx-1), laneConfig:sub(leftIdx, rightIdx), laneConfig:sub(rightIdx+1, numOfLanes)
end
-- Lateral xnorm limits ([0, 1]) of the left most and right most lanes from "lane" in the direction of "lane"
local function getLaneRangeFromLane(laneConfig, lane)
-- laneConfig: a string representing lane configuration ex. "-a-a+a+a"
-- lane: the lane number (1 to n) counting from left to right in the direction of travel
local numOfLanes = #laneConfig
local leftIdx, rightIdx = lane, lane
-- search left
for i = lane-1, 1, -1 do
if laneConfig:byte(i) == 43 then -- "+"
leftIdx = i
else
break
end
end
-- search right
for i = lane+1, numOfLanes do
if laneConfig:byte(i) == 43 then -- "+"
rightIdx = i
else
break
end
end
return (leftIdx-1) / numOfLanes, rightIdx / numOfLanes, leftIdx, rightIdx
end
-- Calculates the composite lane configuration coming into newNode
-- and adjusted lateral position, lane limits and lane range of the last plan node.
local function processNodeIncommingLanes(newNode, planNode)
local leftRange, centerRange, rightRange = splitLaneRange(newNode.inEdgeLanes)
local latXnorm = planNode.lateralXnorm
local laneLimLeft = planNode.laneLimLeft
local laneLimRight = planNode.laneLimRight
local halfWidth = planNode.halfWidth
local numOfOutLanes = numOfLanesInDirection(newNode.outEdgeLanes, "+")
local numOfInLanes = numOfLanesInDirection(newNode.inEdgeLanes, "+")
local defaultLaneWidth = min(2 * halfWidth / (#newNode.inEdgeLanes), 3.45)
local inEdgeDrivabilityInv = 1 / ((newNode.inEdgedrivability or 1) + 1e-30)
if not planNode.rangeLeft then planNode.rangeLeft, planNode.rangeRight = laneRange(newNode.inEdgeLanes) end
local rangeLeft = linearScale(planNode.rangeLeft or 0, 0, 1, -halfWidth, halfWidth)
local rangeRight = linearScale(planNode.rangeRight or 1, 0, 1, -halfWidth, halfWidth)
for nodeId, edgeData in pairs(mapData.graph[newNode.name]) do
if nodeId ~= newNode.nextNodeInPath and nodeId ~= newNode.prevNodeInPath then -- and not mapmgr.signalsData.nodes[newNode.name]
local lanes = getEdgeLaneConfig(nodeId, newNode.name)
local thisEdgeInLanes = numOfLanesInDirection(lanes, '+')
if thisEdgeInLanes > 0 then -- TODO: Should also possibly include traffic light data
local nodePos = mapData.positions[nodeId]
local dirRatio = max(0, (mapData.positions[newNode.nextNodeInPath or newNode.name] - newNode.posOrig):normalized():dot((newNode.posOrig - nodePos):normalized()))
local drivabilityRatio = min(1, edgeData.drivability * inEdgeDrivabilityInv)
thisEdgeInLanes = math.floor(thisEdgeInLanes * dirRatio * drivabilityRatio + 0.5)
if thisEdgeInLanes > 0 then
if newNode.inEdgeNormal:dot(nodePos) < newNode.inEdgeNormal:dot(newNode.posOrig) then -- coming into newNode from the left
numOfInLanes = numOfInLanes + thisEdgeInLanes
centerRange = string.rep("+", thisEdgeInLanes)..centerRange
local radius = thisEdgeInLanes * defaultLaneWidth * 0.5
latXnorm = latXnorm + radius
laneLimLeft = laneLimLeft + radius
laneLimRight = laneLimRight + radius
halfWidth = halfWidth + radius
rangeLeft = rangeLeft - radius
rangeRight = rangeRight + radius -- TODO: is this correct?
elseif numOfInLanes < numOfOutLanes then
local capInLanes = min(numOfInLanes + thisEdgeInLanes, numOfOutLanes)
thisEdgeInLanes = capInLanes - numOfInLanes
numOfInLanes = capInLanes
local radius = thisEdgeInLanes * defaultLaneWidth * 0.5
centerRange = centerRange..string.rep("+", thisEdgeInLanes)
latXnorm = latXnorm - radius
laneLimLeft = laneLimLeft - radius
laneLimRight = laneLimRight - radius
halfWidth = halfWidth + radius
rangeLeft = rangeLeft - radius
rangeRight = rangeRight + radius
end
end
end
end
end
return leftRange..centerRange..rightRange, latXnorm, laneLimLeft, laneLimRight, rangeLeft, rangeRight, halfWidth
end
-- Calculate the most appropriate lane of laneConfig
local function getBestLane(laneConfig, nodeLatPos, laneLeftLimLatPos, laneRightLimLatPos, plan, newNode)
-- nodeLatPos, laneLeftLimLatPos, laneRightLimLatPos are in the [0, 1] interval
local laneWidth = laneRightLimLatPos - laneLeftLimLatPos
local numOfLanes = max(1, #laneConfig)
local bestError = math.huge
local bestLane, newLaneLimLeft, newLaneLimRight
local planPos, dirVec
if plan then
planPos = plan[plan.planCount].pos
dirVec = plan[plan.planCount].pos - plan[max(1, plan.planCount-1)].pos; dirVec:normalize()
else
planPos = vec3()
dirVec = planPos
newNode = {}
newNode.posOrig = planPos
newNode.radiusOrig = 0
newNode.halfWidth = 0
newNode.normal = planPos
newNode.outEdgeLanes = laneConfig
end
if #newNode.outEdgeLanes == #laneConfig then -- TODO: this might not be correct
dirVec:set(0, 0, 0)
end
-- Get driving side parameter of the map for more exterior lane selection
local isRightHandDrive = mapmgr.rules.rightHandDrive
local newNodeWidthVec = newNode.halfWidth * newNode.normal
for i = 1, numOfLanes do -- traverse lanes in laneConfig
local thisLaneLimLeft, thisLaneLimRight = (i - 1) / numOfLanes, i / numOfLanes
local thisLaneLatPos = (thisLaneLimRight + thisLaneLimLeft) * 0.5
local laneColinearity = ((thisLaneLatPos * 2 - 1) * push3(newNodeWidthVec) + newNode.posOrig - planPos):normalized():dot(dirVec)
local colinearityError = 1 - max(0, laneColinearity)
local distError = abs(thisLaneLatPos - nodeLatPos)
local overlapError = laneWidth - max(0, min(thisLaneLimRight, laneRightLimLatPos) - max(thisLaneLimLeft, laneLeftLimLatPos))
local directionError = laneConfig:byte(i) == 43 and 0 or 1
-- lane bias, as driving-side aware, for right-hand drive: prefer leftmost lanes, for left-hand drive: prefer rightmost lanes
local laneBias = isRightHandDrive and i - 1 or numOfLanes - i
local totalError = distError + overlapError * 10 + directionError * 100 + laneBias * 0.1 + colinearityError * 3
if totalError < bestError then
bestError = totalError
bestLane = i
newLaneLimLeft = thisLaneLimLeft
newLaneLimRight = thisLaneLimRight
end
end
return bestLane, newLaneLimLeft, newLaneLimRight -- lateral positions are in the [0, 1] interval
end
local function getPathNodePosition(route, i)
local path = route.path
local wp1 = path[i-1] or route.plan[1].wp
local wp2 = path[i]
local wp3 = path[i+1]
--dump('---- > in', i, path[i])
if not wp1 and not wp3 then
--dump('!!!!!!!!!!!!!!', path, objectId)
return mapData.positions[wp2]:copy()
elseif not wp1 then
local wp2Pos = mapData:getEdgePositions(wp2, wp3)
return wp2Pos:copy()
elseif not wp3 then
local _, wp2Pos = mapData:getEdgePositions(wp1, wp2)
return wp2Pos:copy()
else
local e1P1, e1P2 = mapData:getEdgePositions(wp1, wp2)
local e2P1, e2P2 = mapData:getEdgePositions(wp2, wp3)
--dump(wp1, wp2, wp3, e1P2:squaredDistance(e2P1))
if e1P2:squaredDistance(e2P1) < 0.005 then
--dump('b0')
return (e1P2 + e2P1) * 0.5
else
local e1Xnorm, e2Xnorm = closestLinePoints(e1P1, e1P2, e2P1, e2P2)
local e2Xnorm2 = closestLinePoints(e2P1, e2P2, e1P1, e1P2)
local _, e1R2 = mapData:getEdgeRadii(wp1, wp2)
local e2R1 = mapData:getEdgeRadii(wp2, wp3)
if e1Xnorm == 0 and e2Xnorm2 == 0 then
-- segments are parallel
--dump('b1')
return (e1P2 + e2P1) * 0.5
elseif e1Xnorm >= 0 and (e1Xnorm - 1) * e1P2:distance(e1P1) <= e1R2 and e2Xnorm * e2P1:distance(e2P2) >= -e2R1 and e2Xnorm <= 1 then
-- intersection is within the (radius augmented at P2) e1 and e2 segments
--dump('b2', 'e1Xnorm = ', e1Xnorm, 'e2Xnorm = ', e2Xnorm)
local p1 = linePointFromXnorm(e1P1, e1P2, e1Xnorm)
local p2 = linePointFromXnorm(e2P1, e2P2, e2Xnorm)
p1:setAdd(p2); p1:setScaled(0.5)
return p1
elseif e2Xnorm < 1 and e1Xnorm >= 0 and (e1Xnorm - 1) * e1P2:distance(e1P1) <= e1R2 then
-- intersection is within (radius augmented at P2) e1 segment and not furnter than the end of e2 (enures that it is not over segment 2 end, if it is too short)
--dump('b3', 'e1Xnorm = ', e1Xnorm, 'e2Xnorm = ', e2Xnorm)
local segLen = e1P2:distance(e1P1)
return linePointFromXnorm(e1P1, e1P2, max(e1Xnorm, 1 - e1R2/segLen))
elseif e1Xnorm > 0 and e2Xnorm * e2P1:distance(e2P2) >= -e2R1 and e2Xnorm <= 1 then
-- intersection is within (radius augmented at P2) e2 segment and infront of e1P1
--dump('b4', 'e1Xnorm = ', e1Xnorm, 'e2Xnorm = ', e2Xnorm)
local segLen = e2P1:distance(e2P2)
return linePointFromXnorm(e2P1, e2P2, min(e2Xnorm, e2R1/segLen))
elseif e2Xnorm >= 1 or e1Xnorm < 0 then
-- intersection is over segment 2 end or before segment 1 start (particular case)
--dump('b5')
return (e1P2 + e2P1) * 0.5
else
-- intersection is between segments 1 and 2 but outside of radii extensions
--dump('b6')
local p1 = linePointFromXnorm(e1P1, e1P2, e1Xnorm)
local p2 = linePointFromXnorm(e2P1, e2P2, e2Xnorm)
p1:setAdd(p2); p1:setScaled(0.5)
local avgPoint = (e1P2 + e2P1) * 0.5
local avgPointToP1Line = p1 - avgPoint
return avgPoint + max(0, min(1, (e1R2 + e2R1) * 0.5 / avgPointToP1Line:length())) * avgPointToP1Line
end
end
end
end
local function getPathNodeRadius(path, i)
if not path[i-1] and not path[i+1] then
return mapData.radius[path[i]]
elseif not path[i-1] then
local wp1Rad = mapData:getEdgeRadii(path[i], path[i+1])
return wp1Rad
elseif not path[i+1] then
local _, wp2Rad = mapData:getEdgeRadii(path[i-1], path[i])
return wp2Rad
else
local wp1Pos, wp2Pos = mapData:getEdgePositions(path[i-1], path[i])
local wp3Pos, wp4Pos = mapData:getEdgePositions(path[i], path[i+1])
local e1Xnorm, e2Xnorm = closestLineSegmentPoints(wp1Pos, wp2Pos, wp3Pos, wp4Pos)
local wp1Rad, wp2Rad = mapData:getEdgeRadii(path[i-1], path[i])
local wp3Rad, wp4Rad = mapData:getEdgeRadii(path[i], path[i+1])
local r1 = wp1Rad + (wp2Rad - wp1Rad) * e1Xnorm
local r2 = wp3Rad + (wp4Rad - wp3Rad) * e2Xnorm
return (r1 + r2) * 0.5
end
end
local function buildNextRoute(route, currentplan)
local plan, path = currentplan or route.plan, route.path
local planCount = plan.planCount
local nextPathIdx = (plan[planCount].pathidx or 0) + 1
if loopPath == true and noOfLaps and noOfLaps > 1 and not path[nextPathIdx] then -- in case the path loops
local loopPathId
local pathCount = #path
local lastWayPoint = path[pathCount]
for i = 1, pathCount do
if lastWayPoint == path[i] then
loopPathId = i
break
end
end
nextPathIdx = 1 + loopPathId -- nextPathIdx % #path
noOfLaps = not plan.copy and noOfLaps - 1 or noOfLaps -- avoid decreasing noOfLaps for alternative plans
end
local newNodeName = path[nextPathIdx]
if not newNodeName then return end
local graph = mapData.graph
if not graph[newNodeName] then return end
-- gather information about new node
local tmpPos = getPathNodePosition(route, nextPathIdx)
local newNode = {
name = newNodeName,
posOrig = tmpPos,
pos = tmpPos:copy(),
radiusOrig = getPathNodeRadius(path, nextPathIdx),
biNormal = -mapmgr.surfaceNormalBelow(mapData.positions[newNodeName], mapData.radius[newNodeName] * 0.5),
prevNodeInPath = path[nextPathIdx-1],
inEdgeDrivability = nil,
inEdgeLanes = nil, -- lanes going into the node along the path
inEdgeSpeedLimit = nil,
nextNodeInPath = path[nextPathIdx+1],
outEdgeLanes = nil, -- lanes coming out of the node along the path
outEdgeSpeedLimit = nil,
}
if newNode.prevNodeInPath then
newNode.prevNodePos = mapData.positions[newNode.prevNodeInPath]
local link = graph[newNode.prevNodeInPath][newNode.name]
if link then
newNode.inEdgeLanes = getEdgeLaneConfig(newNode.prevNodeInPath, newNode.name)
newNode.inEdgeSpeedLimit = link.speedLimit
newNode.inEdgeDrivability = link.drivability
end
else -- if previous plan node is the ego.pos then
newNode.prevNodePos = vec3(plan[1].posOrig)
if planCount == 1 and plan[1].wp then -- check whether information is available
--newNode.inEdgeLanes = plan[1].lanes -- TODO: use plan[1].wp to get lane configuration instead of saving the lane configuration in plan[1].lanes?
newNode.prevNodeInPath = plan[1].wp
newNode.inEdgeLanes = getEdgeLaneConfig(newNode.prevNodeInPath, newNode.name)
newNode.inEdgeDrivability = graph[newNode.prevNodeInPath][newNode.name].drivability
end
end
-- if there is no nextNodeInPath can we deduce it by elimination?
-- we need to see if there is a unique node towards which we can lawfully drive
if not newNode.nextNodeInPath then
local nextPosibleNode = nil
for k, v in pairs(graph[newNode.name]) do
if k ~= newNode.prevNodeInPath then
if numOfLanesInDirection(getEdgeLaneConfig(newNode.name, k), '+') > 0 then
if nextPosibleNode then
nextPosibleNode = nil
break
else
nextPosibleNode = k
end
end
end
end
newNode.nextNodeInPath = nextPosibleNode
end
if newNode.nextNodeInPath then
local link = graph[newNode.name][newNode.nextNodeInPath]
if link then
newNode.outEdgeLanes = getEdgeLaneConfig(newNode.name, newNode.nextNodeInPath)
newNode.outEdgeSpeedLimit = link.speedLimit
end
else
if M.mode ~= 'traffic' then
newNode.outEdgeLanes = newNode.inEdgeLanes
newNode.outEdgeSpeedLimit = newNode.inEdgeSpeedLimit
else
return
end
end
if newNode.outEdgeLanes and not newNode.inEdgeLanes then
newNode.inEdgeLanes = newNode.outEdgeLanes
end
-- Adjust last plan node normal given information about node to be inserted:
-- The normal of the node that is currently the last in the plan may need to be updated
-- either because the path has been extended (where previously it was the last node in the path)
-- or because the path has changed from this node forwards
if planCount > 1 then
if plan[planCount].wayPointAhead ~= path[nextPathIdx] then
local norm1 = plan[planCount].biNormal:cross(plan[planCount].posOrig - plan[planCount-1].posOrig); norm1:normalize()
local norm2 = plan[planCount].biNormal:cross(newNode.posOrig - plan[planCount].posOrig); norm2:normalize()
plan[planCount].normal:setAdd2(norm1, norm2)
local tmp = plan[planCount].normal:length()
plan[planCount].normal:setScaled(1 / (tmp + 1e-30))
local chordLength = min(2 / tmp, (1 - norm1:dot(norm2) * 0.5) * tmp) -- 2 / tmp == cos(x/2) : x is angle between norm1, norm2
plan[planCount].halfWidth = plan[planCount].radiusOrig * chordLength
end
else
plan[planCount].normal:setSub2(newNode.posOrig, plan[planCount].posOrig)
plan[planCount].normal:setCross(plan[planCount].biNormal, plan[planCount].normal)
plan[planCount].normal:normalize()
end
-- Calculate normal of node to be inserted into the plan
-- This normal is calculated from the normals of the two path edges incident on it
newNode.inEdgeNormal = vec3()
if newNode.prevNodeInPath then -- TODO: why not check for newNode.prevNodePos?
local wp1Pos, wp2Pos = mapData:getEdgePositions(newNode.prevNodeInPath, newNode.name)
newNode.inEdgeNormal:setSub2(wp2Pos, wp1Pos)
newNode.inEdgeNormal:setCross(newNode.biNormal, newNode.inEdgeNormal)
newNode.inEdgeNormal:normalize()
end
newNode.outEdgeNormal = vec3()
if newNode.nextNodeInPath then
local wp1Pos, wp2Pos = mapData:getEdgePositions(newNode.name, newNode.nextNodeInPath)
newNode.outEdgeNormal:setSub2(wp2Pos, wp1Pos)
newNode.outEdgeNormal:setCross(newNode.biNormal, newNode.outEdgeNormal)
newNode.outEdgeNormal:normalize()
end
newNode.normal = newNode.inEdgeNormal + newNode.outEdgeNormal
local tmp = newNode.normal:length()
newNode.normal:setScaled(1 / (tmp + 1e-30))
newNode.chordLength = min(2 / tmp, (1 - newNode.inEdgeNormal:dot(newNode.outEdgeNormal) * 0.5) * tmp) -- new node road width multiplier
newNode.halfWidth = newNode.radiusOrig * newNode.chordLength
local newNodeLaneLimLeft = 0
local newNodeLaneLimRight = 1
local newNodeRangeLeft = 0
local newNodeRangeRight = 1
local newNodeRangeLaneCount = 1
local rangeBestLane = 0
local bestLane
local newNodeLaneConfig
local newNodeRangeLeftIdx -- index of left most lane in range
local newNodeRangeRightIdx -- index of right most lane in
