-- 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 script = {} local inScript = nil local time = 0 local scriptTime = 0 local prevDirPoint = nil local prevVel = 0 local prevAccel = 0 local aiPos = vec3(0, 0, 0) local aiVel = vec3(0, 0, 0) local aiSpeed = 0 local targetPos = vec3(0, 0, 0) local aiDirVec = vec3(0, 0, 0) local followInitCounter = 0 local targetLength = 1 local initConditions = {} local posError = 0 local aiWidth = 2 local loopCounter = 0 local loopType = "alwaysReset" local min, max, abs, sqrt = math.min, math.max, math.abs, math.sqrt local function velAccelFrom2dist(l1, l2, t1, t2) local t1s, t2s = square(t1), square(t2) local denom = 1 / (t1s * t2 - t1 * t2s) return (l2 * t1s - l1 * t2s) * denom, 2 * (l1 * t2 - l2 * t1) * denom end local function driveCar(steering, throttle, brake, parkingbrake) input.event("steering", clamp(-steering, -1, 1), 1) input.event("throttle", clamp(throttle, 0, 1), 2) input.event("brake", clamp(brake, 0, 1), 2) input.event("parkingbrake", clamp(parkingbrake, 0, 1), 2) end local function calculateTarget() local scriptLen = #script if scriptLen >= 2 then local p1, p2 = vec3(script[1]), vec3(script[2]) local prevPos = linePointFromXnorm(p1, p2, clamp(aiPos:xnormOnLine(p1, p2), 0, 1)) for i = 2, scriptLen do local curPos = vec3(script[i]) local diff = curPos - prevPos local diffLen = diff:length() if diffLen >= targetLength then targetPos = prevPos + diff:normalized() * targetLength return end targetLength = targetLength - diffLen prevPos = curPos end end targetPos = vec3(script[scriptLen]) end local function updateGFXrecord(dt) local pos = vec3(obj:getFrontPosition()) local scriptLen = #script if scriptLen >= 2 then local s0, s1 = script[scriptLen], script[scriptLen - 1] local p0, p1 = vec3(s0), vec3(s1) local posline if prevDirPoint ~= nil then posline = linePointFromXnorm(prevDirPoint, p1, pos:xnormOnLine(prevDirPoint, p1)) else posline = linePointFromXnorm(p0, p1, pos:xnormOnLine(p0, p1)) end local pospjlen = (pos - posline):projectToOriginPlane(vec3(obj:getDirectionVectorUp())):squaredLength() if pospjlen < 0.01 then local t2 = time - s1.t local l2 = pos:distance(p1) if not prevDirPoint then local l1 = p0:distance(p1) prevVel, prevAccel = velAccelFrom2dist(l1, l2, s0.t - s1.t, t2) prevDirPoint = p0 end local pl2 = (prevVel + 0.5 * prevAccel * t2) * t2 if abs(pl2 - l2) < 0.1 then scriptLen = scriptLen - 1 else prevDirPoint = nil end else prevDirPoint = nil end end script[scriptLen + 1] = {x = pos.x, y = pos.y, z = pos.z, t = time} time = time + dt end local function scriptStop(centerWheel, engageParkingbrake) if centerWheel == nil then centerWheel = true engageParkingbrake = true end if centerWheel then driveCar(0, 0, 0, engageParkingbrake and 1 or 0) else if engageParkingbrake then input.event("parkingbrake", 1, 2) end end script = {} M.updateGFX = nop end local function updateGFXfollow(dt) if followInitCounter > 0 then followInitCounter = followInitCounter - 1 return end local scriptLen = #script if scriptLen == 0 then M.updateGFX = nop return end aiPos:set(obj:getFrontPosition()) aiVel:set(obj:getVelocity()) local aiVelLen = aiVel:length() local prevDirVec = aiDirVec aiDirVec = vec3(obj:getDirectionVector()) while scriptLen >= 3 do local p1, p2 = vec3(script[1]), vec3(script[2]) local xnorm = aiPos:xnormOnLine(p1, p2) if (p1:squaredDistance(p2) < 0.0025 and script[2].t < time) or aiPos:xnormOnLine(p1, p2) > 1 or (xnorm < 0 and script[2].t < time and aiVel:dot(p2 - p1) < 0) then table.remove(script, 1) scriptLen = scriptLen - 1 else break end end if scriptLen < 3 then -- finished if loopCounter > 0 then loopCounter = loopCounter - 1 end if loopCounter ~= 0 then M.startFollowing(inScript, inScript.loopTimeOffset, loopCounter, loopType) return else ai.stopFollowing() return end end calculateTarget() local targetPosOnLine = targetPos local reqVel local timeDiff local pbrake local p1, p2 = vec3(script[1]), vec3(script[2]) local p2p1 = p2 - p1 local targetaivec = targetPos - aiPos local targetai = targetaivec:dot(aiDirVec) local posonline = aiPos:xnormOnLine(p1, p2) aiSpeed = aiVel:dot(aiDirVec) * sign(targetai) local p3 = vec3(script[3]) local l1 = p2p1:length() local l2 = p3:distance(p2) + l1 local t1 = script[2].t - script[1].t prevVel, prevAccel = velAccelFrom2dist(l1, l2, t1, script[3].t - script[1].t) local nextVel = prevVel + prevAccel * t1 prevVel, nextVel = max(0, prevVel), max(0, nextVel) local xnorm = clamp(posonline, 0 ,1) if prevAccel == 0 then scriptTime = lerp(script[1].t, script[2].t, xnorm) else local s = xnorm * l1 local delta = sqrt(max(0, 2 * prevAccel * s + prevVel * prevVel)) local ts = (delta - prevVel) / prevAccel if ts >= 0 and ts <= t1 then scriptTime = ts + script[1].t else scriptTime = lerp(script[1].t, script[2].t, xnorm) end end reqVel = lerp(prevVel, nextVel, (scriptTime - script[1].t) / (t1 + 1e-30)) timeDiff = scriptTime - time local up = vec3(obj:getDirectionVectorUp()) local left = aiDirVec:cross(up):normalized() local turnleft = p2p1:cross(up):normalized() local noOversteerCoef = 1 -- oversteer if aiVelLen > 1 then local leftVel = left:dot(aiVel) if leftVel * left:dot(targetPosOnLine - aiPos) > 0 then local dirDiff = -math.asin(left:dot((targetPos - aiPos):normalized())) local rotVel = min(1, (prevDirVec:projectToOriginPlane(up):normalized() - aiDirVec):length() * dt * 10000) noOversteerCoef = max(0, 1 - abs(leftVel * aiVelLen * 0.05) * min(1, dirDiff * dirDiff * aiVelLen * 6) * rotVel) end end -- deviation local tp2 if targetPosOnLine:xnormOnLine(p1,p2) > 1 then tp2 = (targetPosOnLine - p2):normalized():dot(turnleft) else tp2 = (vec3(script[3]) - p2):normalized():dot(turnleft) end local carturn = turnleft:dot(left) local deviation = (aiPos - p2):dot(turnleft) deviation = sign(deviation) * min(5, abs(deviation)) local reldeviation = sign(tp2) * deviation posError = aiPos:distance(linePointFromXnorm(p1, p2, clamp(posonline, 0, 1))) * sign(deviation) -- target bending local grleft = left:dot(vec3(obj:getGravityVector())) if deviation * grleft > 0 then targetPos = targetPosOnLine - left * sign(deviation) * min(5, abs(0.01 * deviation * grleft * aiVelLen * min(1, carturn * carturn))) end local targetVec = (targetPos - aiPos):normalized() local dirDiff = -math.asin(left:dot(targetVec)) -- understeer local steerCoef = reldeviation * min(aiSpeed * aiSpeed, abs(aiSpeed)) * min(1, dirDiff * dirDiff * 4) * 0.2 local understeerCoef = max(0, -steerCoef) * min(1, abs(aiVel:dot(p2p1:normalized()) * 3)) local noUndersteerCoef = max(0, 1 - understeerCoef) targetLength = max(aiVelLen * 0.65, 3) -- reduce time spring when in understeer local curthrottle = clamp((reqVel - aiSpeed) * 3 + clamp(-timeDiff * 5, -1, 1.2) * noUndersteerCoef, -1 ,1) -- stay put when starting with negative offset if timeDiff > 0 and max(aiVelLen) < 0.5 then curthrottle = 0 pbrake = 1 else pbrake = 0 end -- understeer guard if reldeviation < 0 and aiVelLen > 1 then curthrottle = curthrottle * noUndersteerCoef curthrottle = max(curthrottle, min(0, -1 + understeerCoef * understeerCoef)) -- cut off brake else if curthrottle > 0 then curthrottle = min(1, curthrottle * (1 + abs(deviation))) -- push some more when on the inside of the turn end end local throttle, brake = clamp(curthrottle, 0, 1), clamp(-curthrottle, 0, 1) brake = min(1, max(0, brake - 0.1) / (1 - 0.1)) -- reduce brake flutter -- oversteer if throttle > 0 then throttle = throttle * noOversteerCoef end -- print(scriptLen..':'..timeDiff) -- print(noUndersteerCoef..','..noOversteerCoef) -- wheel speed local absAiSpeed = abs(aiSpeed) if absAiSpeed > 0.2 then local avgWheelSpeed = 0 local wheelCount = 0 local minAbsWheelSpeed = math.huge local maxAbsWheelSpeed = 0 local lwheels = wheels.wheels for i = 0, tableSizeC(lwheels) - 1 do local wd = lwheels[i] if not wd.isBroken then wheelCount = wheelCount + wd.isSpeedo local absWheelVel = abs(wd.angularVelocity * wd.radius) avgWheelSpeed = avgWheelSpeed + absWheelVel * wd.isSpeedo if wd.brakeTorque > 0 then minAbsWheelSpeed = min(minAbsWheelSpeed, absWheelVel) end if wd.isPropulsed then maxAbsWheelSpeed = max(maxAbsWheelSpeed, absWheelVel) end end end -- manual abs and tcs if wheelCount > 0 then local latVgrad = 0.5 * aiWidth * abs(sensors.gx2) / (aiVelLen + 1e-30) avgWheelSpeed = avgWheelSpeed / wheelCount -- abs local minGradSpeed = absAiSpeed - latVgrad if avgWheelSpeed < minGradSpeed and brake > 0 then brake = brake * minAbsWheelSpeed / minGradSpeed end -- tcs local maxGradSpeed = max(3, absAiSpeed + latVgrad) if maxAbsWheelSpeed > maxGradSpeed and throttle > 0 then local redcoef = maxGradSpeed / maxAbsWheelSpeed throttle = throttle * redcoef * redcoef end end end -- reverse if targetai < 0 then local targetailen = targetaivec:length() if targetai / (targetailen + 1e-30) < -0.5 or targetailen < 8 then dirDiff = -dirDiff throttle, brake = brake, throttle end end if aiSpeed > 4 and aiSpeed < 30 and abs(dirDiff) > 0.8 and brake == 0 then pbrake = 1 end driveCar(dirDiff, throttle, brake, pbrake) time = time + dt end local function startRecording() table.clear(script) time = 0 M.updateGFX = updateGFXrecord prevDirPoint = nil local dir, up = obj:getDirectionVector(), obj:getDirectionVectorUp() initConditions.dir = {x = dir.x, y = dir.y, z = dir.z} initConditions.up = {x = up.x, y = up.y, z = up.z} end local function stopRecording() --print(">>> AI.stopRecording") M.updateGFX = nop if script[1] ~= nil then script[1].dir = initConditions.dir script[1].up = initConditions.up end return {path = script} -- return script end local function startFollowing(_inScript, _timeOffset, _loopCounter, _loopType) --print(">>> AI.startFollowing: " .. dumps(inScript)) -- inScript = testrec inScript = _inScript if inScript == nil then return end if inScript.path ~= nil then script = inScript.path else script = inScript end script = deepcopy(script) if #script <= 1 then return end loopType = _loopType or "alwaysReset" local timeOffset = _timeOffset or inScript.timeOffset local totalLoopCount = inScript.loopCount or 1 loopCounter = _loopCounter or totalLoopCount if timeOffset ~= nil then if timeOffset >= 0 then while script[2] ~= nil and script[2].t < timeOffset do table.remove(script, 1) end if #script >= 2 then local s1t = script[1].t local sp = linePointFromXnorm(vec3(script[1]), vec3(script[2]), (timeOffset - s1t) / (script[2].t - s1t)) script[1] = {x = sp.x, y = sp.y, z = sp.z, t = timeOffset} end end for _, s in ipairs(script) do s.t = s.t - timeOffset end if #script <= 1 then return end end local initDir = script[1].dir local initUp = script[1].up followInitCounter = 3 prevVel = 0 time = 0 scriptTime = 0 posError = 0 local p1 = vec3(script[1]) local dir local up local pos if initDir ~= nil then dir = vec3(initDir) pos = p1 - dir * vec3(obj:getFrontPositionRelative()):dot(vec3(obj:getDirectionVector())) if initUp ~= nil then up = vec3(initUp) else up = mapmgr.surfaceNormalBelow(pos) end else local p2 for i = 2, #script do if p1:z0():distance(vec3(script[i]):z0()) > 0.2 then p2 = vec3(script[i]) break end end if p2 ~= nil then dir = (p2 - p1):normalized() pos = p1 - dir * vec3(obj:getFrontPositionRelative()):dot(vec3(obj:getDirectionVector())) up = mapmgr.surfaceNormalBelow(pos) end end if dir ~= nil then if loopType == 'alwaysReset' or (loopType == 'startReset' and loopCounter == totalLoopCount) then aiPos = pos obj:requestReset(RESET_PHYSICS) obj:queueGameEngineLua('be:getObjectByID('..tostring(obj:getID())..'):resetBrokenFlexMesh()') local rot = quatFromDir(dir:cross(up):cross(up), up) obj:queueGameEngineLua("vehicleSetPositionRotation("..obj:getID()..","..pos.x..","..pos.y..","..pos.z..","..rot.x..","..rot.y..","..rot.z..","..rot.w..")") end if controller.mainController then controller.mainController.setGearboxMode("arcade") end wheels.setABSBehavior("arcade") aiWidth = obj:getObjectInitialWidth(objectId) M.updateGFX = updateGFXfollow end end local function debugDraw() local debugDrawer = obj.debugDrawProxy if M.debugMode == 'all' or M.debugMode == 'target' then if M.updateGFX == updateGFXfollow then debugDrawer:drawSphere(0.2, vec3(targetPos):toFloat3(), color(0,0,255,255)) end end if M.debugMode == 'all' or M.debugMode == 'path' then for _, s in ipairs(script) do debugDrawer:drawSphere(0.2, vec3(s):toFloat3(), color(255,0,0,255)) end end end local function scriptState() if M.updateGFX == updateGFXrecord then return {status = 'recording', time = time} elseif M.updateGFX == updateGFXfollow then return {status = 'following', scriptTime = scriptTime, time = time, endScriptTime = script[#script].t, posError = posError, targetPos = vec3(targetPos)} end return nil end local function isDriving() return M.updateGFX == updateGFXfollow end M.updateGFX = nop M.startRecording = startRecording M.stopRecording = stopRecording M.startFollowing = startFollowing M.stopFollowing = scriptStop M.scriptStop = scriptStop M.debugDraw = debugDraw M.scriptState = scriptState M.isDriving = isDriving M.debugMode = 'all' return M