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