24 | function AITurnStrategyHalfCircle:startTurn(driveStrategyStraight) |
25 | if not AITurnStrategyHalfCircle:superClass().startTurn(self, driveStrategyStraight) then |
26 | return false |
27 | end |
28 | local turnData = driveStrategyStraight.turnData |
29 | |
30 | local sideOffset |
31 | if self.turnLeft then |
32 | sideOffset = turnData.sideOffsetLeft |
33 | else |
34 | sideOffset = turnData.sideOffsetRight |
35 | end |
36 | |
37 | --# |
38 | --self.usePredictionToSkipToNextSegment = true |
39 | |
40 | -- center of first circle |
41 | local c1X,c1Y,c1Z |
42 | if sideOffset >= 0 then |
43 | c1X,c1Y,c1Z = self:getVehicleToWorld(turnData.radius,0,turnData.zOffsetTurn) |
44 | else |
45 | c1X,c1Y,c1Z = self:getVehicleToWorld(-turnData.radius,0,turnData.zOffsetTurn) |
46 | end |
47 | |
48 | -- center of second circle |
49 | local c2X,c2Y,c2Z |
50 | if sideOffset >= 0 then |
51 | c2X,c2Y,c2Z = self:getVehicleToWorld(2*turnData.sideOffsetLeft-turnData.radius,0,turnData.zOffsetTurn) |
52 | else |
53 | c2X,c2Y,c2Z = self:getVehicleToWorld(2*turnData.sideOffsetRight+turnData.radius,0,turnData.zOffsetTurn) |
54 | end |
55 | |
56 | local rvX,rvY,rvZ = getWorldRotation(self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
57 | |
58 | self:addNoFullCoverageSegment(self.turnSegments) |
59 | |
60 | --# first straight |
61 | local segment = {} |
62 | segment.isCurve = false |
63 | segment.moveForward = true |
64 | segment.slowDown = true |
65 | segment.startPoint = self:getVehicleToWorld(0,0,0, true) |
66 | segment.endPoint = self:getVehicleToWorld(0,0,turnData.zOffsetTurn, true) |
67 | table.insert(self.turnSegments, segment) |
68 | |
69 | --# first curve |
70 | local segment = {} |
71 | segment.isCurve = true |
72 | segment.moveForward = true |
73 | segment.radius = turnData.radius |
74 | segment.o = createTransformGroup("segment1") |
75 | link(getRootNode(), segment.o) |
76 | setTranslation(segment.o, c1X,c1Y,c1Z) |
77 | setRotation(segment.o, rvX,rvY,rvZ) |
78 | if sideOffset >= 0 then |
79 | segment.startAngle = math.rad(180) |
80 | segment.endAngle = math.rad(90) |
81 | else |
82 | segment.startAngle = math.rad(0) |
83 | segment.endAngle = math.rad(90) |
84 | end |
85 | table.insert(self.turnSegments, segment) |
86 | |
87 | --# second straight |
88 | local segment = {} |
89 | segment.isCurve = false |
90 | segment.moveForward = true |
91 | segment.skipToNextSegmentDistanceThreshold = 3 |
92 | local shiftX,shiftY,shiftZ = localDirectionToWorld(self.vehicle:getAIVehicleDirectionNode(), 0,0,turnData.radius) |
93 | segment.startPoint = { c1X+shiftX, c1Y+shiftY, c1Z+shiftZ } |
94 | segment.endPoint = { c2X+shiftX, c2Y+shiftY, c2Z+shiftZ } |
95 | table.insert(self.turnSegments, segment) |
96 | |
97 | --# third curve |
98 | local segment = {} |
99 | segment.isCurve = true |
100 | segment.moveForward = true |
101 | segment.radius = turnData.radius |
102 | segment.o = createTransformGroup("segment3") |
103 | link(getRootNode(), segment.o) |
104 | setTranslation(segment.o, c2X,c2Y,c2Z) |
105 | setRotation(segment.o, rvX,rvY,rvZ) |
106 | if sideOffset >= 0 then |
107 | segment.startAngle = math.rad(90) |
108 | segment.endAngle = math.rad(0) |
109 | else |
110 | segment.startAngle = math.rad(90) |
111 | segment.endAngle = math.rad(180) |
112 | end |
113 | table.insert(self.turnSegments, segment) |
114 | |
115 | --# final straight |
116 | local segment = {} |
117 | segment.isCurve = false |
118 | segment.moveForward = true |
119 | segment.sowDown = true |
120 | segment.startPoint = self:getVehicleToWorld(2*sideOffset,0,turnData.zOffsetTurn, true) |
121 | segment.endPoint = self:getVehicleToWorld(2*sideOffset,0,math.min(turnData.zOffset, turnData.zOffsetTurn-0.1), true) |
122 | table.insert(self.turnSegments, segment) |
123 | |
124 | self:startTurnFinalization() |
125 | |
126 | return true |
127 | end |
132 | function AITurnStrategyHalfCircle:updateTurningSizeBox(box, turnLeft, turnData, lookAheadDistance) |
133 | |
134 | local sideOffset |
135 | if turnLeft then |
136 | sideOffset = turnData.sideOffsetLeft |
137 | else |
138 | sideOffset = turnData.sideOffsetRight |
139 | end |
140 | |
141 | -- center of first circle |
142 | local c1X,c1Z |
143 | if sideOffset >= 0 then |
144 | c1X,c1Z = turnData.radius, turnData.zOffsetTurn |
145 | else |
146 | c1X,c1Z = -turnData.radius, turnData.zOffsetTurn |
147 | end |
148 | |
149 | -- center of second circle |
150 | local c2X,c2Z |
151 | if sideOffset >= 0 then |
152 | c2X,c2Z = 2*sideOffset-turnData.radius, turnData.zOffsetTurn |
153 | else |
154 | c2X,c2Z = 2*sideOffset+turnData.radius, turnData.zOffsetTurn |
155 | end |
156 | |
157 | local xb = math.max(turnData.toolOverhang.front.xb, turnData.toolOverhang.back.xb) |
158 | local zb = math.max(turnData.toolOverhang.front.zb, turnData.toolOverhang.back.zb) |
159 | local xt = math.max(turnData.toolOverhang.front.xt, turnData.toolOverhang.back.xt) |
160 | --local zt = |
161 | |
162 | local maxX, minX |
163 | if sideOffset >= 0 then |
164 | minX = math.min(-xb, -xt) |
165 | maxX = math.max(c2X + xb, c2X + turnData.radius + xt) |
166 | else |
167 | maxX = math.max(xb, xt) |
168 | minX = math.min(c2X - xb, c2X - turnData.radius - xt) |
169 | end |
170 | |
171 | local maxZ = math.max(c1Z + zb, c1Z + turnData.radius + xt) |
172 | |
173 | box.center[1], box.center[2], box.center[3] = maxX - (maxX-minX)/2, 0, maxZ/2 + lookAheadDistance/2 |
174 | box.size[1], box.size[2], box.size[3] = (maxX-minX)/2, 5, maxZ/2 + lookAheadDistance/2 |
175 | |
176 | self:adjustHeightOfTurningSizeBox(box) |
177 | end |