28 | function AITurnStrategyDefaultReverse:startTurn(driveStrategyStraight) |
29 | if not AITurnStrategyDefaultReverse:superClass().startTurn(self, driveStrategyStraight) then |
30 | return false |
31 | end |
32 | local turnData = driveStrategyStraight.turnData |
33 | |
34 | local sideOffset |
35 | if self.turnLeft then |
36 | sideOffset = turnData.sideOffsetLeft |
37 | else |
38 | sideOffset = turnData.sideOffsetRight |
39 | end |
40 | |
41 | local radius = self:getTurnRadius(turnData.radius, sideOffset) |
42 | |
43 | -- shall we check for free space ? |
44 | local zOffset = self.distanceToCollision |
45 | self:updateTurningSizeBox(self.turnBox, self.turnLeft, turnData, 0) |
46 | |
47 | -- always go the turn size box length back, so we want touch untested area with the vehicle or tool |
48 | zOffset = zOffset + 2*self.turnBox.size[3] |
49 | |
50 | -- center of first circle |
51 | local c1X,c1Y,c1Z |
52 | if sideOffset >= 0 then |
53 | c1X,c1Y,c1Z = radius, 0, 0 |
54 | else |
55 | c1X,c1Y,c1Z = -radius, 0, 0 |
56 | end |
57 | |
58 | -- center of second circle |
59 | local c2X,c2Y,c2Z |
60 | local a = 2*math.abs(sideOffset) |
61 | local b = math.sqrt( (2*radius)*(2*radius) - a*a ) |
62 | if sideOffset >= 0 then |
63 | c2X,c2Y,c2Z = radius + a, 0, -b |
64 | else |
65 | c2X,c2Y,c2Z = -radius - a, 0, -b |
66 | end |
67 | |
68 | local alpha = math.acos(a/(2*radius)) |
69 | |
70 | -- |
71 | local rvX,rvY,rvZ = getWorldRotation(self.vehicle:getAIDirectionNode(), 0,0,0) |
72 | |
73 | self:addNoFullCoverageSegment(self.turnSegments) |
74 | |
75 | --# pre turn straight - only used to get combine harvester after straw drop back to turn start |
76 | local segment = {} |
77 | segment.isCurve = false |
78 | segment.moveForward = true |
79 | segment.slowDown = true |
80 | segment.startPoint = self:getVehicleToWorld(0,0,-1, true) |
81 | segment.endPoint = self:getVehicleToWorld(0,0,0, true) |
82 | table.insert(self.turnSegments, segment) |
83 | |
84 | --# first curve |
85 | segment = {} |
86 | segment.isCurve = true |
87 | segment.moveForward = false |
88 | segment.slowDown = true |
89 | segment.usePredictionToSkipToNextSegment = false |
90 | segment.radius = radius |
91 | segment.o = createTransformGroup("segment1") |
92 | link(getRootNode(), segment.o) |
93 | setTranslation(segment.o, self:getVehicleToWorld(c1X,c1Y,c1Z)) |
94 | setRotation(segment.o, rvX,rvY,rvZ) |
95 | if sideOffset >= 0 then |
96 | segment.startAngle = math.rad(180) |
97 | segment.endAngle = math.rad(360) - alpha |
98 | else |
99 | segment.startAngle = 0 |
100 | segment.endAngle = -math.rad(180) + alpha |
101 | end |
102 | table.insert(self.turnSegments, segment) |
103 | |
104 | --# second curve |
105 | segment = {} |
106 | segment.isCurve = true |
107 | segment.moveForward = true |
108 | segment.slowDown = true |
109 | --segment.usePredictionToSkipToNextSegment = false |
110 | segment.radius = radius |
111 | segment.o = createTransformGroup("segment2") |
112 | link(getRootNode(), segment.o) |
113 | setTranslation(segment.o, self:getVehicleToWorld(c2X,c2Y,c2Z)) |
114 | setRotation(segment.o, rvX,rvY,rvZ) |
115 | if sideOffset >= 0 then |
116 | segment.startAngle = math.rad(180) - alpha |
117 | segment.endAngle = math.rad(180) |
118 | else |
119 | segment.startAngle = alpha |
120 | segment.endAngle = 0 |
121 | end |
122 | table.insert(self.turnSegments, segment) |
123 | |
124 | --# third straight |
125 | segment = {} |
126 | segment.isCurve = false |
127 | segment.moveForward = c2Z > turnData.zOffset |
128 | if not segment.moveForward then |
129 | self.turnSegments[#self.turnSegments].usePredictionToSkipToNextSegment = false |
130 | end |
131 | segment.slowDown = true |
132 | --segment.findEndOfField = true |
133 | segment.skipToNextSegmentDistanceThreshold = 0.001 |
134 | local x = 2*sideOffset |
135 | segment.startPoint = self:getVehicleToWorld(x,0,c2Z, true) |
136 | segment.endPoint = self:getVehicleToWorld(x,0,turnData.zOffset, true) |
137 | table.insert(self.turnSegments, segment) |
138 | |
139 | self:startTurnFinalization() |
140 | |
141 | return true |
142 | end |
146 | function AITurnStrategyDefaultReverse:updateTurningSizeBox(box, turnLeft, turnData, lookAheadDistance) |
147 | --print("function AITurnStrategyDefaultReverse:updateTurningSizeBox("..tostring(turnLeft)..", "..tostring(turnData)..", "..tostring(lookAheadDistance)) |
148 | |
149 | local sideOffset |
150 | if turnLeft then |
151 | sideOffset = turnData.sideOffsetLeft |
152 | else |
153 | sideOffset = turnData.sideOffsetRight |
154 | end |
155 | |
156 | local radius = self:getTurnRadius(turnData.radius, sideOffset) |
157 | |
158 | -- center of first circle |
159 | local c1X,c1Y,c1Z |
160 | if sideOffset >= 0 then |
161 | c1X,c1Y,c1Z = radius, 0, turnData.minZOffset |
162 | else |
163 | c1X,c1Y,c1Z = -radius, 0, turnData.minZOffset |
164 | end |
165 | |
166 | -- center of second circle |
167 | local c2X,c2Y,c2Z |
168 | c2Y = 0 |
169 | if sideOffset >= 0 then |
170 | c2X = radius + 2*sideOffset |
171 | c2Z = -2*radius + turnData.minZOffset |
172 | else |
173 | c2X = -radius + 2*sideOffset |
174 | c2Z = -2*radius + turnData.minZOffset |
175 | end |
176 | |
177 | --# 4) |
178 | local maxX |
179 | local minX |
180 | if sideOffset >= 0 then |
181 | local xt = -math.max(turnData.toolOverhang.front.xt, turnData.toolOverhang.back.xt) |
182 | minX = math.min(xt, c1X - math.max(turnData.toolOverhang.front.zt, turnData.toolOverhang.front.xt, turnData.toolOverhang.front.xb, turnData.toolOverhang.front.zb)) |
183 | |
184 | maxX = c2X + turnData.toolOverhang.back.zt |
185 | else |
186 | local xt = math.max(turnData.toolOverhang.front.xt, turnData.toolOverhang.back.xt) |
187 | maxX = math.max(xt, c1X + math.max(turnData.toolOverhang.front.zt, turnData.toolOverhang.front.xt, turnData.toolOverhang.front.xb, turnData.toolOverhang.front.zb)) |
188 | |
189 | minX = c2X - turnData.toolOverhang.back.zt |
190 | end |
191 | |
192 | --local maxZ = math.max(turnData.toolOverhang.front.zt, turnData.toolOverhang.back.zt) |
193 | --local xb = math.max(turnData.toolOverhang.front.xb, turnData.toolOverhang.back.xb) |
194 | --local zb = math.max(turnData.toolOverhang.front.zb, turnData.toolOverhang.back.zb) |
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 | --local maxZ = math.max(xb, zb, turnData.radius + xt, turnData.radius + zt) |
198 | --local maxZ = math.max(xb, zb, xt, zt, turnData.maxZOffset) |
199 | |
200 | local maxZ = math.max(turnData.toolOverhang.front.zt, turnData.zOffset + turnData.toolOverhang.back.zt) |
201 | --local maxZ = turnData.toolOverhang.front.zt |
202 | |
203 | box.center[1], box.center[2], box.center[3] = maxX - (maxX-minX)/2, 0, maxZ/2 + lookAheadDistance/2 |
204 | box.size[1], box.size[2], box.size[3] = (maxX-minX)/2, 5, maxZ/2 + lookAheadDistance/2 |
205 | |
206 | self:adjustHeightOfTurningSizeBox(box) |
207 | end |