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