37 | function AITurnStrategyBulb3:startTurn(driveStrategyStraight) |
38 | if not AITurnStrategyBulb3:superClass().startTurn(self, driveStrategyStraight) then |
39 | return false |
40 | end |
41 | local turnData = driveStrategyStraight.turnData |
42 | |
43 | local sideOffset |
44 | if self.turnLeft then |
45 | sideOffset = turnData.sideOffsetLeft |
46 | else |
47 | sideOffset = turnData.sideOffsetRight |
48 | end |
49 | |
50 | --# |
51 | --self.usePredictionToSkipToNextSegment = true |
52 | |
53 | -- first circle |
54 | local c1X,c1Y,c1Z |
55 | if sideOffset >= 0 then |
56 | c1X,c1Y,c1Z = -turnData.radius,0,0 |
57 | else |
58 | c1X,c1Y,c1Z = turnData.radius,0,0 |
59 | end |
60 | |
61 | local alpha = math.acos( math.abs(sideOffset) / turnData.radius ) |
62 | |
63 | -- second circle |
64 | local c2X,c2Y,c2Z |
65 | if sideOffset >= 0 then |
66 | c2X = 2*sideOffset - turnData.radius |
67 | else |
68 | c2X = 2*sideOffset + turnData.radius |
69 | end |
70 | c2Y = 0 |
71 | c2Z = math.sin(alpha) * 2 * turnData.radius |
72 | |
73 | c1Z = c1Z + turnData.zOffsetTurn |
74 | c2Z = c2Z + turnData.zOffsetTurn |
75 | |
76 | local rvX,rvY,rvZ = getWorldRotation(self.vehicle:getAIDirectionNode()) |
77 | |
78 | self:addNoFullCoverageSegment(self.turnSegments) |
79 | |
80 | --# first straight |
81 | local segment = {} |
82 | segment.isCurve = false |
83 | segment.moveForward = true |
84 | segment.slowDown = true |
85 | segment.startPoint = self:getVehicleToWorld(0,0,0, true) |
86 | segment.endPoint = self:getVehicleToWorld(0,0,c1Z, true) |
87 | table.insert(self.turnSegments, segment) |
88 | |
89 | --# first curve |
90 | local segment = {} |
91 | segment.isCurve = true |
92 | segment.moveForward = true |
93 | segment.radius = turnData.radius |
94 | segment.o = createTransformGroup("segment1") |
95 | link(getRootNode(), segment.o) |
96 | setTranslation(segment.o, self:getVehicleToWorld(c1X,c1Y,c1Z) ) |
97 | setRotation(segment.o, rvX,rvY,rvZ) |
98 | if sideOffset >= 0 then |
99 | segment.startAngle = 0 |
100 | segment.endAngle = alpha |
101 | else |
102 | segment.startAngle = math.pi |
103 | segment.endAngle = math.pi - alpha |
104 | end |
105 | table.insert(self.turnSegments, segment) |
106 | |
107 | --# second curve |
108 | local segment = {} |
109 | segment.isCurve = true |
110 | segment.moveForward = true |
111 | segment.radius = turnData.radius |
112 | segment.o = createTransformGroup("segment2") |
113 | --segment.checkForValidArea = not self.vehicle.aiAlignedProcessing --true |
114 | link(getRootNode(), segment.o) |
115 | setTranslation(segment.o, self:getVehicleToWorld(c2X,c2Y,c2Z) ) |
116 | setRotation(segment.o, rvX,rvY,rvZ) |
117 | if sideOffset >= 0 then |
118 | segment.startAngle = math.pi + alpha |
119 | segment.endAngle = 0 |
120 | else |
121 | segment.startAngle = -alpha |
122 | segment.endAngle = math.pi |
123 | end |
124 | table.insert(self.turnSegments, segment) |
125 | |
126 | --# final straight |
127 | local segment = {} |
128 | segment.isCurve = false |
129 | segment.moveForward = true |
130 | segment.slowDown = true |
131 | --segment.checkForValidArea = not self.vehicle.aiAlignedProcessing --true |
132 | local x = 2*sideOffset |
133 | segment.startPoint = self:getVehicleToWorld(x,0,c2Z, true) |
134 | segment.endPoint = self:getVehicleToWorld(x,0,math.min(turnData.zOffset, c2Z-0.1), true) |
135 | table.insert(self.turnSegments, segment) |
136 | |
137 | self:startTurnFinalization() |
138 | |
139 | return true |
140 | end |
145 | function AITurnStrategyBulb3:updateTurningSizeBox(box, turnLeft, turnData, lookAheadDistance) |
146 | |
147 | local sideOffset |
148 | if turnLeft then |
149 | sideOffset = turnData.sideOffsetLeft |
150 | else |
151 | sideOffset = turnData.sideOffsetRight |
152 | end |
153 | |
154 | -- first circle |
155 | local c1X,c1Y,c1Z |
156 | if sideOffset >= 0 then |
157 | c1X,c1Y,c1Z = -turnData.radius,0,0 |
158 | else |
159 | c1X,c1Y,c1Z = turnData.radius,0,0 |
160 | end |
161 | |
162 | local alpha = math.acos( math.abs(sideOffset) / turnData.radius ) |
163 | |
164 | -- second circle |
165 | local c2X,c2Y,c2Z |
166 | if sideOffset >= 0 then |
167 | c2X = 2*sideOffset - turnData.radius |
168 | else |
169 | c2X = 2*sideOffset + turnData.radius |
170 | end |
171 | c2Y = 0 |
172 | c2Z = math.sin(alpha) * 2 * turnData.radius |
173 | |
174 | c1Z = c1Z + turnData.zOffsetTurn |
175 | c2Z = c2Z + turnData.zOffsetTurn |
176 | |
177 | --# 3) estimate final size of bounding box |
178 | |
179 | local xb = math.max(turnData.toolOverhang.front.xb, turnData.toolOverhang.back.xb) |
180 | --local zb = math.max(turnData.toolOverhang.front.zb, turnData.toolOverhang.back.zb) |
181 | local xt = math.max(turnData.toolOverhang.front.xt, turnData.toolOverhang.back.xt) |
182 | --local zt = math.max(turnData.toolOverhang.front.zt, turnData.toolOverhang.back.zt) |
183 | local delta = math.max(xb, turnData.radius + xt) |
184 | |
185 | local maxX = c2X + delta |
186 | local minX = c2X - delta |
187 | local maxZ = c2Z + delta |
188 | |
189 | box.center[1], box.center[2], box.center[3] = maxX - (maxX-minX)/2, 0, maxZ/2 + lookAheadDistance/2 |
190 | box.size[1], box.size[2], box.size[3] = (maxX-minX)/2, 5, maxZ/2 + lookAheadDistance/2 |
191 | |
192 | self:adjustHeightOfTurningSizeBox(box) |
193 | |
194 | return box |
195 | end |