38 | function AITurnStrategyBulb3Reverse:startTurn(driveStrategyStraight) |
39 | if not AITurnStrategyDefault:superClass().startTurn(self, driveStrategyStraight) then |
40 | return false |
41 | end |
42 | local turnData = driveStrategyStraight.turnData |
43 | |
44 | local sideOffset |
45 | if self.turnLeft then |
46 | sideOffset = turnData.sideOffsetLeft |
47 | else |
48 | sideOffset = turnData.sideOffsetRight |
49 | end |
50 | |
51 | -- shall we check for free space ? |
52 | local zOffset = self.distanceToCollision |
53 | self:updateTurningSizeBox(self.turnBox, self.turnLeft, turnData, 0) |
54 | if self.collisionDetected == false then |
55 | zOffset = self:getZOffsetForTurn(self.turnBox, zOffset) |
56 | else |
57 | zOffset = zOffset + 2*self.turnBox.size[3] |
58 | end |
59 | |
60 | -- first circle |
61 | local c1X,c1Y,c1Z |
62 | if sideOffset >= 0 then |
63 | c1X,c1Y,c1Z = -turnData.radius,0,0 |
64 | else |
65 | c1X,c1Y,c1Z = turnData.radius,0,0 |
66 | end |
67 | |
68 | local alpha = math.acos( math.abs(sideOffset) / turnData.radius ) |
69 | |
70 | -- second circle |
71 | local c2X,c2Y,c2Z |
72 | if sideOffset >= 0 then |
73 | c2X = 2*sideOffset - turnData.radius |
74 | else |
75 | c2X = 2*sideOffset + turnData.radius |
76 | end |
77 | c2Y = 0 |
78 | c2Z = math.sin(alpha) * 2 * turnData.radius |
79 | |
80 | c1Z = c1Z + turnData.zOffsetTurn |
81 | c2Z = c2Z + turnData.zOffsetTurn |
82 | |
83 | local rvX,rvY,rvZ = getWorldRotation(self.vehicle:getAIVehicleDirectionNode()) |
84 | |
85 | -- now shift segments by length of bulb |
86 | local xb = math.max(turnData.toolOverhang.front.xb, turnData.toolOverhang.back.xb) |
87 | local zb = 0 --math.max(turnData.toolOverhang.front.zb, turnData.toolOverhang.back.zb) |
88 | local xt = math.max(turnData.toolOverhang.front.xt, turnData.toolOverhang.back.xt) |
89 | local zt = 0 --math.max(turnData.toolOverhang.front.zt, turnData.toolOverhang.back.zt) |
90 | local delta = math.max(xb, zb, turnData.radius + xt, turnData.radius + zt) |
91 | |
92 | local fullBulbLength = c2Z + delta |
93 | local bulbLength = math.max(0, fullBulbLength - zOffset) --self.distanceToCollision - maxZ) |
94 | |
95 | self:addNoFullCoverageSegment(self.turnSegments) |
96 | |
97 | --# first straight |
98 | local segment = {} |
99 | segment.isCurve = false |
100 | segment.moveForward = (c1Z-bulbLength) > 0 |
101 | segment.slowDown = true |
102 | if segment.moveForward then |
103 | segment.skipToNextSegmentDistanceThreshold = 3 |
104 | end |
105 | segment.startPoint = self:getVehicleToWorld(0,0,0, true) |
106 | segment.endPoint = self:getVehicleToWorld(0,0,c1Z-bulbLength, true) |
107 | table.insert(self.turnSegments, segment) |
108 | |
109 | --# first curve |
110 | local segment = {} |
111 | segment.isCurve = true |
112 | segment.moveForward = true |
113 | segment.radius = turnData.radius |
114 | segment.o = createTransformGroup("segment1") |
115 | link(getRootNode(), segment.o) |
116 | setTranslation(segment.o, self:getVehicleToWorld(c1X,c1Y,c1Z-bulbLength) ) |
117 | setRotation(segment.o, rvX,rvY,rvZ) |
118 | if sideOffset >= 0 then |
119 | segment.startAngle = 0 |
120 | segment.endAngle = alpha |
121 | else |
122 | segment.startAngle = math.pi |
123 | segment.endAngle = math.pi - alpha |
124 | end |
125 | table.insert(self.turnSegments, segment) |
126 | |
127 | --# second curve |
128 | local segment = {} |
129 | segment.isCurve = true |
130 | segment.moveForward = true |
131 | segment.radius = turnData.radius |
132 | segment.o = createTransformGroup("segment2") |
133 | link(getRootNode(), segment.o) |
134 | setTranslation(segment.o, self:getVehicleToWorld(c2X,c2Y,c2Z-bulbLength) ) |
135 | setRotation(segment.o, rvX,rvY,rvZ) |
136 | if sideOffset >= 0 then |
137 | segment.startAngle = math.pi + alpha |
138 | segment.endAngle = 0 |
139 | else |
140 | segment.startAngle = -alpha |
141 | segment.endAngle = math.pi |
142 | end |
143 | table.insert(self.turnSegments, segment) |
144 | |
145 | --# pre final straight |
146 | local segment = {} |
147 | segment.isCurve = false |
148 | --segment.moveForward = true |
149 | segment.moveForward = (c2Z-bulbLength) > (c2Z-2*fullBulbLength) |
150 | segment.slowDown = true |
151 | segment.checkAlignmentToSkipSegment = true |
152 | --segment.checkForValidArea = not self.vehicle.aiAlignedProcessing --true |
153 | local x = 2*sideOffset |
154 | segment.startPoint = self:getVehicleToWorld(x,0,c2Z-bulbLength, true) |
155 | segment.endPoint = self:getVehicleToWorld(x,0,c2Z-2*fullBulbLength, true) |
156 | table.insert(self.turnSegments, segment) |
157 | |
158 | --# final straight |
159 | --local zFinal = math.min(turnData.zOffset, turnData.toolOverhang.front.zt - turnData.toolOverhang.back.zt) |
160 | local zFinal = turnData.zOffset |
161 | local segment = {} |
162 | segment.isCurve = false |
163 | segment.moveForward = (c2Z-2*fullBulbLength) > zFinal |
164 | segment.slowDown = true |
165 | --segment.findEndOfField = true |
166 | segment.startPoint = self:getVehicleToWorld(x,0,c2Z-2*fullBulbLength, true) |
167 | segment.endPoint = self:getVehicleToWorld(x,0,zFinal, true) |
168 | table.insert(self.turnSegments, segment) |
169 | |
170 | self:startTurnFinalization() |
171 | |
172 | return true |
173 | end |
178 | function AITurnStrategyBulb3Reverse:updateTurningSizeBox(box, turnLeft, turnData, lookAheadDistance) |
179 | |
180 | local sideOffset |
181 | if turnLeft then |
182 | sideOffset = turnData.sideOffsetLeft |
183 | else |
184 | sideOffset = turnData.sideOffsetRight |
185 | end |
186 | |
187 | -- first circle |
188 | local c1X,c1Y,c1Z |
189 | if sideOffset >= 0 then |
190 | c1X,c1Y,c1Z = -turnData.radius,0,0 |
191 | else |
192 | c1X,c1Y,c1Z = turnData.radius,0,0 |
193 | end |
194 | |
195 | local alpha = math.acos( math.abs(sideOffset) / turnData.radius ) |
196 | |
197 | -- second circle |
198 | local c2X,c2Y,c2Z |
199 | if sideOffset >= 0 then |
200 | c2X = 2*sideOffset - turnData.radius |
201 | else |
202 | c2X = 2*sideOffset + turnData.radius |
203 | end |
204 | c2Y = 0 |
205 | c2Z = math.sin(alpha) * 2 * turnData.radius |
206 | |
207 | c1Z = c1Z + turnData.zOffsetTurn |
208 | c2Z = c2Z + turnData.zOffsetTurn |
209 | |
210 | --local bulbLength = c2Z + math.max(turnData.sideOffsetLeft, turnData.sideOffsetRight, turnData.toolOverhang.back.xt, turnData.toolOverhang.back.zt, turnData.toolOverhang.front.xt, turnData.toolOverhang.front.zt) |
211 | --c1Z = c1Z - bulbLength |
212 | --c2Z = c2Z - bulbLength |
213 | |
214 | --# 3) estimate final size of bounding box |
215 | |
216 | local xb = math.max(turnData.toolOverhang.front.xb, turnData.toolOverhang.back.xb) |
217 | local xt = math.max(turnData.toolOverhang.front.xt, turnData.toolOverhang.back.xt) |
218 | --local zt = math.max(turnData.toolOverhang.front.zt, turnData.toolOverhang.back.zt) |
219 | --local delta = math.max(xb, zb, turnData.radius + xt, turnData.radius + zt) |
220 | local delta = math.max(xb, turnData.radius + xt) |
221 | |
222 | local maxX = c2X + delta |
223 | local minX = c2X - delta |
224 | |
225 | local maxZ = math.max(turnData.toolOverhang.front.zt, turnData.zOffset + turnData.toolOverhang.back.zt) |
226 | --local maxZ = turnData.toolOverhang.front.zt |
227 | |
228 | box.center[1], box.center[2], box.center[3] = maxX - (maxX-minX)/2, 0, maxZ/2 + lookAheadDistance/2 |
229 | box.size[1], box.size[2], box.size[3] = (maxX-minX)/2, 5, maxZ/2 + lookAheadDistance/2 |
230 | |
231 | self:adjustHeightOfTurningSizeBox(box) |
232 | end |