| 152 | function AIDriveStrategyStraight:getDriveData(dt, vX, vY, vZ) |
| 153 | |
| 154 | -- during turn |
| 155 | if self.activeTurnStrategy ~= nil then |
| 156 | self.fieldEndGabDetected = false |
| 157 | self.fieldEndGabLastPos = {} |
| 158 | |
| 159 | self.lastValidTurnLeftPosition = {0,0,0} |
| 160 | self.lastValidTurnLeftValue = 0 |
| 161 | self.lastValidTurnRightPosition = {0,0,0} |
| 162 | self.lastValidTurnRightValue = 0 |
| 163 | |
| 164 | self.gabAllowTurnLeft = true |
| 165 | self.gabAllowTurnRight = true |
| 166 | self.resetGabDetection = true |
| 167 | |
| 168 | self.rowStartTranslation = nil |
| 169 | |
| 170 | local tX, tZ, moveForwards, maxSpeed, distanceToStop = self.activeTurnStrategy:getDriveData(dt, vX,vY,vZ, self.turnData) |
| 171 | if tX ~= nil then |
| 172 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 173 | self.vehicle:addAIDebugText("===> distanceToStop = "..distanceToStop) |
| 174 | end |
| 175 | return tX, tZ, moveForwards, maxSpeed, distanceToStop |
| 176 | else |
| 177 | for _,turnStrategy in pairs(self.turnStrategies) do |
| 178 | turnStrategy:onEndTurn(self.activeTurnStrategy.turnLeft) |
| 179 | end |
| 180 | self.turnLeft = self.activeTurnStrategy.turnLeft |
| 181 | |
| 182 | self.activeTurnStrategy = nil |
| 183 | self.idealTurnStrategy = nil |
| 184 | |
| 185 | -- ToDo: decide wether to measure again ?! (yes for plows, no for all other tools?) |
| 186 | self.turnDataIsStable = false |
| 187 | self.turnDataIsStableCounter = 0 |
| 188 | |
| 189 | self.lastLookAheadDistance = 5 -- 30 |
| 190 | self.foundField = false |
| 191 | self.foundNoBetterTurnStrategy = false |
| 192 | |
| 193 | self.lastHasNoField = false |
| 194 | end |
| 195 | end |
| 196 | |
| 197 | if self.rowStartTranslation == nil then |
| 198 | local x, y, z = getWorldTranslation(self.vehicle:getAIVehicleDirectionNode()) |
| 199 | self.rowStartTranslation = {x, y, z} |
| 200 | end |
| 201 | |
| 202 | -- |
| 203 | local distanceToEndOfField, hasField, ownedField = self:getDistanceToEndOfField(dt, vX,vY,vZ) |
| 204 | local attachedAIImplements = self.vehicle:getAttachedAIImplements() |
| 205 | |
| 206 | if hasField and distanceToEndOfField > 0 then |
| 207 | self.foundField = true |
| 208 | end |
| 209 | |
| 210 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 211 | self.vehicle:addAIDebugText(string.format("==(I)=> distanceToEndOfField: %.1f", distanceToEndOfField)) |
| 212 | end |
| 213 | |
| 214 | -- this is the case when turn is finished and tool is too far away from field |
| 215 | if self.foundField == false and distanceToEndOfField <= 0 and self.turnLeft ~= nil then |
| 216 | local _,_,lz = worldToLocal(self.vehicle:getAIVehicleDirectionNode(), self.vehicle.aiDriveTarget[1], 0, self.vehicle.aiDriveTarget[2]) |
| 217 | if lz > 0 then |
| 218 | distanceToEndOfField = self.lookAheadDistanceField |
| 219 | self.lastHasNoField = false |
| 220 | end |
| 221 | end |
| 222 | |
| 223 | if not hasField and self.foundField ~= true and self.turnLeft == nil then |
| 224 | if ownedField then |
| 225 | self.vehicle:stopAIVehicle(AIVehicle.STOP_REASON_REGULAR) |
| 226 | self:debugPrint("Stopping AIVehicle - unable to find field") |
| 227 | else |
| 228 | self.vehicle:stopAIVehicle(AIVehicle.STOP_REASON_FIELD_NOT_OWNED) |
| 229 | self:debugPrint("Stopping AIVehicle - field not owned") |
| 230 | end |
| 231 | |
| 232 | return nil |
| 233 | end |
| 234 | |
| 235 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 236 | self.vehicle:addAIDebugText(string.format("==(II)=> distanceToEndOfField: %.1f", distanceToEndOfField)) |
| 237 | self.vehicle:addAIDebugText(string.format("===> foundField: %s", tostring(self.foundField))) |
| 238 | self.vehicle:addAIDebugText(string.format("===> lastHasNoField: %s", tostring(self.lastHasNoField))) |
| 239 | if self.turnData ~= nil then |
| 240 | self.vehicle:addAIDebugText(string.format("===> useExtraStraight: %s %s", tostring(self.turnData.useExtraStraightLeft), tostring(self.turnData.useExtraStraightRight))) |
| 241 | end |
| 242 | end |
| 243 | |
| 244 | -- if the field border is found in front of us we continue to drive straight until we reach the field border on the side we turn (or on both sides if we have not decided the direction yet) |
| 245 | -- this helps us to complete fields with a 45 degress field border to 100% |
| 246 | if distanceToEndOfField <= 0 then |
| 247 | for _, implement in ipairs(attachedAIImplements) do |
| 248 | local leftMarker, rightMarker, _ = implement.object:getAIMarkers() |
| 249 | local hasNoFullCoverageArea, _ = implement.object:getAIHasNoFullCoverageArea() |
| 250 | |
| 251 | local allowCheck = self.turnLeft == nil or (self.turnLeft and self.gabAllowTurnRight) or (self.turnLeft == false and self.gabAllowTurnLeft) |
| 252 | allowCheck = allowCheck and not hasNoFullCoverageArea |
| 253 | |
| 254 | if allowCheck then |
| 255 | local dir = self.turnLeft and -1 or 1 |
| 256 | local width = calcDistanceFrom(leftMarker, rightMarker) |
| 257 | local sX, sZ, wX, wZ, hX, hZ = AIVehicleUtil.getAreaDimensions(self.vehicle.aiDriveDirection[1], self.vehicle.aiDriveDirection[2], leftMarker, rightMarker, dir*width, 0, 1, false) |
| 258 | local area, _ = AIVehicleUtil.getAIAreaOfVehicle(implement.object, sX,sZ, wX,wZ, hX,hZ, false) |
| 259 | if area > 0 then |
| 260 | if not AIVehicleUtil.getIsAreaOwned(self.vehicle, sX, sZ, wX, wZ, hX, hZ) then |
| 261 | area = 0 |
| 262 | end |
| 263 | end |
| 264 | |
| 265 | if self.turnLeft == nil then |
| 266 | if not self.gabAllowTurnLeft then |
| 267 | area = 0 |
| 268 | end |
| 269 | |
| 270 | if area <= 0 then |
| 271 | if self.gabAllowTurnRight then |
| 272 | dir = -dir |
| 273 | sX, sZ, wX, wZ, hX, hZ = AIVehicleUtil.getAreaDimensions(self.vehicle.aiDriveDirection[1], self.vehicle.aiDriveDirection[2], leftMarker, rightMarker, dir*width, 0, 1, false) |
| 274 | area, _ = AIVehicleUtil.getAIAreaOfVehicle(implement.object, sX,sZ, wX,wZ, hX,hZ, false) |
| 275 | |
| 276 | if area > 0 then |
| 277 | if not AIVehicleUtil.getIsAreaOwned(self.vehicle, sX, sZ, wX, wZ, hX, hZ) then |
| 278 | area = 0 |
| 279 | end |
| 280 | end |
| 281 | end |
| 282 | end |
| 283 | end |
| 284 | |
| 285 | if area > 0 then |
| 286 | distanceToEndOfField = 5 |
| 287 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 288 | self.vehicle:addAIDebugText(string.format("===> continue until field border on left/right (area: %d)", area)) |
| 289 | end |
| 290 | self.driveExtraDistanceToFieldBorder = true |
| 291 | end |
| 292 | end |
| 293 | end |
| 294 | else |
| 295 | self.driveExtraDistanceToFieldBorder = false |
| 296 | end |
| 297 | |
| 298 | local lookAheadDistance = self.lastLookAheadDistance |
| 299 | |
| 300 | local distanceToCollision = 0 |
| 301 | |
| 302 | if distanceToEndOfField > 0 and not self.useCorridor then |
| 303 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 304 | self.vehicle:addAIDebugText(string.format(" turnDataIsStable: %s | %d", tostring(self.turnDataIsStable), self.turnDataIsStableCounter)) |
| 305 | end |
| 306 | if not self.turnDataIsStable then |
| 307 | self:updateTurnData() |
| 308 | end |
| 309 | |
| 310 | local searchForTurnStrategy = self.idealTurnStrategy == nil |
| 311 | |
| 312 | if self.idealTurnStrategy ~= nil then |
| 313 | distanceToCollision = self.idealTurnStrategy:getDistanceToCollision(dt, vX,vY,vZ, self.turnData, lookAheadDistance) |
| 314 | if distanceToCollision < lookAheadDistance then |
| 315 | searchForTurnStrategy = true |
| 316 | else |
| 317 | self.foundNoBetterTurnStrategy = false |
| 318 | end |
| 319 | end |
| 320 | |
| 321 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 322 | self.vehicle:addAIDebugText(string.format(" searchForTurnStrategy: %s", tostring(searchForTurnStrategy))) |
| 323 | end |
| 324 | |
| 325 | if searchForTurnStrategy and self.foundNoBetterTurnStrategy ~= true then |
| 326 | for i,turnStrategy in pairs(self.turnStrategies) do |
| 327 | if turnStrategy ~= self.idealTurnStrategy then |
| 328 | local colDist = turnStrategy:getDistanceToCollision(dt, vX,vY,vZ, self.turnData, lookAheadDistance) |
| 329 | |
| 330 | if colDist >= lookAheadDistance and not turnStrategy.collisionDetected then |
| 331 | self.idealTurnStrategy = turnStrategy |
| 332 | distanceToCollision = colDist |
| 333 | break |
| 334 | end |
| 335 | end |
| 336 | end |
| 337 | end |
| 338 | |
| 339 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 340 | self.vehicle:addAIDebugText(string.format("===> distanceToCollision: %.1f", distanceToCollision)) |
| 341 | end |
| 342 | |
| 343 | if self.idealTurnStrategy ~= nil then |
| 344 | -- reset valid position of opposite turn side since there is a reason why not to turn into this direction |
| 345 | if self.idealTurnStrategy.turnLeft ~= self.turnLeft then |
| 346 | if self.idealTurnStrategy.turnLeft then |
| 347 | self.lastValidTurnRightPosition[1] = 0 |
| 348 | self.lastValidTurnRightPosition[2] = 0 |
| 349 | self.lastValidTurnRightPosition[3] = 0 |
| 350 | self.lastValidTurnRightValue = 0 |
| 351 | else |
| 352 | self.lastValidTurnLeftPosition[1] = 0 |
| 353 | self.lastValidTurnLeftPosition[2] = 0 |
| 354 | self.lastValidTurnLeftPosition[3] = 0 |
| 355 | self.lastValidTurnLeftValue = 0 |
| 356 | end |
| 357 | end |
| 358 | |
| 359 | self.turnLeft = self.idealTurnStrategy.turnLeft |
| 360 | end |
| 361 | end |
| 362 | |
| 363 | local distanceToTurn = math.min(distanceToEndOfField, distanceToCollision) |
| 364 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 365 | self.vehicle:addAIDebugText(string.format("===> Distance to turn: %.1f", distanceToTurn)) |
| 366 | self.vehicle:addAIDebugText(string.format("===> turnLeft: %s", tostring(self.turnLeft))) |
| 367 | end |
| 368 | |
| 369 | if distanceToCollision < lookAheadDistance and distanceToCollision < distanceToEndOfField then |
| 370 | if self.turnLeft ~= nil or self.idealTurnStrategy == nil then |
| 371 | AIVehicleUtil.updateInvertLeftRightMarkers(self.vehicle, self.vehicle) |
| 372 | for _,implement in pairs(attachedAIImplements) do |
| 373 | AIVehicleUtil.updateInvertLeftRightMarkers(self.vehicle, implement.object) |
| 374 | end |
| 375 | |
| 376 | local leftAreaPercentage, rightAreaPercentage = AIVehicleUtil.getValidityOfTurnDirections(self.vehicle, self.turnData) |
| 377 | |
| 378 | if (self.turnLeft and rightAreaPercentage < AIVehicleUtil.VALID_AREA_THRESHOLD) or (not self.turnLeft and leftAreaPercentage < AIVehicleUtil.VALID_AREA_THRESHOLD) or self.idealTurnStrategy == nil then |
| 379 | self.foundNoBetterTurnStrategy = false |
| 380 | |
| 381 | local collision = self.turnStrategies[1]:checkCollisionInFront(self.turnData) |
| 382 | if collision or distanceToEndOfField <= 0 then |
| 383 | self.foundNoBetterTurnStrategy = false |
| 384 | self.idealTurnStrategy = nil |
| 385 | |
| 386 | if self.turnLeft == nil then |
| 387 | self.vehicle:stopAIVehicle(AIVehicle.STOP_REASON_REGULAR) |
| 388 | return nil |
| 389 | end |
| 390 | |
| 391 | else |
| 392 | distanceToTurn = lookAheadDistance |
| 393 | end |
| 394 | end |
| 395 | |
| 396 | end |
| 397 | end |
| 398 | |
| 399 | -- if we collide to the left or to the right and we still got some area to work in front of us |
| 400 | -- we check if there is no collision and still valid ground in the front and continue to work |
| 401 | -- as soon as we reach the field end or a collision we will start to reverse and turn in front of the collision |
| 402 | if self.allowTurnBackward or self.aiToolReverserDirectionNode ~= nil then |
| 403 | if distanceToTurn <= 0 and distanceToEndOfField > 0 then |
| 404 | local collision = self.turnStrategies[1]:checkCollisionInFront(self.turnData, 0) |
| 405 | |
| 406 | -- if the vehicle is somehow blocked and can not move (slopes etc.) we cancel the corridor and start to reverse |
| 407 | if not collision then |
| 408 | if self.vehicle:getLastSpeed() < 1.5 then |
| 409 | self.useCorridorTimeOut = self.useCorridorTimeOut + dt |
| 410 | else |
| 411 | self.useCorridorTimeOut = 0 |
| 412 | end |
| 413 | |
| 414 | if self.useCorridorTimeOut > 3000 then |
| 415 | collision = true |
| 416 | end |
| 417 | end |
| 418 | |
| 419 | if not collision then |
| 420 | distanceToTurn = distanceToEndOfField |
| 421 | self.useCorridor = true |
| 422 | if self.useCorridorStart == nil then |
| 423 | self.useCorridorStart = {vX, vY, vZ} |
| 424 | else |
| 425 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 426 | local distance = MathUtil.vector3Length(self.useCorridorStart[1]-vX, self.useCorridorStart[2]-vY,self.useCorridorStart[3]-vZ) |
| 427 | self.vehicle:addAIDebugText(string.format("===> is using a corridor (%.1fm)", distance)) |
| 428 | end |
| 429 | end |
| 430 | else |
| 431 | self.useCorridor = false |
| 432 | end |
| 433 | else |
| 434 | self.useCorridor = false |
| 435 | end |
| 436 | end |
| 437 | |
| 438 | if distanceToTurn <= 0 and not self.useCorridor then |
| 439 | -- call end line for all tools that were still 'on field' |
| 440 | for _, implement in ipairs(attachedAIImplements) do |
| 441 | if implement.aiEndLineCalled == nil or not implement.aiEndLineCalled then |
| 442 | implement.aiEndLineCalled = true |
| 443 | implement.aiStartLineCalled = nil |
| 444 | |
| 445 | implement.object:aiImplementEndLine() |
| 446 | |
| 447 | local rootVehicle = implement.object:getRootVehicle() |
| 448 | rootVehicle:raiseStateChange(Vehicle.STATE_CHANGE_AI_END_LINE) |
| 449 | end |
| 450 | end |
| 451 | |
| 452 | self.lastHasNoField = false -- reset state for end of field check |
| 453 | |
| 454 | self.activeTurnStrategy = self.idealTurnStrategy |
| 455 | if self.turnData ~= nil and self.activeTurnStrategy ~= nil then |
| 456 | if self.useCorridorStart ~= nil then |
| 457 | local distance = MathUtil.vector3Length(self.useCorridorStart[1]-vX, self.useCorridorStart[2]-vY,self.useCorridorStart[3]-vZ) |
| 458 | self.corridorDistance = distance |
| 459 | self.useCorridorStart = nil |
| 460 | self:debugPrint(string.format("start turn with corridor offset: %.2f", distance)) |
| 461 | end |
| 462 | local canTurn = self.activeTurnStrategy:startTurn(self) |
| 463 | self.activeTurnStrategy.lastValidTurnPositionOffset = 0 |
| 464 | self.corridorDistance = 0 |
| 465 | if not canTurn then |
| 466 | self.vehicle:stopAIVehicle(AIVehicle.STOP_REASON_REGULAR) |
| 467 | self:debugPrint("Stopping AIVehicle - could not start to turn") |
| 468 | |
| 469 | return nil |
| 470 | end |
| 471 | return self.activeTurnStrategy:getDriveData(dt, vX,vY,vZ, self.turnData) |
| 472 | else |
| 473 | self.vehicle:stopAIVehicle(AIVehicle.STOP_REASON_REGULAR) |
| 474 | end |
| 475 | return nil |
| 476 | else |
| 477 | self.vehicle:addAIDebugText("===> Drive straight") |
| 478 | return self:getDriveStraightData(dt, vX,vY,vZ, distanceToTurn, distanceToEndOfField) |
| 479 | end |
| 480 | |
| 481 | end |
| 485 | function AIDriveStrategyStraight:getDriveStraightData(dt, vX, vY, vZ, distanceToTurn, distanceToEndOfField) |
| 486 | if self.vehicle.aiDriveDirection == nil then |
| 487 | return nil, nil, true, 0, 0 |
| 488 | end |
| 489 | |
| 490 | -- drive along the desired direction |
| 491 | local pX, pZ = MathUtil.projectOnLine(vX, vZ, self.vehicle.aiDriveTarget[1], self.vehicle.aiDriveTarget[2], self.vehicle.aiDriveDirection[1], self.vehicle.aiDriveDirection[2]) |
| 492 | local tX = pX + self.vehicle.aiDriveDirection[1] * self.vehicle.maxTurningRadius |
| 493 | local tZ = pZ + self.vehicle.aiDriveDirection[2] * self.vehicle.maxTurningRadius |
| 494 | |
| 495 | local maxSpeed = self.vehicle:getSpeedLimit() |
| 496 | local attachedAIImplements = self.vehicle:getAttachedAIImplements() |
| 497 | |
| 498 | |
| 499 | -- 1: raise, nil: stay, -1: lower |
| 500 | for i=#self.toolLineStates, 1, -1 do |
| 501 | self.toolLineStates[i] = nil |
| 502 | end |
| 503 | |
| 504 | |
| 505 | local nrOfImplements = #attachedAIImplements |
| 506 | for i=nrOfImplements,1,-1 do |
| 507 | local implement = attachedAIImplements[i] |
| 508 | |
| 509 | if self.toolLineStates[i+1] == -1 and attachedAIImplements[i+1].object:getAttacherVehicle() == implement.object then |
| 510 | self.toolLineStates[i] = -1 |
| 511 | else |
| 512 | local leftMarker, rightMarker, backMarker = implement.object:getAIMarkers() |
| 513 | -- we apply a savety offset here cause on same implements the markers are moved on x axis while lifting the tool |
| 514 | -- with this offset we don't check the next row |
| 515 | local safetyOffset = 0.2 |
| 516 | |
| 517 | local markerZOffset = 0 |
| 518 | local _, _, areaLength = localToLocal(backMarker, leftMarker, 0,0,0) |
| 519 | local size = 1 |
| 520 | local doAdditionalFieldEndChecks = false |
| 521 | local hasNoFullCoverageArea, hasNoFullCoverageAreaOffset = implement.object:getAIHasNoFullCoverageArea() |
| 522 | if hasNoFullCoverageArea then |
| 523 | markerZOffset = areaLength |
| 524 | size = math.abs(markerZOffset) + hasNoFullCoverageAreaOffset |
| 525 | doAdditionalFieldEndChecks = true |
| 526 | end |
| 527 | |
| 528 | local getAreaDimensions = function(leftNode, rightNode, xOffset, zOffset, areaSize, invertXOffset) |
| 529 | local xOffsetLeft, xOffsetRight = xOffset, xOffset |
| 530 | if invertXOffset == nil or invertXOffset then |
| 531 | xOffsetLeft = -xOffsetLeft |
| 532 | end |
| 533 | local lX, _, lZ = localToWorld(leftNode, xOffsetLeft, 0, zOffset) |
| 534 | local rX, _, rZ = localToWorld(rightNode, xOffsetRight, 0, zOffset) |
| 535 | |
| 536 | local sX = lX - (0.5 * self.vehicle.aiDriveDirection[1]) |
| 537 | local sZ = lZ - (0.5 * self.vehicle.aiDriveDirection[2]) |
| 538 | local wX = rX - (0.5 * self.vehicle.aiDriveDirection[1]) |
| 539 | local wZ = rZ - (0.5 * self.vehicle.aiDriveDirection[2]) |
| 540 | local hX = lX + (areaSize * self.vehicle.aiDriveDirection[1]) |
| 541 | local hZ = lZ + (areaSize * self.vehicle.aiDriveDirection[2]) |
| 542 | |
| 543 | return sX, sZ, wX, wZ, hX, hZ |
| 544 | end |
| 545 | |
| 546 | local sX, sZ, wX, wZ, hX, hZ = getAreaDimensions(leftMarker, rightMarker, safetyOffset, markerZOffset, size) |
| 547 | |
| 548 | local area, totalArea = AIVehicleUtil.getAIAreaOfVehicle(implement.object, sX,sZ, wX,wZ, hX,hZ, false) |
| 549 | if area / totalArea > 0.025 then |
| 550 | if not self.fieldEndGabDetected then |
| 551 | self.toolLineStates[i] = -1 |
| 552 | end |
| 553 | |
| 554 | -- detect gab at the end of the field to another field |
| 555 | -- needed since the plow check area is as long as the plow is and this length can be longer than the distance between fields |
| 556 | -- so if the back of the plow is on a field, the center not and the front also on a field we reached the headland |
| 557 | if doAdditionalFieldEndChecks then |
| 558 | local distance1 = 0 |
| 559 | local distance2 = 0 |
| 560 | |
| 561 | local sX1, sZ1, wX1, wZ1, hX1, hZ1 = getAreaDimensions(leftMarker, rightMarker, safetyOffset, 1, 0.75) |
| 562 | local area2, _ = AIVehicleUtil.getAIAreaOfVehicle(implement.object, sX1,sZ1, wX1,wZ1, hX1,hZ1, false) |
| 563 | if #self.fieldEndGabLastPos > 0 then |
| 564 | distance1 = math.abs(MathUtil.vector2Length(sX1-self.fieldEndGabLastPos[1], sZ1-self.fieldEndGabLastPos[2])) |
| 565 | end |
| 566 | |
| 567 | local sX2, sZ2, wX2, wZ2, hX2, hZ2 = getAreaDimensions(leftMarker, rightMarker, safetyOffset, -2, 0.75) |
| 568 | local area3, _ = AIVehicleUtil.getAIAreaOfVehicle(implement.object, sX2,sZ2, wX2,wZ2, hX2,hZ2, false) |
| 569 | if #self.fieldEndGabLastPos > 0 then |
| 570 | distance2 = math.abs(MathUtil.vector2Length(sX2-self.fieldEndGabLastPos[1], sZ2-self.fieldEndGabLastPos[2])) |
| 571 | end |
| 572 | |
| 573 | if area3 == 0 and area2 > 0 and distance1 > 0 and distance2 > 0 then |
| 574 | if distance1 > 3 then |
| 575 | if distance1 > distance2 then |
| 576 | self.fieldEndGabDetected = true |
| 577 | self.toolLineStates[i] = 1 |
| 578 | end |
| 579 | end |
| 580 | end |
| 581 | |
| 582 | if area2 > 0 then |
| 583 | self.fieldEndGabLastPos[1] = sX1 |
| 584 | self.fieldEndGabLastPos[2] = sZ1 |
| 585 | end |
| 586 | end |
| 587 | |
| 588 | -- check the left and right side of the tool for valid ground |
| 589 | -- in case we got a "L" field we save the last valid position and reverse to that position if we reach the headland but don't have valid ground |
| 590 | if not self.driveExtraDistanceToFieldBorder then |
| 591 | local usedAreaLength = math.abs(areaLength) |
| 592 | if markerZOffset ~= 0 then |
| 593 | usedAreaLength = 0 |
| 594 | end |
| 595 | |
| 596 | local dir = self.turnLeft and -1 or 1 |
| 597 | local width = calcDistanceFrom(leftMarker, rightMarker) |
| 598 | sX, sZ, wX, wZ, hX, hZ = getAreaDimensions(leftMarker, rightMarker, dir*width, markerZOffset-usedAreaLength, size, false) |
| 599 | area, _ = AIVehicleUtil.getAIAreaOfVehicle(implement.object, sX,sZ, wX,wZ, hX,hZ, false) |
| 600 | local x, y, z = 0, 0, 0 |
| 601 | |
| 602 | if not AIVehicleUtil.getIsAreaOwned(self.vehicle, sX, sZ, wX, wZ, hX, hZ) then |
| 603 | area = 0 |
| 604 | end |
| 605 | |
| 606 | if area > 0 then |
| 607 | x, y, z = getWorldTranslation(self.vehicle:getAIVehicleDirectionNode()) |
| 608 | end |
| 609 | |
| 610 | -- eather check the other side if we found nothing |
| 611 | -- or if we don't have a turn direction set yet so we check in both directions to be able to turn in both |
| 612 | if area == 0 or self.turnLeft == nil then |
| 613 | sX, sZ, wX, wZ, hX, hZ = getAreaDimensions(leftMarker, rightMarker, -dir*width, markerZOffset-usedAreaLength, size, false) |
| 614 | local areaOpp, _ = AIVehicleUtil.getAIAreaOfVehicle(implement.object, sX,sZ, wX,wZ, hX,hZ, false) |
| 615 | |
| 616 | if not AIVehicleUtil.getIsAreaOwned(self.vehicle, sX, sZ, wX, wZ, hX, hZ) then |
| 617 | areaOpp = 0 |
| 618 | end |
| 619 | |
| 620 | if areaOpp > 0 then |
| 621 | x, y, z = getWorldTranslation(self.vehicle:getAIVehicleDirectionNode()) |
| 622 | end |
| 623 | |
| 624 | if self.turnLeft ~= nil then |
| 625 | area = areaOpp |
| 626 | dir = -dir |
| 627 | else |
| 628 | -- if we don't have a turn direction set yet we check in both directions for valid ground and set the lastValidPosition on both sides |
| 629 | if area > 0 then |
| 630 | self.lastValidTurnLeftPosition[1], self.lastValidTurnLeftPosition[2], self.lastValidTurnLeftPosition[3] = x, y, z |
| 631 | self.lastValidTurnLeftValue = math.max(area, self.lastValidTurnLeftValue) |
| 632 | end |
| 633 | |
| 634 | if areaOpp > 0 then |
| 635 | self.lastValidTurnRightPosition[1], self.lastValidTurnRightPosition[2], self.lastValidTurnRightPosition[3] = x, y, z |
| 636 | self.lastValidTurnRightValue = math.max(area, self.lastValidTurnRightValue) |
| 637 | end |
| 638 | end |
| 639 | end |
| 640 | |
| 641 | if self.turnLeft ~= nil then |
| 642 | if area > 0 then |
| 643 | if dir > 0 then |
| 644 | if area > self.lastValidTurnRightValue then |
| 645 | self.lastValidTurnLeftPosition[1], self.lastValidTurnLeftPosition[2], self.lastValidTurnLeftPosition[3] = x, y, z |
| 646 | self.lastValidTurnLeftValue = math.max(area, self.lastValidTurnLeftValue) |
| 647 | self.lastValidTurnRightPosition[1], self.lastValidTurnRightPosition[2], self.lastValidTurnRightPosition[3] = 0, 0, 0 |
| 648 | end |
| 649 | else |
| 650 | if area > self.lastValidTurnLeftValue then |
| 651 | self.lastValidTurnRightPosition[1], self.lastValidTurnRightPosition[2], self.lastValidTurnRightPosition[3] = x, y, z |
| 652 | self.lastValidTurnRightValue = math.max(area, self.lastValidTurnRightValue) |
| 653 | self.lastValidTurnLeftPosition[1], self.lastValidTurnLeftPosition[2], self.lastValidTurnLeftPosition[3] = 0, 0, 0 |
| 654 | end |
| 655 | end |
| 656 | end |
| 657 | end |
| 658 | |
| 659 | self.lastValidTurnCheckPosition[1], self.lastValidTurnCheckPosition[2], self.lastValidTurnCheckPosition[3] = getWorldTranslation(self.vehicle:getAIVehicleDirectionNode()) |
| 660 | |
| 661 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 662 | DebugUtil.drawDebugGizmoAtWorldPos(self.lastValidTurnLeftPosition[1], self.lastValidTurnLeftPosition[2], self.lastValidTurnLeftPosition[3], 0, 1, 0, 0, 1, 0, "last valid left", true) |
| 663 | DebugUtil.drawDebugGizmoAtWorldPos(self.lastValidTurnRightPosition[1], self.lastValidTurnRightPosition[2], self.lastValidTurnRightPosition[3], 0, 1, 0, 0, 1, 0, "last valid right", true) |
| 664 | end |
| 665 | end |
| 666 | else |
| 667 | -- If we don't need to lower, check if we need to raise it, either not on field or nothing close by (check slightly further ahead than for lowering) |
| 668 | if self.lastHasNoField then |
| 669 | self.toolLineStates[i] = 1 |
| 670 | else |
| 671 | local lX, _, lZ = localToWorld(leftMarker, -safetyOffset, 0, markerZOffset) |
| 672 | local hX2 = lX + (math.max(distanceToTurn, 2.5) * self.vehicle.aiDriveDirection[1]) |
| 673 | local hZ2 = lZ + (math.max(distanceToTurn, 2.5) * self.vehicle.aiDriveDirection[2]) |
| 674 | |
| 675 | local area2, _ = AIVehicleUtil.getAIAreaOfVehicle(implement.object, sX,sZ, wX,wZ, hX2,hZ2, false) |
| 676 | if area2 <= 0 then |
| 677 | self.toolLineStates[i] = 1 |
| 678 | end |
| 679 | end |
| 680 | end |
| 681 | end |
| 682 | end |
| 683 | |
| 684 | for i=nrOfImplements,1,-1 do |
| 685 | local implement = attachedAIImplements[i] |
| 686 | |
| 687 | if implement.aiLastStateChangeDistance == nil then |
| 688 | implement.aiLastStateChangeDistance = 0 |
| 689 | end |
| 690 | |
| 691 | -- allow state changes only after the vehicle has been moved at least 25cm |
| 692 | -- this helps us if the area position slightly changes while lowering/lifting |
| 693 | -- and the result of the ai area checks is then different -> this can result in an endless lower/lift cycle [Bug #32754] |
| 694 | implement.aiLastStateChangeDistance = implement.aiLastStateChangeDistance + implement.object.lastMovedDistance |
| 695 | if implement.aiLastStateChangeDistance > 0.25 then |
| 696 | if self.toolLineStates[i] == -1 then |
| 697 | if implement.aiStartLineCalled == nil or not implement.aiStartLineCalled then |
| 698 | implement.aiStartLineCalled = true |
| 699 | implement.aiEndLineCalled = nil |
| 700 | |
| 701 | implement.object:aiImplementStartLine() |
| 702 | implement.aiLastStateChangeDistance = 0 |
| 703 | |
| 704 | local rootVehicle = implement.object:getRootVehicle() |
| 705 | rootVehicle:raiseStateChange(Vehicle.STATE_CHANGE_AI_START_LINE) |
| 706 | end |
| 707 | elseif self.toolLineStates[i] == 1 then |
| 708 | if implement.aiEndLineCalled == nil or not implement.aiEndLineCalled then |
| 709 | implement.aiEndLineCalled = true |
| 710 | implement.aiStartLineCalled = nil |
| 711 | |
| 712 | implement.object:aiImplementEndLine() |
| 713 | implement.aiLastStateChangeDistance = 0 |
| 714 | |
| 715 | local rootVehicle = implement.object:getRootVehicle() |
| 716 | rootVehicle:raiseStateChange(Vehicle.STATE_CHANGE_AI_END_LINE) |
| 717 | end |
| 718 | end |
| 719 | end |
| 720 | end |
| 721 | |
| 722 | local canContinueWork = self.vehicle:getCanAIVehicleContinueWork() |
| 723 | if not canContinueWork then |
| 724 | maxSpeed = 0 |
| 725 | end |
| 726 | |
| 727 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 728 | self.vehicle:addAIDebugText(string.format("===> canContinueWork: %s", tostring(canContinueWork))) |
| 729 | self.vehicle:addAIDebugLine({vX,vY,vZ}, {tX,vY,tZ}, {1,1,1}) |
| 730 | end |
| 731 | |
| 732 | --detect gabs to other fields |
| 733 | self.gabAllowTurnLeft = true |
| 734 | self.gabAllowTurnRight = true |
| 735 | local gabBits = "" |
| 736 | local gabPos = -1 |
| 737 | |
| 738 | -- do gab detection only for the tool with the higest working width |
| 739 | -- assumes that the tools are aligned well behind the tractor and the widest tool covers the full width |
| 740 | local maxWidth = -math.huge |
| 741 | local maxWidthVehicle |
| 742 | for _, implement in ipairs(attachedAIImplements) do |
| 743 | local leftMarker, rightMarker, _ = implement.object:getAIMarkers() |
| 744 | |
| 745 | -- only calculate for tool with the biggest width |
| 746 | local width = calcDistanceFrom(leftMarker, rightMarker) + 0.8 |
| 747 | if width > maxWidth then |
| 748 | maxWidth = width |
| 749 | maxWidthVehicle = implement.object |
| 750 | end |
| 751 | end |
| 752 | |
| 753 | if maxWidthVehicle ~= nil then |
| 754 | local leftMarker, rightMarker, _ = maxWidthVehicle:getAIMarkers() |
| 755 | |
| 756 | local changeCounter = 0 |
| 757 | local lastBit = false |
| 758 | local gabPosCount = 0 |
| 759 | |
| 760 | local divisions = 2.5 |
| 761 | if maxWidth < 8.5 then |
| 762 | divisions = 1.5 |
| 763 | end |
| 764 | if maxWidth < 4.5 then |
| 765 | divisions = 1 |
| 766 | end |
| 767 | |
| 768 | local checkpoints = maxWidthVehicle.aiImplementGabCheckpoints |
| 769 | if checkpoints == nil then |
| 770 | checkpoints = MathUtil.round(maxWidth / divisions, 0) |
| 771 | if checkpoints > 0 then |
| 772 | checkpoints = checkpoints + 1 |
| 773 | end |
| 774 | |
| 775 | maxWidthVehicle.aiImplementGabCheckpoints = checkpoints |
| 776 | end |
| 777 | if maxWidthVehicle.aiImplementGabCheckpointValues == nil or self.resetGabDetection then |
| 778 | maxWidthVehicle.aiImplementGabCheckpointValues = {} |
| 779 | self.resetGabDetection = false |
| 780 | end |
| 781 | local values = maxWidthVehicle.aiImplementGabCheckpointValues |
| 782 | |
| 783 | maxWidthVehicle.aiImplementCurCheckpoint = (maxWidthVehicle.aiImplementCurCheckpoint or -1) + 1 |
| 784 | if maxWidthVehicle.aiImplementCurCheckpoint >= checkpoints then |
| 785 | maxWidthVehicle.aiImplementCurCheckpoint = 0 |
| 786 | end |
| 787 | local currentCheckpoint = maxWidthVehicle.aiImplementCurCheckpoint |
| 788 | |
| 789 | if checkpoints > 2 then |
| 790 | local checkpointWidth = maxWidth / (checkpoints - 1) |
| 791 | |
| 792 | local x1, y1, z1 = localToWorld(leftMarker, 0.4, 0, 0) |
| 793 | local x2, y2, z2 = localToWorld(rightMarker, -0.4, 0, 0) |
| 794 | |
| 795 | local x = x1 - ((x1 - x2) * ((currentCheckpoint*checkpointWidth) / maxWidth)) |
| 796 | local y = y1 - ((y1 - y2) * ((currentCheckpoint*checkpointWidth) / maxWidth)) |
| 797 | local z = z1 - ((z1 - z2) * ((currentCheckpoint*checkpointWidth) / maxWidth)) |
| 798 | |
| 799 | |
| 800 | local isOnField = getDensityAtWorldPos(g_currentMission.terrainDetailId, x, y, z) ~= 0 |
| 801 | local bit = values[currentCheckpoint+1] |
| 802 | bit = bit or isOnField |
| 803 | |
| 804 | values[currentCheckpoint+1] = bit |
| 805 | |
| 806 | for i=1, checkpoints do |
| 807 | if values[i] ~= lastBit then |
| 808 | changeCounter = changeCounter + 1 |
| 809 | if changeCounter == 2 then |
| 810 | gabPosCount = i/checkpoints |
| 811 | elseif changeCounter == 3 then |
| 812 | gabPosCount = gabPosCount + (i-1)/checkpoints |
| 813 | gabPos = gabPosCount / 2 |
| 814 | end |
| 815 | lastBit = values[i] |
| 816 | end |
| 817 | |
| 818 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 819 | gabBits = gabBits .. tostring((values[i] and 1) or 0) |
| 820 | end |
| 821 | end |
| 822 | |
| 823 | -- block turning on both directions if we found a permanent gab in the field |
| 824 | -- this blocked ai working if you start the ai on a gab |
| 825 | -- but correctly behaves if you reach the field border on the other side of the field |
| 826 | local hasGab = gabPos > 0 |
| 827 | self.gabAllowTurnLeft = self.gabAllowTurnLeft and values[1] and not hasGab |
| 828 | self.gabAllowTurnRight = self.gabAllowTurnRight and values[#values] and not hasGab |
| 829 | end |
| 830 | end |
| 831 | |
| 832 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 833 | self.vehicle:addAIDebugText(string.format("===> gab bits: %s (%s)", gabBits, Utils.getFilenameInfo(maxWidthVehicle.configFileName, true))) |
| 834 | if gabPos > 0 then |
| 835 | self.vehicle:addAIDebugText(string.format("===> gab pos: %.2f%% side: %s", gabPos*100, (gabPos < 0.5 and "left") or "right")) |
| 836 | end |
| 837 | self.vehicle:addAIDebugText(string.format("===> gab allow Left: %s", self.gabAllowTurnLeft)) |
| 838 | self.vehicle:addAIDebugText(string.format("===> gab allow right: %s", self.gabAllowTurnRight)) |
| 839 | end |
| 840 | |
| 841 | return tX, tZ, true, maxSpeed, distanceToTurn |
| 842 | end |
| 36 | function AIDriveStrategyStraight:setAIVehicle(vehicle) |
| 37 | AIDriveStrategyStraight:superClass().setAIVehicle(self, vehicle) |
| 38 | |
| 39 | --# set AI direction acccording to current orientation |
| 40 | local dx,_,dz = localDirectionToWorld(self.vehicle:getAIVehicleDirectionNode(), 0, 0, 1) |
| 41 | if g_currentMission.snapAIDirection then |
| 42 | local snapAngle = self.vehicle:getDirectionSnapAngle() |
| 43 | snapAngle = math.max(snapAngle, math.pi/(g_currentMission.terrainDetailAngleMaxValue+1)) |
| 44 | |
| 45 | local angleRad = MathUtil.getYRotationFromDirection(dx, dz) |
| 46 | angleRad = math.floor(angleRad / snapAngle + 0.5) * snapAngle |
| 47 | |
| 48 | dx, dz = MathUtil.getDirectionFromYRotation(angleRad) |
| 49 | else |
| 50 | local length = MathUtil.vector2Length(dx,dz) |
| 51 | dx = dx / length |
| 52 | dz = dz / length |
| 53 | end |
| 54 | self.vehicle.aiDriveDirection = {dx, dz} |
| 55 | |
| 56 | local x,_,z = getWorldTranslation(self.vehicle:getAIVehicleDirectionNode()) |
| 57 | self.vehicle.aiDriveTarget = {x, z} |
| 58 | |
| 59 | --# create turn strategy |
| 60 | local useDefault = true |
| 61 | |
| 62 | -- check if all attached implements allow turning backward |
| 63 | self.allowTurnBackward = AIVehicleUtil.getAttachedImplementsAllowTurnBackward(vehicle) |
| 64 | |
| 65 | if not self.allowTurnBackward then |
| 66 | useDefault = false |
| 67 | end |
| 68 | |
| 69 | self.aiToolReverserDirectionNode = AIVehicleUtil.getAIToolReverserDirectionNode(self.vehicle) |
| 70 | self.vehicleAIReverserNode = self.vehicle:getAIVehicleReverserNode() |
| 71 | |
| 72 | self.turnStrategies = {} |
| 73 | |
| 74 | local usedStrategies = '' |
| 75 | |
| 76 | if useDefault then |
| 77 | usedStrategies = usedStrategies .. " +DEFAULT " |
| 78 | table.insert(self.turnStrategies, AITurnStrategyDefault:new()) |
| 79 | |
| 80 | usedStrategies = usedStrategies .. " +DEFAULT (reverse)" |
| 81 | table.insert(self.turnStrategies, AITurnStrategyDefaultReverse:new()) |
| 82 | end |
| 83 | |
| 84 | if self.aiToolReverserDirectionNode ~= nil or useDefault or self.vehicleAIReverserNode then |
| 85 | usedStrategies = usedStrategies .. " +BULBs (reverse)" |
| 86 | table.insert(self.turnStrategies, AITurnStrategyBulb1Reverse:new()) |
| 87 | table.insert(self.turnStrategies, AITurnStrategyBulb2Reverse:new()) |
| 88 | table.insert(self.turnStrategies, AITurnStrategyBulb3Reverse:new()) |
| 89 | else |
| 90 | usedStrategies = usedStrategies .. " +BULBs" |
| 91 | table.insert(self.turnStrategies, AITurnStrategyBulb1:new()) |
| 92 | table.insert(self.turnStrategies, AITurnStrategyBulb2:new()) |
| 93 | table.insert(self.turnStrategies, AITurnStrategyBulb3:new()) |
| 94 | end |
| 95 | |
| 96 | if self.aiToolReverserDirectionNode ~= nil or useDefault or self.vehicleAIReverserNode then |
| 97 | usedStrategies = usedStrategies .. " +HALFCIRCLE (reverse)" |
| 98 | table.insert(self.turnStrategies, AITurnStrategyHalfCircleReverse:new()) |
| 99 | else |
| 100 | usedStrategies = usedStrategies .. " +HALFCIRCLE" |
| 101 | table.insert(self.turnStrategies, AITurnStrategyHalfCircle:new()) |
| 102 | end |
| 103 | |
| 104 | if VehicleDebug.state == VehicleDebug.DEBUG_AI then |
| 105 | print("AI is using strategies: "..usedStrategies.." for "..tostring(self.vehicle.configFileName)) |
| 106 | end |
| 107 | |
| 108 | for _,turnStrategy in pairs(self.turnStrategies) do |
| 109 | turnStrategy:setAIVehicle(self.vehicle, self) |
| 110 | end |
| 111 | |
| 112 | self.activeTurnStrategy = nil |
| 113 | |
| 114 | self.turnDataIsStable = false |
| 115 | self.turnDataIsStableCounter = 0 |
| 116 | |
| 117 | self.fieldEndGabDetected = false |
| 118 | self.fieldEndGabLastPos = {} |
| 119 | self.gabAllowTurnLeft = true |
| 120 | self.gabAllowTurnRight = true |
| 121 | self.resetGabDetection = true |
| 122 | |
| 123 | self.lastValidTurnLeftPosition = {0,0,0} |
| 124 | self.lastValidTurnLeftValue = 0 |
| 125 | self.lastValidTurnRightPosition = {0,0,0} |
| 126 | self.lastValidTurnRightValue = 0 |
| 127 | self.lastValidTurnCheckPosition = {0,0,0} |
| 128 | |
| 129 | self.useCorridor = false |
| 130 | self.useCorridorStart = nil |
| 131 | self.useCorridorTimeOut = 0 |
| 132 | |
| 133 | self.rowStartTranslation = nil |
| 134 | |
| 135 | self.lastLookAheadDistance = 5 |
| 136 | |
| 137 | self.driveExtraDistanceToFieldBorder = false |
| 138 | |
| 139 | self.toolLineStates = {} |
| 140 | end |
| 847 | function AIDriveStrategyStraight:updateTurnData() |
| 848 | -- calculate basic data, which is needed by turn strategies |
| 849 | self.turnData = Utils.getNoNil(self.turnData, {}) |
| 850 | |
| 851 | local attachedAIImplements = self.vehicle:getAttachedAIImplements() |
| 852 | |
| 853 | -- determine turning radius |
| 854 | self.turnData.radius = self.vehicle.maxTurningRadius * 1.1 -- needs ackermann steering |
| 855 | if self.vehicle:getAIMinTurningRadius() ~= nil then |
| 856 | self.turnData.radius = math.max(self.turnData.radius, self.vehicle:getAIMinTurningRadius()) |
| 857 | end |
| 858 | |
| 859 | local maxToolRadius = 0 |
| 860 | for _,implement in pairs(attachedAIImplements) do |
| 861 | maxToolRadius = math.max(maxToolRadius, AIVehicleUtil.getMaxToolRadius(implement)) |
| 862 | end |
| 863 | self.turnData.radius = math.max(self.turnData.radius, maxToolRadius) |
| 864 | |
| 865 | -- determine tool with smallest AI area and zOffsets for estimation of waypoints/segments |
| 866 | local minWidthOfAIArea = math.huge |
| 867 | self.turnData.maxZOffset = -math.huge |
| 868 | self.turnData.minZOffset = math.huge |
| 869 | self.turnData.aiAreaMaxX = -math.huge |
| 870 | self.turnData.aiAreaMinX = math.huge |
| 871 | |
| 872 | local lastTypeName = nil |
| 873 | local allImplementsOfSameType = true |
| 874 | |
| 875 | for _,implement in pairs(attachedAIImplements) do |
| 876 | if lastTypeName == nil then |
| 877 | lastTypeName = implement.object.typeName |
| 878 | end |
| 879 | |
| 880 | allImplementsOfSameType = allImplementsOfSameType and lastTypeName == implement.object.typeName |
| 881 | |
| 882 | local leftMarker, rightMarker, backMarker = implement.object:getAIMarkers() |
| 883 | |
| 884 | local xL,_,zL = localToLocal(leftMarker, self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
| 885 | local xR,_,zR = localToLocal(rightMarker, self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
| 886 | local xB,_,zB = localToLocal(backMarker, self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
| 887 | |
| 888 | local lrDistance = math.abs(xL - xR) |
| 889 | if lrDistance < minWidthOfAIArea then |
| 890 | minWidthOfAIArea = lrDistance |
| 891 | self.turnData.minAreaImplement = implement |
| 892 | end |
| 893 | |
| 894 | self.turnData.aiAreaMinX = math.min(self.turnData.aiAreaMinX, xL, xR, xB) |
| 895 | self.turnData.aiAreaMaxX = math.max(self.turnData.aiAreaMaxX, xL, xR, xB) |
| 896 | |
| 897 | self.turnData.maxZOffset = math.max(self.turnData.maxZOffset, math.max(zL, zR)) |
| 898 | self.turnData.minZOffset = math.min(self.turnData.minZOffset, math.min(zL, zR)) |
| 899 | end |
| 900 | |
| 901 | self.turnData.allImplementsOfSameType = allImplementsOfSameType |
| 902 | |
| 903 | if self.turnData.maxZOffset == self.turnData.minZOffset then |
| 904 | self.turnData.zOffset = 2 * self.turnData.maxZOffset |
| 905 | self.turnData.zOffsetTurn = math.max(1, 2 * self.turnData.maxZOffset) |
| 906 | else |
| 907 | if self.turnData.maxZOffset > 0 and self.turnData.minZOffset < 0 then |
| 908 | self.turnData.zOffset = self.turnData.minZOffset + self.turnData.maxZOffset |
| 909 | self.turnData.zOffsetTurn = math.max(1, self.turnData.minZOffset + self.turnData.maxZOffset) |
| 910 | elseif self.turnData.maxZOffset > 0 and self.turnData.minZOffset > 0 then |
| 911 | self.turnData.zOffset = 2 * self.turnData.maxZOffset |
| 912 | self.turnData.zOffsetTurn = math.max(1, 2 * self.turnData.maxZOffset) |
| 913 | elseif self.turnData.maxZOffset < 0 and self.turnData.minZOffset < 0 then |
| 914 | self.turnData.zOffset = self.turnData.minZOffset + self.turnData.maxZOffset |
| 915 | self.turnData.zOffsetTurn = math.max(1, self.turnData.minZOffset + self.turnData.maxZOffset) |
| 916 | end |
| 917 | end |
| 918 | |
| 919 | local minLeftMarker, minRightMarker, _ = self.turnData.minAreaImplement.object:getAIMarkers() |
| 920 | self.turnData.sideOffsetLeft = localToLocal(minLeftMarker, self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
| 921 | self.turnData.sideOffsetRight = localToLocal(minRightMarker, self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
| 922 | |
| 923 | if allImplementsOfSameType then |
| 924 | self.turnData.sideOffsetLeft = self.turnData.aiAreaMaxX |
| 925 | self.turnData.sideOffsetRight = self.turnData.aiAreaMinX |
| 926 | end |
| 927 | |
| 928 | -- shrink area size to force overlap (0.26m) |
| 929 | local overlapPerSide = AIVehicleUtil.AREA_OVERLAP / 2 |
| 930 | self.turnData.sideOffsetLeft = self.turnData.sideOffsetLeft - overlapPerSide |
| 931 | self.turnData.sideOffsetRight = self.turnData.sideOffsetRight + overlapPerSide |
| 932 | |
| 933 | -- turn radius should be always higher than the side offset (otherwise some calculation in the strategies won't work) |
| 934 | self.turnData.radius = math.max(self.turnData.radius, self.turnData.sideOffsetLeft, -self.turnData.sideOffsetRight) |
| 935 | |
| 936 | -- check if we need to invert the side offset (happens on plows that are rotating) |
| 937 | if self.turnLeft ~= nil then |
| 938 | local canInvertMarkerOnTurn = false |
| 939 | for _, implement in pairs(attachedAIImplements) do |
| 940 | canInvertMarkerOnTurn = canInvertMarkerOnTurn or implement.object:getAIInvertMarkersOnTurn(self.turnLeft) |
| 941 | end |
| 942 | |
| 943 | if canInvertMarkerOnTurn then |
| 944 | local offset = math.abs(self.turnData.sideOffsetLeft - self.turnData.sideOffsetRight)/2 |
| 945 | self.turnData.sideOffsetLeft = offset |
| 946 | self.turnData.sideOffsetRight = -offset |
| 947 | end |
| 948 | end |
| 949 | |
| 950 | self.turnData.useExtraStraightLeft = self.turnData.sideOffsetLeft > self.turnData.radius |
| 951 | self.turnData.useExtraStraightRight = self.turnData.sideOffsetRight < -self.turnData.radius |
| 952 | |
| 953 | self.turnData.toolOverhang = {} |
| 954 | self.turnData.toolOverhang['front'] = {} |
| 955 | self.turnData.toolOverhang['back'] = {} |
| 956 | |
| 957 | |
| 958 | self.turnData.allToolsAtFront = true |
| 959 | |
| 960 | -- ToDo: ? improve this fallback estimation of overhang ... |
| 961 | local xt = self.vehicle.sizeWidth * 0.5 |
| 962 | local zt = self.vehicle.sizeLength * 0.75 |
| 963 | local alphaX = math.atan(-zt /(xt + self.turnData.radius)) |
| 964 | local alphaZ = math.atan((xt + self.turnData.radius) / zt) |
| 965 | local xb = math.cos(alphaX)*xt - math.sin(alphaX)*zt + math.cos(alphaX)*self.turnData.radius |
| 966 | local zb = math.sin(alphaZ)*xt + math.cos(alphaZ)*zt + math.sin(alphaZ)*self.turnData.radius |
| 967 | for _,side in pairs({'front', 'back'}) do |
| 968 | self.turnData.toolOverhang[side].xt = xt |
| 969 | self.turnData.toolOverhang[side].zt = zt |
| 970 | self.turnData.toolOverhang[side].xb = xb |
| 971 | self.turnData.toolOverhang[side].zb = zb |
| 972 | end |
| 973 | |
| 974 | -- overhang for tools in hydraulic |
| 975 | for _,implement in pairs(attachedAIImplements) do |
| 976 | if implement.object:getAIAllowTurnBackward() then |
| 977 | local leftMarker, rightMarker, backMarker = implement.object:getAIMarkers() |
| 978 | local leftSizeMarker, rightSizeMarker, backSizeMarker = implement.object:getAISizeMarkers() |
| 979 | |
| 980 | local xL,_,zL = localToLocal( Utils.getNoNil(leftSizeMarker, leftMarker), self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
| 981 | local xR,_,zR = localToLocal( Utils.getNoNil(rightSizeMarker, rightMarker), self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
| 982 | local xB,_,zB = localToLocal( Utils.getNoNil(backSizeMarker, backMarker), self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
| 983 | |
| 984 | self.turnData.allToolsAtFront = self.turnData.allToolsAtFront and zB > 0 |
| 985 | |
| 986 | local xt = math.max(math.abs(xL), math.abs(xR), math.abs(xB)) |
| 987 | local zt = math.max(math.abs(zL), math.abs(zR), math.abs(zB)) |
| 988 | |
| 989 | --local alphaX = math.atan(-zt /(xt + self.turnData.radius)) |
| 990 | --local alphaZ = math.atan((xt + self.turnData.radius) / zt) |
| 991 | -- |
| 992 | --local xb = math.cos(alphaX)*xt - math.sin(alphaX)*zt + math.cos(alphaX)*self.turnData.radius |
| 993 | --local zb = math.sin(alphaZ)*xt + math.cos(alphaZ)*zt + math.sin(alphaZ)*self.turnData.radius |
| 994 | -- |
| 995 | ----print(" --------------------------------------------------- ") |
| 996 | ----print( string.format(" OLD :: alphaX=%.2f alphaZ=%.2f ", math.deg(alphaX), math.deg(alphaZ)) ) |
| 997 | ----print( string.format(" -> xb /zb = %.2f / %.2f ", xb, zb) ) |
| 998 | ---- |
| 999 | ---- |
| 1000 | ----local alphaX = math.atan( -(self.turnData.radius + xt) / zt ) |
| 1001 | ----local alphaZ = math.atan( xt / (self.turnData.radius + zt) ) |
| 1002 | ---- |
| 1003 | ----local xb = math.cos(alphaX)*xt - math.sin(alphaX)*zt + math.cos(alphaX)*self.turnData.radius |
| 1004 | ----local zb = math.sin(alphaZ)*xt + math.cos(alphaZ)*zt + math.sin(alphaZ)*self.turnData.radius |
| 1005 | ---- |
| 1006 | ----print( string.format(" NEW :: alphaX=%.2f alphaZ=%.2f ", math.deg(alphaX), math.deg(alphaZ)) ) |
| 1007 | ----print( string.format(" -> xb /zb = %.2f / %.2f ", xb, zb) ) |
| 1008 | ----print( string.format(" -> xt /zt = %.2f / %.2f ", xt, zt) ) |
| 1009 | ---- |
| 1010 | ----print( string.format(" -> c = %.2f / r = %.2f / c+r = %.2f", math.sqrt(xt*xt + zt*zt), self.turnData.radius, math.sqrt(xt*xt + zt*zt)+self.turnData.radius) ) |
| 1011 | |
| 1012 | -- dismiss all the math, just take the worst case |
| 1013 | local xb = math.sqrt(xt*xt + zt*zt) + self.turnData.radius |
| 1014 | local zb = math.sqrt(xt*xt + zt*zt) + self.turnData.radius |
| 1015 | |
| 1016 | |
| 1017 | local side = 'back' |
| 1018 | if zB > 0 then |
| 1019 | side = 'front' |
| 1020 | end |
| 1021 | |
| 1022 | self.turnData.toolOverhang[side].xb = math.max(xb, self.turnData.toolOverhang[side].xb) |
| 1023 | self.turnData.toolOverhang[side].zb = math.max(zb, self.turnData.toolOverhang[side].zb) |
| 1024 | self.turnData.toolOverhang[side].xt = math.max(xt, self.turnData.toolOverhang[side].xt) |
| 1025 | self.turnData.toolOverhang[side].zt = math.max(zt, self.turnData.toolOverhang[side].zt) |
| 1026 | end |
| 1027 | end |
| 1028 | |
| 1029 | -- overhang for trailer implements |
| 1030 | local rotTime = 1/self.vehicle.wheelSteeringDuration * math.atan(1/self.turnData.radius) / math.atan(1/self.vehicle.maxTurningRadius) |
| 1031 | local angle |
| 1032 | if rotTime >= 0 then |
| 1033 | angle = (rotTime / self.vehicle.maxRotTime) * self.vehicle.maxRotation |
| 1034 | else |
| 1035 | angle = (rotTime / self.vehicle.minRotTime) * self.vehicle.maxRotation |
| 1036 | end |
| 1037 | |
| 1038 | for _,implement in pairs(attachedAIImplements) do |
| 1039 | if not implement.object:getAIAllowTurnBackward() then |
| 1040 | local leftMarker, rightMarker, backMarker = implement.object:getAIMarkers() |
| 1041 | local leftSizeMarker, rightSizeMarker, backSizeMarker = implement.object:getAISizeMarkers() |
| 1042 | |
| 1043 | local lX,_,lZ = localToLocal(Utils.getNoNil(leftSizeMarker, leftMarker), implement.object.components[1].node, 0,0,0) |
| 1044 | local rX,_,rZ = localToLocal(Utils.getNoNil(rightSizeMarker, rightMarker), implement.object.components[1].node, 0,0,0) |
| 1045 | local bX,_,bZ = localToLocal(Utils.getNoNil(backSizeMarker, backMarker), implement.object.components[1].node, 0,0,0) |
| 1046 | |
| 1047 | local nX = math.max( math.abs(lX), math.abs(rX), math.abs(bX) ) |
| 1048 | local nZ = math.min( -math.abs(lZ), -math.abs(rZ), -math.abs(bZ) ) |
| 1049 | |
| 1050 | if implement.object.getActiveInputAttacherJoint then |
| 1051 | local inputAttacherJoint = implement.object:getActiveInputAttacherJoint() |
| 1052 | |
| 1053 | local xAtt,_,zAtt = localToLocal(implement.object.components[1].node, inputAttacherJoint.node, nX,0,nZ) |
| 1054 | xAtt, zAtt = zAtt, -xAtt |
| 1055 | |
| 1056 | local xRot = xAtt*math.cos(-angle) - zAtt*math.sin(-angle) |
| 1057 | local zRot = xAtt*math.sin(-angle) + zAtt*math.cos(-angle) |
| 1058 | |
| 1059 | local xFin,_,_ = localToLocal(implement.object.components[1].node, self.vehicle:getAIVehicleDirectionNode(), xRot,0,zRot) |
| 1060 | |
| 1061 | xFin = xFin + self.turnData.radius |
| 1062 | |
| 1063 | self.turnData.toolOverhang.back.xb = math.max(self.turnData.toolOverhang.back.xb, xFin) |
| 1064 | |
| 1065 | local xL,_,_ = localToLocal( Utils.getNoNil(leftSizeMarker, leftMarker), self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
| 1066 | local xR,_,_ = localToLocal( Utils.getNoNil(rightSizeMarker, rightMarker), self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
| 1067 | local _,_,zB = localToLocal( Utils.getNoNil(backSizeMarker, backMarker), self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
| 1068 | self.turnData.toolOverhang.back.xt = math.max(self.turnData.toolOverhang.back.xt, math.max(math.abs(xL), math.abs(xR))) |
| 1069 | self.turnData.toolOverhang.back.zt = math.max(self.turnData.toolOverhang.back.zt, -zB) |
| 1070 | |
| 1071 | local _, rotationJoint, wheels = implement.object:getAITurnRadiusLimitation() |
| 1072 | |
| 1073 | |
| 1074 | local angleSteer = 0 |
| 1075 | if rotationJoint ~= nil then |
| 1076 | for _, wheel in pairs(wheels) do |
| 1077 | if wheel.steeringAxleScale ~= 0 and wheel.steeringAxleRotMax ~= 0 then |
| 1078 | angleSteer = math.max(angleSteer, math.abs(wheel.steeringAxleRotMax)) |
| 1079 | end |
| 1080 | end |
| 1081 | end |
| 1082 | |
| 1083 | if angleSteer ~= 0 and rotationJoint ~= nil then |
| 1084 | local wheelIndexCount = #wheels |
| 1085 | if rotationJoint ~= nil and wheelIndexCount > 0 then |
| 1086 | local cx,cz = 0,0 |
| 1087 | for _, wheel in pairs(wheels) do |
| 1088 | local x,_,z = localToLocal(wheel.repr, implement.object.components[1].node, 0,0,0) |
| 1089 | cx = cx + x |
| 1090 | cz = cz + z |
| 1091 | end |
| 1092 | |
| 1093 | cx = cx / wheelIndexCount |
| 1094 | cz = cz / wheelIndexCount |
| 1095 | |
| 1096 | local dx = nX - cx |
| 1097 | local dz = nZ - cz |
| 1098 | |
| 1099 | local delta = math.sqrt(dx*dx + dz*dz) |
| 1100 | local xFin = delta + self.turnData.radius |
| 1101 | |
| 1102 | self.turnData.toolOverhang.back.xb = math.max(self.turnData.toolOverhang.back.xb, xFin) |
| 1103 | end |
| 1104 | end |
| 1105 | end |
| 1106 | end |
| 1107 | end |
| 1108 | |
| 1109 | for _,implement in pairs(attachedAIImplements) do |
| 1110 | local leftMarker, _, _ = implement.object:getAIMarkers() |
| 1111 | local _,_,z = localToLocal(leftMarker, self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
| 1112 | implement.distToVehicle = z |
| 1113 | end |
| 1114 | |
| 1115 | local sortImplementsByDistance = function(arg1, arg2) |
| 1116 | return arg1.distToVehicle > arg2.distToVehicle |
| 1117 | end |
| 1118 | |
| 1119 | table.sort(attachedAIImplements, sortImplementsByDistance) |
| 1120 | |
| 1121 | -- check if turn data is stable |
| 1122 | if self.lastTurnData == nil then |
| 1123 | self.lastTurnData = {} |
| 1124 | self.lastTurnData.radius = self.turnData.radius |
| 1125 | self.lastTurnData.maxZOffset = self.turnData.maxZOffset |
| 1126 | self.lastTurnData.minZOffset = self.turnData.minZOffset |
| 1127 | self.lastTurnData.aiAreaMaxX = self.turnData.aiAreaMaxX |
| 1128 | self.lastTurnData.aiAreaMinX = self.turnData.aiAreaMinX |
| 1129 | self.lastTurnData.sideOffsetLeft = self.turnData.sideOffsetLeft |
| 1130 | self.lastTurnData.sideOffsetRight = self.turnData.sideOffsetRight |
| 1131 | else |
| 1132 | if self.vehicle:getLastSpeed() > 2 and |
| 1133 | math.abs(self.lastTurnData.radius - self.turnData.radius) < 0.03 and |
| 1134 | math.abs(self.lastTurnData.maxZOffset - self.turnData.maxZOffset) < 0.03 and |
| 1135 | math.abs(self.lastTurnData.minZOffset - self.turnData.minZOffset) < 0.03 and |
| 1136 | math.abs(self.lastTurnData.aiAreaMaxX - self.turnData.aiAreaMaxX) < 0.03 and |
| 1137 | math.abs(self.lastTurnData.aiAreaMinX - self.turnData.aiAreaMinX) < 0.03 and |
| 1138 | math.abs(self.lastTurnData.sideOffsetLeft - self.turnData.sideOffsetLeft) < 0.03 and |
| 1139 | math.abs(self.lastTurnData.sideOffsetRight - self.turnData.sideOffsetRight) < 0.03 |
| 1140 | then |
| 1141 | self.turnDataIsStableCounter = self.turnDataIsStableCounter + 1 |
| 1142 | if self.turnDataIsStableCounter > 120 then |
| 1143 | self.turnDataIsStable = true |
| 1144 | end |
| 1145 | else |
| 1146 | self.lastTurnData.radius = self.turnData.radius |
| 1147 | self.lastTurnData.maxZOffset = self.turnData.maxZOffset |
| 1148 | self.lastTurnData.minZOffset = self.turnData.minZOffset |
| 1149 | self.lastTurnData.aiAreaMaxX = self.turnData.aiAreaMaxX |
| 1150 | self.lastTurnData.aiAreaMinX = self.turnData.aiAreaMinX |
| 1151 | self.lastTurnData.sideOffsetLeft = self.turnData.sideOffsetLeft |
| 1152 | self.lastTurnData.sideOffsetRight = self.turnData.sideOffsetRight |
| 1153 | self.turnDataIsStableCounter = 0 |
| 1154 | end |
| 1155 | end |
| 1156 | end |