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