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