--[[ This file is part of Courseplay (https://github.com/Courseplay/courseplay) Copyright (C) 2019 Peter Vajko This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>. ]] ---@class CollisionDetector CollisionDetector = CpObject() function CollisionDetector:init(vehicle, course) -- channel 12 until the legacy code is spamming channel 3 self.debugChannel = 3 self.debugTicks = 100 -- show sparse debug information only at every debugTicks update self.vehicle = vehicle self:debug('creating CollisionDetector') self.course = course self.collidingObjects = {} self.nCollidingObjects = 0 self.ignoredNodes = {} self:addToIgnoreList(self.vehicle) self.trafficCollisionTriggers = {} self.trafficCollisionTriggers[1] = nil self:createTriggers() self:adaptCollisHeight() end -- destructor function CollisionDetector:delete() self:debug('deleting CollisionDetector') -- work from the back of the list as these are linked together and deleting the -- first one removes all and then there's a warning about deleting object before -- removing the trigger if self.trafficCollisionTriggers then self:deleteTriggers() self.trafficCollisionTriggers = nil self.collidingObjects = {} -- clear all detected collisions self.nCollidingObjects = 0 -- clear all detected collisions self.ignoredNodes = {} -- clear all detected collisions end end function CollisionDetector:reset() self:debug('reset CollisionDetector triggers') if self.trafficCollisionTriggers then self:delete() end self:createTriggers() end --- Create collision detection triggers: make four copies of the existing collision box and link them together -- so they form a snake in front of the vehicle along the path. When this snakes collides with something, the -- the onCollision() callback is triggered by the game engine function CollisionDetector:createTriggers() if not courseplay:findAiCollisionTrigger(self.vehicle) then return end -- create triggers only for enterable vehicles -- self.aiTrafficCollisionTrigger = courseplay:findAiCollisionTrigger(self.vehicle) -- if not self.aiTrafficCollisionTrigger then return end if not self.trafficCollisionTriggers then self.trafficCollisionTriggers = {} end self.vehicle.cp.trafficCollisionTriggerToTriggerIndex = {} -- self.vehicle.aiTrafficCollisionTrigger = self.aiTrafficCollisionTrigger if self.trafficCollisionTriggers[1] == nil then for i = 1, self.vehicle.cp.numTrafficCollisionTriggers do local newTrigger = clone(self.vehicle.aiTrafficCollisionTrigger, true) self.trafficCollisionTriggers[i] = newTrigger self.vehicle.cp.trafficCollisionTriggerToTriggerIndex[newTrigger] = i; setName(newTrigger, 'cpAiCollisionTrigger ' .. tostring(i)) if i > 1 then unlink(newTrigger) link(self.trafficCollisionTriggers[i - 1], newTrigger) setTranslation(newTrigger, 0, 0, 4) end; addTrigger(newTrigger, 'onCollision', self) end; end end function CollisionDetector:deleteTriggers() for i = self.vehicle.cp.numTrafficCollisionTriggers, 1, -1 do local node = self.trafficCollisionTriggers[i] if node then removeTrigger(node) if entityExists(node) then unlink(node) self.vehicle:removeWashableNode(node) self.vehicle:removeWearableNode(node) delete(node) end end self.trafficCollisionTriggers[i] = nil end end --- Add and object to the list of ignored nodes. We must ignore collisions with our own collision boxes, -- and with our own vehicle/implements and their collision triggers. This one adds object and all the objects -- attached to it to the ignore list recursively function CollisionDetector:addToIgnoreList(object) self:debug('will ignore collisions with %q (%q)', nameNum(object), tostring(object.cp.xmlFileName)) self.ignoredNodes[object.rootNode] = true; -- add the vehicle or implement's own collision trigger to the ignore list -- local aiCollisionTrigger = courseplay:findAiCollisionTrigger(object) courseplay:findAiCollisionTrigger(object) -- get aiTrafficCollisionTrigger for vehicles if object.aiTrafficCollisionTrigger then self:debug('-- %q', getName(object.aiTrafficCollisionTrigger)) self.ignoredNodes[object.aiTrafficCollisionTrigger] = true end if object.components then self:debug('will ignore collisions with %q (%q) components', nameNum(object), tostring(object.cp.xmlFileName)) for _, component in pairs(object.components) do self:debug('-- %q', getName(component.node)) self.ignoredNodes[component.node] = true; end end -- add all attached implements recursively for _, impl in pairs(object:getAttachedImplements()) do self:addToIgnoreList(impl.object) end end function CollisionDetector:isIgnored(node) local parent = getParent(node) if self.ignoredNodes[node] or CpManager.trafficCollisionIgnoreList[node] or self.ignoredNodes[parent] or CpManager.trafficCollisionIgnoreList[parent] then return true end end function CollisionDetector:onCollision(triggerId, otherId, onEnter, onLeave, onStay, otherShapeId) if not self:isIgnored(otherId) then if onEnter then if not self.collidingObjects[otherId] then self.collidingObjects[otherId] = otherId self.nCollidingObjects = self.nCollidingObjects + 1 self:debug('collision trigger %s entered: %s, %d colliding objects.', getName(triggerId), getName(otherId), self.nCollidingObjects) end end end if onLeave and self.collidingObjects[otherId] then self.nCollidingObjects = self.nCollidingObjects - 1 self:debug('collision trigger %s left: %s, %d colliding objects.', getName(triggerId), getName(otherId), self.nCollidingObjects) self.collidingObjects[otherId] = nil end end function CollisionDetector:getStatus(dt) local isInTraffic = false local trafficSpeed = 0 if self.nCollidingObjects > 0 then local collidingVehicleId = self:findTheValidCollisionVehicle() if collidingVehicleId ~= nil then local collidingVehicle = g_currentMission.nodeToObject[collidingVehicleId] if collidingVehicle ~= nil then if collidingVehicle.isCpPathVehicle then self:setPathVehiclesSpeed(collidingVehicle, dt) end if collidingVehicle.lastSpeedReal == nil or collidingVehicle.lastSpeedReal*3600 < 0.01 then -- collidingVehicle not moving -> STOP isInTraffic = true else trafficSpeed = collidingVehicle.lastSpeedReal*3600 end end end end return isInTraffic, trafficSpeed end function CollisionDetector:doesVehicleGoMyDirection(collidingVehicleId) local x, y, z = getWorldTranslation(self.vehicle.cp.directionNode); local x1,z1 = AIVehicleUtil.getDriveDirection(collidingVehicleId, x, y, z); if z1 > -0.9 then -- I'm in front of vehicle, face2face or beside < 4 o'clock return false; end; return true; end function CollisionDetector:findTheValidCollisionVehicle() --go throught the objects to figure out the valid target local currentCollisionVehicleId = 0 local distanceToCollisionVehicle = math.huge local distance = math.huge --toggle through my collisionTriggerHits for targetId,_ in pairs (self.collidingObjects) do --does it still exist? (straw bales) if entityExists(targetId) then --get the vehicle concerned local collisionVehicle = g_currentMission.nodeToObject[targetId] --print(string.format("collisionVehicle[%s](%s): %s",tostring(targetId),tostring(getName(targetId)),tostring(collisionVehicle))) if collisionVehicle ~= nil then --if the collisionVehicle is valid, check whether it's the closest if self:isItARailCrossing(collisionVehicle) then local _,transY,_ = getTranslation(targetId); if transY > 0 then distance = courseplay:nodeToNodeDistance(self.vehicle.cp.directionNode or self.vehicle.rootNode, targetId) end else distance = courseplay:distanceToObject(self.vehicle, collisionVehicle) end if distanceToCollisionVehicle > distance then --print(string.format(" %d is closer (%.2f m)",targetId,distance)); distanceToCollisionVehicle = distance currentCollisionVehicleId = targetId; end else self:isItATrafficVehicle(targetId) --[[ if self:isItATrafficVehicle(targetId) then distance = courseplay:nodeToNodeDistance(self.vehicle.cp.directionNode or self.vehicle.rootNode, targetId) if distanceToCollisionVehicle > distance then --print(string.format(" %d is closer (%.2f m)",targetId,distance)); distanceToCollisionVehicle = distance currentCollisionVehicleId = targetId; end end ]] end else --delete NodeID e.g. StrawBales will be deleted and don't get onLeave end end --print("findTheValidCollisionVehicle: return:"..tostring(currentCollisionVehicleId)) return currentCollisionVehicleId end --check whether we hit the trafficBlokker of an railway crossing function CollisionDetector:isItARailCrossing(collisionVehicle) if collisionVehicle.railroadObjects then return true; end end -- check, whether its a traffic vehicle. -- if yes ,set it to g_cM.nodeToObject function CollisionDetector:isItATrafficVehicle(nodeId) local cm = getCollisionMask(nodeId); local currentCollisionVehicleId -- if bit21 is part of the collisionMask then set new vehicle in GCM.NTV -- if nodeId == nil and bitAND(cm, 2097152) ~= 0 and not string.match(getName(nodeId),'Trigger') and not string.match(getName(nodeId),'trigger') then if currentCollisionVehicleId == nil and bitAND(cm, 2097152) ~= 0 and not string.match(getName(nodeId),'Trigger') and not string.match(getName(nodeId),'trigger') then local pathVehicle = { rootNode = nodeId, isCpPathVehicle = true, name = "PathVehicle", sizeLength = 7, sizeWidth = 3, } g_currentMission.nodeToObject[nodeId] = pathVehicle -- currentCollisionVehicleId = nodeId; end return currentCollisionVehicleId end --- Update the collision detection boxes. This bends the snake according to the next waypoints in the path so -- we can detect objects along the path. ---@param course Course -- @param lx, lz vehicle-local coordinates of the goal point the vehicle is driving to function CollisionDetector:update(course, ix, lx, lz, disableLongCheck) local colDirX = lx local colDirZ = lz self:debugSparse('has %d colliding object(s)', self.nCollidingObjects) if self.trafficCollisionTriggers[1] ~= nil then self:setCollisionDirection(self.vehicle.cp.directionNode, self.trafficCollisionTriggers[1], colDirX, colDirZ) local recordNumber = ix for i = 2, self.vehicle.cp.numTrafficCollisionTriggers do -- if disableLongCheck or recordNumber + i >= course:getNumberOfWaypoints() or recordNumber < 2 then if disableLongCheck or recordNumber + i >= course:getNumberOfWaypoints() then -- enable the snake on the way to the start point of a course self:setCollisionDirection(self.trafficCollisionTriggers[i-1], self.trafficCollisionTriggers[i], 0, -1) else local nodeX, nodeY, nodeZ = getWorldTranslation(self.trafficCollisionTriggers[i]) local x, y, z = course:getWaypointPosition(recordNumber) local nodeDirX, nodeDirY, nodeDirZ, distance = courseplay:getWorldDirection(nodeX,nodeY,nodeZ, x, y, z) local _,_,Z = worldToLocal(self.trafficCollisionTriggers[i], x, y, z) local index = 1 local oldValue = Z while Z < 5.5 do recordNumber = recordNumber + index if recordNumber > course:getNumberOfWaypoints() then -- just a backup break end x, y, z = course:getWaypointPosition(recordNumber) nodeDirX, nodeDirY, nodeDirZ, distance = courseplay:getWorldDirection(nodeX, nodeY, nodeZ, x, y, z) _,_,Z = worldToLocal(self.trafficCollisionTriggers[i], x, y, z) if oldValue > Z then self:setCollisionDirection(self.trafficCollisionTriggers[1], self.trafficCollisionTriggers[i], 0, 1) break end index = index + 1 oldValue = Z end nodeDirX, nodeDirY, nodeDirZ = worldDirectionToLocal(self.trafficCollisionTriggers[i - 1], nodeDirX, nodeDirY, nodeDirZ) self:setCollisionDirection(self.trafficCollisionTriggers[i - 1], self.trafficCollisionTriggers[i], nodeDirX, nodeDirZ) end end end end function CollisionDetector:setCollisionDirection(node, col, colDirX, colDirZ) local parent = getParent(col) local colDirY = 0 if parent ~= node then colDirX, colDirY, colDirZ = worldDirectionToLocal(parent, localDirectionToWorld(node, colDirX, 0, colDirZ)) end if not ( math.abs( colDirX ) < 0.001 and math.abs( colDirZ ) < 0.001 ) then setDirection(col, colDirX, colDirY, colDirZ, 0, 1, 0) end end function CollisionDetector:debug(...) courseplay.debugVehicle(self.debugChannel, self.vehicle, ...) end function CollisionDetector:debugSparse(...) if g_updateLoopIndex % self.debugTicks == 0 then courseplay.debugVehicle(self.debugChannel, self.vehicle, ...) end end function CollisionDetector:setPathVehiclesSpeed(pathVehicle,dt) --print("update speed") if pathVehicle.speedDisplayDt == nil then pathVehicle.speedDisplayDt = 0; pathVehicle.lastSpeed = 0; pathVehicle.lastSpeedReal = 0; pathVehicle.movingDirection = 1; end; pathVehicle.speedDisplayDt = pathVehicle.speedDisplayDt + dt; if pathVehicle.speedDisplayDt > 100 then local newX, newY, newZ = getWorldTranslation(pathVehicle.rootNode); if pathVehicle.lastPosition == nil then pathVehicle.lastPosition = { newX, newY, newZ }; end; local lastMovingDirection = pathVehicle.movingDirection; local dx, dy, dz = worldDirectionToLocal(pathVehicle.rootNode, newX - pathVehicle.lastPosition[1], newY - pathVehicle.lastPosition[2], newZ - pathVehicle.lastPosition[3]); if dz > 0.001 then pathVehicle.movingDirection = 1; elseif dz < -0.001 then pathVehicle.movingDirection = -1; else pathVehicle.movingDirection = 0; end; pathVehicle.lastMovedDistance = MathUtil.vector3Length(dx, dy, dz); local lastLastSpeedReal = pathVehicle.lastSpeedReal; pathVehicle.lastSpeedReal = pathVehicle.lastMovedDistance * 0.01; pathVehicle.lastSpeedAcceleration = (pathVehicle.lastSpeedReal * pathVehicle.movingDirection - lastLastSpeedReal * lastMovingDirection) * 0.01; pathVehicle.lastSpeed = pathVehicle.lastSpeed * 0.85 + pathVehicle.lastSpeedReal * 0.15; pathVehicle.lastPosition[1], pathVehicle.lastPosition[2], pathVehicle.lastPosition[3] = newX, newY, newZ; pathVehicle.speedDisplayDt = pathVehicle.speedDisplayDt - 100; end; end -- adapt collis height to vehicles height function CollisionDetector:adaptCollisHeight() local vehicle = self.vehicle if self.trafficCollisionTriggers[1] ~= nil then local height = 0; local step = (vehicle.sizeLength/2)+1 ; local stepBehind, stepFront = step, step; if vehicle.getAttachedImplements ~= nil then for index, implement in pairs(vehicle:getAttachedImplements()) do local tool = implement.object local x,y,z = getWorldTranslation(tool.rootNode); local _,_,nz = worldToLocal(vehicle.cp.directionNode, x, y, z); if nz > 0 then stepFront = stepFront + (tool.sizeLength)+2 else stepBehind = stepBehind + (tool.sizeLength)+2 end end end local distance = math.max(vehicle.sizeLength,5) local nx, ny, nz = localDirectionToWorld(vehicle.rootNode, 0, -1, 0); vehicle.cp.HeightsFound = 0; vehicle.cp.HeightsFoundColli = 0; for i=-stepBehind,stepFront,0.5 do local x,y,z = localToWorld(vehicle.rootNode, 0, distance, i); raycastAll(x, y, z, nx, ny, nz, "findVehicleHeights", distance, vehicle); --print("drive raycast "..tostring(i).." end"); --cpDebug:drawLine(x, y, z, 1, 0, 0, x+(nx*distance), y+(ny*distance), z+(nz*distance)); end local difference = vehicle.cp.HeightsFound - vehicle.cp.HeightsFoundColli; local trigger = self.trafficCollisionTriggers[1]; local Tx,Ty,Tz = getTranslation(trigger,vehicle.rootNode); setTranslation(trigger, Tx,Ty+difference,Tz); end end