30 | function AITurnStrategyDefault:startTurn(driveStrategyStraight) |
31 | if not AITurnStrategyDefault:superClass().startTurn(self, driveStrategyStraight) then |
32 | return false |
33 | end |
34 | local turnData = driveStrategyStraight.turnData |
35 | |
36 | local sideOffset |
37 | if self.turnLeft then |
38 | sideOffset = turnData.sideOffsetLeft |
39 | else |
40 | sideOffset = turnData.sideOffsetRight |
41 | end |
42 | |
43 | --# |
44 | --self.usePredictionToSkipToNextSegment = false |
45 | |
46 | -- center of first circle |
47 | local c1X,c1Y,c1Z |
48 | if sideOffset >= 0 then |
49 | c1X,c1Y,c1Z = turnData.radius, 0, turnData.zOffsetTurn |
50 | else |
51 | c1X,c1Y,c1Z = -turnData.radius, 0, turnData.zOffsetTurn |
52 | end |
53 | |
54 | -- center of second circle |
55 | local c2X,c2Y,c2Z |
56 | local a = 2*math.abs(sideOffset) |
57 | local b = math.sqrt( (2*turnData.radius)*(2*turnData.radius) - a*a ) |
58 | if sideOffset >= 0 then |
59 | c2X,c2Y,c2Z = turnData.radius + a, 0, b+turnData.zOffsetTurn |
60 | else |
61 | c2X,c2Y,c2Z = -turnData.radius - a, 0, b+turnData.zOffsetTurn |
62 | end |
63 | |
64 | local alpha = math.acos(a/(2*turnData.radius)) |
65 | |
66 | -- center of fourth circle |
67 | local c4X,c4Y,c4Z |
68 | if sideOffset >= 0 then |
69 | c4X,c4Y,c4Z = turnData.radius + a, 0, turnData.zOffsetTurn |
70 | else |
71 | c4X,c4Y,c4Z = -turnData.radius - a, 0, turnData.zOffsetTurn |
72 | end |
73 | |
74 | -- center of third circle |
75 | local c3X,c3Y,c3Z |
76 | c3Z = c4Z + (c2Z - c4Z)/2 |
77 | c3Y = 0 |
78 | |
79 | local b = math.sqrt( (2*turnData.radius)*(2*turnData.radius) - (b/2)*(b/2) ) |
80 | if sideOffset >= 0 then |
81 | c3X = c2X - b |
82 | else |
83 | c3X = c2X + b |
84 | end |
85 | |
86 | local beta = math.acos(b/(2*turnData.radius)) |
87 | |
88 | -- |
89 | local rvX,rvY,rvZ = getWorldRotation(self.vehicle:getAIVehicleDirectionNode(), 0,0,0) |
90 | |
91 | self:addNoFullCoverageSegment(self.turnSegments) |
92 | |
93 | --# first straight |
94 | if turnData.zOffsetTurn > 0 then |
95 | local segment = {} |
96 | segment.isCurve = false |
97 | segment.moveForward = true |
98 | segment.slowDown = true |
99 | segment.startPoint = self:getVehicleToWorld(0,0,0, true) |
100 | segment.endPoint = self:getVehicleToWorld(0,0,turnData.zOffsetTurn, true) |
101 | table.insert(self.turnSegments, segment) |
102 | end |
103 | |
104 | --# first curve |
105 | local segment = {} |
106 | segment.isCurve = true |
107 | segment.moveForward = true |
108 | segment.slowDown = true |
109 | segment.usePredictionToSkipToNextSegment = false |
110 | segment.radius = turnData.radius |
111 | segment.o = createTransformGroup("segment1") |
112 | link(getRootNode(), segment.o) |
113 | setTranslation(segment.o, self:getVehicleToWorld(c1X,c1Y,c1Z)) |
114 | setRotation(segment.o, rvX,rvY,rvZ) |
115 | if sideOffset >= 0 then |
116 | segment.startAngle = math.rad(180) |
117 | segment.endAngle = alpha |
118 | else |
119 | segment.startAngle = math.rad(0) |
120 | segment.endAngle = math.rad(180) - alpha |
121 | end |
122 | table.insert(self.turnSegments, segment) |
123 | |
124 | |
125 | --# second curve |
126 | local segment = {} |
127 | segment.isCurve = true |
128 | segment.moveForward = false |
129 | segment.slowDown = true |
130 | segment.usePredictionToSkipToNextSegment = false |
131 | segment.radius = turnData.radius |
132 | segment.o = createTransformGroup("segment2") |
133 | link(getRootNode(), segment.o) |
134 | setTranslation(segment.o, self:getVehicleToWorld(c2X,c2Y,c2Z)) |
135 | setRotation(segment.o, rvX,rvY,rvZ) |
136 | if sideOffset >= 0 then |
137 | segment.startAngle = math.rad(180) + alpha |
138 | segment.endAngle = math.rad(180) + beta |
139 | else |
140 | segment.startAngle = -alpha |
141 | segment.endAngle = -beta |
142 | end |
143 | table.insert(self.turnSegments, segment) |
144 | |
145 | --# third curve |
146 | local segment = {} |
147 | segment.isCurve = true |
148 | segment.moveForward = true |
149 | segment.slowDown = true |
150 | --segment.checkForValidArea = not self.vehicle.aiAlignedProcessing --true |
151 | segment.radius = turnData.radius |
152 | segment.o = createTransformGroup("segment3") |
153 | link(getRootNode(), segment.o) |
154 | setTranslation(segment.o, self:getVehicleToWorld(c3X,c3Y,c3Z)) |
155 | setRotation(segment.o, rvX,rvY,rvZ) |
156 | if sideOffset >= 0 then |
157 | segment.startAngle = beta |
158 | segment.endAngle = -beta |
159 | else |
160 | segment.startAngle = math.pi-beta |
161 | segment.endAngle = math.pi+beta |
162 | end |
163 | table.insert(self.turnSegments, segment) |
164 | |
165 | --# fourth curve |
166 | local segment = {} |
167 | segment.isCurve = true |
168 | segment.moveForward = true |
169 | segment.slowDown = true |
170 | --segment.checkForValidArea = not self.vehicle.aiAlignedProcessing --true |
171 | segment.radius = turnData.radius |
172 | segment.o = createTransformGroup("segment4") |
173 | link(getRootNode(), segment.o) |
174 | setTranslation(segment.o, self:getVehicleToWorld(c4X,c4Y,c4Z)) |
175 | setRotation(segment.o, rvX,rvY,rvZ) |
176 | if sideOffset >= 0 then |
177 | segment.startAngle = math.pi-beta |
178 | segment.endAngle = math.pi |
179 | else |
180 | segment.startAngle = beta |
181 | segment.endAngle = 0 |
182 | end |
183 | table.insert(self.turnSegments, segment) |
184 | |
185 | --# final straight |
186 | local segment = {} |
187 | segment.isCurve = false |
188 | -- segment.moveForward = true -- not true for e.g. mex5 |
189 | --segment.moveForward = turnData.zOffset < c4Z |
190 | local zTarget = math.min(c4Z - 0.05, turnData.zOffset) |
191 | segment.moveForward = zTarget < c4Z |
192 | segment.slowDown = true |
193 | --segment.checkForValidArea = not self.vehicle.aiAlignedProcessing --true |
194 | local x = 2*sideOffset |
195 | segment.startPoint = self:getVehicleToWorld(x,0,c4Z, true) |
196 | segment.endPoint = self:getVehicleToWorld(x,0,zTarget, true) |
197 | table.insert(self.turnSegments, segment) |
198 | |
199 | self:startTurnFinalization() |
200 | |
201 | return true |
202 | end |
206 | function AITurnStrategyDefault:updateTurningSizeBox(box, turnLeft, turnData, lookAheadDistance) |
207 | |
208 | local sideOffset |
209 | if turnLeft then |
210 | sideOffset = turnData.sideOffsetLeft |
211 | else |
212 | sideOffset = turnData.sideOffsetRight |
213 | end |
214 | |
215 | |
216 | --# 2) get turn data, center of first circle and radius |
217 | --local xr,_,zr = turnData.radius,0,turnData.zOffsetTurn |
218 | |
219 | local c1X,c1Y,c1Z |
220 | if sideOffset >= 0 then |
221 | c1X,c1Y,c1Z = turnData.radius,0,turnData.zOffsetTurn |
222 | else |
223 | c1X,c1Y,c1Z = -turnData.radius,0,turnData.zOffsetTurn |
224 | end |
225 | |
226 | --# 3) |
227 | local a = 2*math.abs(sideOffset) |
228 | local b = math.sqrt( (2*turnData.radius)*(2*turnData.radius) - a*a ) |
229 | local c2X,c2Y,c2Z |
230 | if sideOffset >= 0 then |
231 | c2X,c2Y,c2Z = turnData.radius + a, 0, b+turnData.zOffsetTurn |
232 | else |
233 | c2X,c2Y,c2Z = -turnData.radius - a, 0, b+turnData.zOffsetTurn |
234 | end |
235 | |
236 | local alpha = math.acos(a/(2*turnData.radius)) |
237 | |
238 | -- center of fourth circle |
239 | local c4Z |
240 | if sideOffset >= 0 then |
241 | c4Z = turnData.zOffsetTurn |
242 | else |
243 | c4Z = turnData.zOffsetTurn |
244 | end |
245 | |
246 | local b = math.sqrt( (2*turnData.radius)*(2*turnData.radius) - (b/2)*(b/2) ) |
247 | |
248 | local beta = math.acos(b/(2*turnData.radius)) |
249 | |
250 | --# 4) |
251 | local maxX |
252 | local minX |
253 | if sideOffset >= 0 then |
254 | maxX = c1X + math.cos(alpha)*turnData.radius + turnData.toolOverhang.front.zt |
255 | |
256 | minX = math.min(-turnData.toolOverhang.front.xt, -turnData.toolOverhang.back.xt) |
257 | if not turnData.allToolsAtFront then |
258 | minX = math.min(minX, c1X - turnData.toolOverhang.back.xb) |
259 | end |
260 | else |
261 | --minX = c1X + math.cos(math.pi - alpha)*turnData.radius - turnData.toolOverhang.front.zt |
262 | minX = c1X - math.cos(alpha)*turnData.radius - turnData.toolOverhang.front.zt |
263 | |
264 | maxX = math.max(turnData.toolOverhang.front.xt, turnData.toolOverhang.back.xt) |
265 | if not turnData.allToolsAtFront then |
266 | maxX = math.max(maxX, c1X + turnData.toolOverhang.back.xb) |
267 | end |
268 | end |
269 | |
270 | --local maxZ = c2Z + zt |
271 | local maxZ |
272 | --[[ |
273 | if allToolsAtFront then |
274 | maxZ = math.max(c1Z + zb, c2Z) |
275 | else |
276 | maxZ = math.max(c1Z + zb, c2Z + zt) |
277 | end |
278 | ]]-- |
279 | --maxZ = math.max(c1Z + math.max(turnData.toolOverhang.front.zb, turnData.toolOverhang.back.zb), c2Z + turnData.toolOverhang.back.zt) |
280 | maxZ = math.max(c1Z + math.max(turnData.toolOverhang.front.zb, turnData.toolOverhang.back.zb), c2Z - math.sin(beta)*turnData.radius + turnData.toolOverhang.back.zt) |
281 | |
282 | box.center[1], box.center[2], box.center[3] = maxX - (maxX-minX)/2, 0, maxZ/2 + lookAheadDistance/2 |
283 | box.size[1], box.size[2], box.size[3] = (maxX-minX)/2, 5, maxZ/2 + lookAheadDistance/2 |
284 | |
285 | self:adjustHeightOfTurningSizeBox(box) |
286 | end |