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