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