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