1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All right reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Radu Serban, Michael Taylor
13 // =============================================================================
14 //
15 // Base class for continuous band track assembly using rigid tread.
16 // Derived classes specify the actual template defintions, using different track
17 // shoes.
18 //
19 // The reference frame for a vehicle follows the ISO standard: Z-axis up, X-axis
20 // pointing forward, and Y-axis towards the left of the vehicle.
21 //
22 // =============================================================================
23
24 #include <algorithm>
25 #include <cmath>
26 #include <utility>
27
28 #include "chrono_vehicle/tracked_vehicle/track_assembly/ChTrackAssemblyBand.h"
29
30 #include "chrono/core/ChLog.h"
31 #include "chrono/utils/ChUtilsInputOutput.h"
32
33 namespace chrono {
34 namespace vehicle {
35
36 // -----------------------------------------------------------------------------
37 // Calculate points for continuous band track assembly.
38 //
39 // Returns true if the track shoes are initialized in a counter clockwise
40 // direction and false otherwise.
41 //
42 // This procedure is performed in the chassis reference frame, taking into
43 // account the convention that the chassis reference frame has the x-axis
44 // pointing to the front of the vehicle and the z-axis pointing up.
45 // It is also assumed that the sprocket, idler, and road wheels lie in the
46 // same vertical plane (in the chassis reference frame). The assembly is done
47 // in the (x-z) plane.
48 //
49 // TODO: NEEDS fixes for clock-wise wrapping (idler in front of sprocket)
50 //
51 // -----------------------------------------------------------------------------
FindAssemblyPoints(std::shared_ptr<ChBodyAuxRef> chassis,int num_shoes,const std::vector<double> & connection_lengths,std::vector<ChVector2<>> & shoe_points)52 bool ChTrackAssemblyBand::FindAssemblyPoints(std::shared_ptr<ChBodyAuxRef> chassis,
53 int num_shoes,
54 const std::vector<double>& connection_lengths,
55 std::vector<ChVector2<>>& shoe_points) {
56 // Shift the start point by a fraction of a shoe length so that the assembly can start with the belt
57 // tooth aligned with the sprocket grove
58 double FirstShoeFractionalOffset = -0.5;
59
60 // Tolerance on how close the iterative algorithm will try to connect the beginning and end of the belt
61 double Tolerance = 1e-12;
62
63 // Stop after this number of iterations to prevent an infinite loop in case something unexpected goes wrong
64 int Maxiter = 200;
65
66 // Position of sprocket (in chassis reference frame)
67 ChVector<> sprocket_pos_3d = chassis->TransformPointParentToLocal(m_sprocket->GetGearBody()->GetPos());
68 m_sprocket_offset = sprocket_pos_3d.y();
69
70 // Number of wheels
71 int num_wheels = static_cast<int>(m_suspensions.size());
72
73 // X & Z coordinates for the sprocket, idler, and all of the road wheels
74 std::vector<ChVector2<>> CirclePosAll(2 + num_wheels);
75
76 // Radii for the sprocket, idler, and all of the road wheels
77 std::vector<double> CircleRadiusAll(2 + num_wheels);
78
79 ChVector<> idler_pos_3d = chassis->TransformPointParentToLocal(m_idler->GetWheelBody()->GetPos());
80
81 CirclePosAll[0].x() = sprocket_pos_3d.x();
82 CirclePosAll[0].y() = sprocket_pos_3d.z();
83 CircleRadiusAll[0] = m_sprocket->GetAssemblyRadius();
84
85 CirclePosAll[1].x() = idler_pos_3d.x();
86 CirclePosAll[1].y() = idler_pos_3d.z();
87 CircleRadiusAll[1] = m_idler->GetWheelRadius();
88
89 for (int i = 0; i < num_wheels; i++) {
90 ChVector<> wheel_pos = chassis->TransformPointParentToLocal(m_suspensions[i]->GetWheelBody()->GetPos());
91 CirclePosAll[i + 2].x() = wheel_pos.x();
92 CirclePosAll[i + 2].y() = wheel_pos.z();
93 CircleRadiusAll[i + 2] = m_suspensions[i]->GetWheelRadius();
94 }
95
96 // ----------------------------------------------------------
97 // Find the path that encloses the sprocket, idler, and road wheels
98 // Shrink wrap type algorithm
99 // ----------------------------------------------------------
100 // Step 1: Determine if the sprocket is ahead or behind of the other circles
101 double AverageXPos = 0;
102 for (int i = 0; i < CirclePosAll.size(); i++) {
103 AverageXPos += CirclePosAll[i].x();
104 }
105 AverageXPos /= CirclePosAll.size();
106
107 bool SprocketInFront = (CirclePosAll[0].x() >= AverageXPos) ? true : false;
108 double StartingAngle = (CirclePosAll[0].x() >= AverageXPos) ? 0 : CH_C_PI;
109
110 // Start building the path around the sprocket, idler, and wheels
111
112 int Current_Circle = 0;
113
114 // Tangent points (start and end) between consecutive circles
115 std::vector<std::pair<ChVector2<>, ChVector2<>>> TangentPoints(CirclePosAll.size());
116
117 // Vector to save the indices of the circles that form the boundaries of the belt wrap
118 std::vector<int> CircleIndex(CirclePosAll.size());
119
120 // At most each circle is visted once. Something went wrong if
121 // the loop is not closed within this many passes through the algorithm
122 int iter;
123 for (iter = 0; iter < CirclePosAll.size(); iter++) {
124 // Step 2: Create an ordered list of the circles with repect to their
125 // distance from the starting circle
126
127 std::vector<double> Distances;
128 std::vector<int> DistanceIndices;
129 Distances.reserve(CirclePosAll.size() - 1);
130 DistanceIndices.reserve(CirclePosAll.size() - 1);
131
132 for (int c = 0; c < CirclePosAll.size(); c++) {
133 if (c == Current_Circle) { // Don't count the distance between the circle and itself
134 continue;
135 }
136 ChVector2<> Dist = CirclePosAll[c] - CirclePosAll[Current_Circle];
137 Distances.push_back(Dist.Length2());
138 DistanceIndices.push_back(c);
139 }
140
141 std::vector<size_t> idx(Distances.size());
142 std::size_t n(0);
143 std::generate(std::begin(idx), std::end(idx), [&] { return n++; });
144
145 // Sort the DistanceIndices based on Distances
146 // Based on https://stackoverflow.com/questions/25921706/creating-a-vector-of-indices-of-a-sorted-vector
147 // similar to MATLAB functions [~,idx] = sort(Distances,'descend');
148 // DistanceIndices = DistanceIndices(idx);
149 std::sort(std::begin(idx), std::end(idx),
150 [&Distances](size_t idx1, size_t idx2) { return Distances[idx1] > Distances[idx2]; });
151
152 // Step 3: Going down the ordered list of circles from farthest to closest,
153 // find the tangent point that is the closest CCW angle to to the starting
154 // angle and save those tangent points. Attacking the problem in this order
155 // skips over multiple circle tangents that are co-linear
156
157 double minAngle = 0;
158 int nextCircle = 0;
159
160 ChVector2<> Tan1Pnt1;
161 ChVector2<> Tan1Pnt2;
162 ChVector2<> Tan2Pnt1;
163 ChVector2<> Tan2Pnt2;
164
165 for (size_t i = 0; i < DistanceIndices.size(); i++) {
166 // Find the tangent points between the current circle and the next circle to check the wrap angles on
167 ChVector2<> Circle1Pos = CirclePosAll[Current_Circle];
168 double Circle1Rad = CircleRadiusAll[Current_Circle];
169 ChVector2<> Circle2Pos = CirclePosAll[DistanceIndices[idx[i]]];
170 double Circle2Rad = CircleRadiusAll[DistanceIndices[idx[i]]];
171
172 FindCircleTangentPoints(Circle1Pos, Circle1Rad, Circle2Pos, Circle2Rad, Tan1Pnt1, Tan1Pnt2, Tan2Pnt1,
173 Tan2Pnt2);
174
175 // Calculate the wrap angles for each of the 2 external circle tangent lines
176 double Angle1 = std::atan2(Tan1Pnt1.y() - CirclePosAll[Current_Circle].y(),
177 Tan1Pnt1.x() - CirclePosAll[Current_Circle].x());
178 double Angle2 = std::atan2(Tan2Pnt1.y() - CirclePosAll[Current_Circle].y(),
179 Tan2Pnt1.x() - CirclePosAll[Current_Circle].x());
180
181 // Ensure that all of the angles are greater than the starting angle by at least a little bit to prevent
182 // numerical noise
183 while (Angle1 < StartingAngle + .00001) {
184 Angle1 += CH_C_2PI;
185 }
186 while (Angle2 < StartingAngle + .00001) {
187 Angle2 += CH_C_2PI;
188 }
189
190 // If this is the first point that is examined, then save that point as the inital comparison value.
191 if (i == 0) {
192 minAngle = Angle1;
193 TangentPoints[iter].first = Tan1Pnt1;
194 TangentPoints[iter].second = Tan1Pnt2;
195
196 nextCircle = DistanceIndices[idx[i]];
197 }
198
199 if (Angle1 < minAngle) {
200 // Save Tangent as it currently has the smallest change in the belt angle to wrap around to it
201 minAngle = Angle1;
202 TangentPoints[iter].first = Tan1Pnt1;
203 TangentPoints[iter].second = Tan1Pnt2;
204
205 nextCircle = DistanceIndices[idx[i]];
206 }
207
208 if (Angle2 < minAngle) {
209 // Save Tangent as it currently has the smallest change in the belt angle to wrap around to it
210 minAngle = Angle2;
211 TangentPoints[iter].first = Tan2Pnt1;
212 TangentPoints[iter].second = Tan2Pnt2;
213
214 nextCircle = DistanceIndices[idx[i]];
215 }
216 }
217
218 // Setup for the next loop through
219 CircleIndex[iter] = nextCircle;
220 Current_Circle = nextCircle;
221 StartingAngle = std::atan2(TangentPoints[iter].second.y() - CirclePosAll[Current_Circle].y(),
222 TangentPoints[iter].second.x() - CirclePosAll[Current_Circle].x());
223 if (StartingAngle < 0) {
224 StartingAngle += CH_C_2PI;
225 }
226
227 // Check to see if the wrap has made it back onto the starting circle (sprocket) and therefore would complete
228 // the belt wrap
229 if (Current_Circle == 0) {
230 break;
231 }
232 }
233
234 // Save the reduced geometry corresponding to the tight wrapping of the belt path.
235
236 // X & Z coordinates for the sprocket and idler or any of the road wheels on the belt wrap loop
237 std::vector<ChVector2<>> CirclePos(iter + 1);
238 // radii for the sprocket and idler or any of the road wheels on the belt wrap loop
239 std::vector<double> CircleRadius(iter + 1);
240
241 // Move the sprocket to the beginning position from the end
242 CirclePos[0] = CirclePosAll[CircleIndex[iter]];
243 CircleRadius[0] = CircleRadiusAll[CircleIndex[iter]];
244 for (int i = 0; i < iter; i++) {
245 CirclePos[i + 1] = CirclePosAll[CircleIndex[i]];
246 CircleRadius[i + 1] = CircleRadiusAll[CircleIndex[i]];
247 }
248
249 //--------------------------------------------------------------------------
250 // Fit the track around the outermost wrapping.
251 //
252 // Start the iterations with the scale such that the belt length equals the
253 // outer wrapping circumferance(too small of a scale, but not by much).
254 // After that, overshoot the extra length by a factor of 10 to make sure
255 // that the scale is too large.
256 // Then binary search tree on the scale to converge.
257 //
258 // TODO: Make sure that there is too much, rather than too little track left
259 // over so that last two shoes can be very slightly folded to exactly fit.
260 //--------------------------------------------------------------------------
261
262 double ScaleMin = 0;
263 double ScaleMax = 0;
264
265 // Start by calculating the original tangent(constant) and arc lengths (variable)
266 // to determine the inital scale for sprocket/idler/road wheel circles
267
268 double CombineShoeLength = 0;
269 for (int i = 0; i < connection_lengths.size(); i++) {
270 CombineShoeLength += connection_lengths[i]; // sum all of the separate lengths per shoe
271 }
272 CombineShoeLength *= num_shoes;
273
274 // for each circle [Arc Length, Starting Angle of Arc, Ending Angle of Arc]
275 ChMatrixDynamic<> Arcs(CircleRadius.size(), 3);
276
277 StartingAngle = std::atan2(TangentPoints[CircleRadius.size() - 1].second.y() - CirclePos[0].y(),
278 TangentPoints[CircleRadius.size() - 1].second.x() - CirclePos[0].x());
279 double EndingAngle =
280 std::atan2(TangentPoints[0].first.y() - CirclePos[0].y(), TangentPoints[0].first.x() - CirclePos[0].x());
281
282 if (StartingAngle < 0) {
283 StartingAngle += CH_C_2PI;
284 }
285 while (EndingAngle < StartingAngle) {
286 EndingAngle += CH_C_2PI;
287 }
288
289 Arcs(0, 0) = EndingAngle - StartingAngle;
290 Arcs(0, 1) = StartingAngle;
291 Arcs(0, 2) = EndingAngle;
292
293 for (int i = 1; i < CircleRadius.size(); i++) {
294 StartingAngle =
295 std::atan2(TangentPoints[i - 1].second.y() - CirclePos[i].y(), TangentPoints[i - 1].second.x() - CirclePos[i].x());
296 EndingAngle = std::atan2(TangentPoints[i].first.y() - CirclePos[i].y(), TangentPoints[i].first.x() - CirclePos[i].x());
297
298 if (StartingAngle < 0) {
299 StartingAngle += CH_C_2PI;
300 }
301 while (EndingAngle < StartingAngle) {
302 EndingAngle += CH_C_2PI;
303 }
304
305 Arcs(i, 0) = EndingAngle - StartingAngle;
306 Arcs(i, 1) = StartingAngle;
307 Arcs(i, 2) = EndingAngle;
308 }
309
310 // Calculate the belt wrap length
311 double LengthOfArcs = 0;
312 double LengthOfTangents = 0;
313
314 for (int i = 0; i < Arcs.rows(); i++) {
315 LengthOfArcs += (Arcs(i, 0) * CircleRadius[i]);
316 LengthOfTangents += (TangentPoints[i].second - TangentPoints[i].first).Length();
317 }
318
319 // Calculate how the arcs need to be scaled to fit so that tangent length +
320 // the arc lengths = the belt length.This should be a slight underestimate
321 // of the needed scale to place all of the belt wrapping positions.
322
323 // Divide by 2*PI since the belt wrap forms a closed loop/circle
324 double DeltaRadius = (CombineShoeLength - (LengthOfArcs + LengthOfTangents)) / (CH_C_2PI);
325 ScaleMin = DeltaRadius;
326
327 // Properly size the container of output points
328 shoe_points.resize(1 + num_shoes * connection_lengths.size());
329
330 for (iter = 0; iter < Maxiter; iter++) {
331 auto ScaledCircleRadius = CircleRadius;
332 for (int i = 0; i < ScaledCircleRadius.size(); i++) {
333 ScaledCircleRadius[i] += DeltaRadius;
334 }
335
336 // Calculate the new tangents based off of the arc starting and ending points for each circle
337 for (int i = 0; i < ScaledCircleRadius.size(); i++) {
338 double StartingArcPoint_x = cos(Arcs(i, 1)) * ScaledCircleRadius[i] + CirclePos[i].x();
339 double StartingArcPoint_z = sin(Arcs(i, 1)) * ScaledCircleRadius[i] + CirclePos[i].y();
340
341 double EndingArcPoint_x = cos(Arcs(i, 2)) * ScaledCircleRadius[i] + CirclePos[i].x();
342 double EndingArcPoint_z = sin(Arcs(i, 2)) * ScaledCircleRadius[i] + CirclePos[i].y();
343
344 if (i == 0) { // Sprocket
345 TangentPoints[0].first.x() = EndingArcPoint_x;
346 TangentPoints[0].first.y() = EndingArcPoint_z;
347 TangentPoints[ScaledCircleRadius.size() - 1].second.x() = StartingArcPoint_x;
348 TangentPoints[ScaledCircleRadius.size() - 1].second.y() = StartingArcPoint_z;
349 } else {
350 TangentPoints[i].first.x() = EndingArcPoint_x;
351 TangentPoints[i].first.y() = EndingArcPoint_z;
352 TangentPoints[i - 1].second.x() = StartingArcPoint_x;
353 TangentPoints[i - 1].second.y() = StartingArcPoint_z;
354 }
355 }
356
357 // Start wrapping the belt.Start with the first link all the way forward
358 // (or all the way rearward) shifted by the desired offset which is used to
359 // center the first track link to make orienting the sprocket profile easier
360
361 // cordLength = 2 * r*sin(theta / 2)
362 StartingAngle = 2 * std::asin(FirstShoeFractionalOffset * connection_lengths[0] / (2 * ScaledCircleRadius[0]));
363
364 if (!SprocketInFront) {
365 StartingAngle += CH_C_PI; // Start on the back side of the sprocket instead of the front
366 }
367
368 while (StartingAngle < Arcs(0, 1)) {
369 StartingAngle += CH_C_2PI;
370 }
371
372 shoe_points[0] = ChVector2<>(std::cos(StartingAngle) * ScaledCircleRadius[0] + CirclePos[0].x(),
373 std::sin(StartingAngle) * ScaledCircleRadius[0] + CirclePos[0].y());
374
375 int shoelinktype = 0;
376 int CurrentFeature = 0;
377
378 // Build features array used for determining shoe points on the arcs and tangents
379 ChMatrixDynamic<> Features(2 * ScaledCircleRadius.size(), 6);
380
381 for (int i = 0; i < ScaledCircleRadius.size(); i++) {
382 // Add Arc Wrap Feature (id = 0)
383 Features(2 * i, 0) = 0; // feature type id
384 Features(2 * i, 1) = CirclePos[i].x();
385 Features(2 * i, 2) = CirclePos[i].y();
386 Features(2 * i, 3) = Arcs(i, 1);
387 Features(2 * i, 4) = Arcs(i, 2);
388 Features(2 * i, 5) = ScaledCircleRadius[i];
389
390 // Add Tangent Wrap Feature (id = 1)
391 Features(1 + (2 * i), 0) = 1; // feature type id
392 Features(1 + (2 * i), 1) = TangentPoints[i].first.x();
393 Features(1 + (2 * i), 2) = TangentPoints[i].first.y();
394 Features(1 + (2 * i), 3) = TangentPoints[i].second.x();
395 Features(1 + (2 * i), 4) = TangentPoints[i].second.y();
396 Features(1 + (2 * i), 5) = 0; // Unused
397 }
398
399 // Calculate the remaining shoe points
400 bool FoundPoint = false;
401 bool InitalSprocketWrap = true;
402 double ExtraLength = 0;
403 double CurrentAngle = 0;
404 int shoeidx;
405
406 for (shoeidx = 1; shoeidx < shoe_points.size(); shoeidx++) {
407 ChVector2<> Point;
408 ChVector2<> StartingPoint = shoe_points[shoeidx - 1];
409
410 // Make sure that all the features are only searched once to prevent and endless loop
411 for (int c = 0; c < Features.rows(); c++) {
412 if (Features(CurrentFeature, 0) == 0) {
413 CheckCircleCircle(FoundPoint, Point, Features, CurrentFeature, StartingPoint,
414 connection_lengths[shoelinktype]);
415 } else {
416 CheckCircleLine(FoundPoint, Point, Features, CurrentFeature, StartingPoint,
417 connection_lengths[shoelinktype]);
418 }
419
420 if (FoundPoint) {
421 break; // Still on the current Feature (arc or tangent)
422 }
423
424 InitalSprocketWrap = false;
425 CurrentFeature += 1;
426 if (CurrentFeature >= Features.rows()) {
427 CurrentFeature = 0;
428 }
429 }
430
431 if (!FoundPoint) {
432 GetLog() << "Belt Assembly ERROR: Something went wrong\n";
433 assert(FoundPoint);
434 return true;
435 }
436
437 shoe_points[shoeidx] = Point;
438
439 shoelinktype += 1;
440 if (shoelinktype >= connection_lengths.size()) {
441 shoelinktype = 0;
442 }
443
444 ExtraLength = 0;
445 // Check to see if I have wrapped past the inital point
446 if ((!InitalSprocketWrap) && (CurrentFeature == 0)) {
447 CurrentAngle = std::atan2(Point.y() - CirclePos[0].y(), Point.x() - CirclePos[0].x());
448 while (CurrentAngle < Features(0, 3)) {
449 CurrentAngle += CH_C_2PI;
450 }
451 if (CurrentAngle > StartingAngle) {
452 ExtraLength = (CurrentAngle - StartingAngle) * Features(0, 5);
453 break;
454 }
455 }
456 }
457
458 for (int j = shoeidx + 1; j < shoe_points.size(); j++) {
459 ExtraLength += connection_lengths[shoelinktype];
460
461 shoelinktype += 1;
462 if (shoelinktype >= connection_lengths.size()) {
463 shoelinktype = 0;
464 }
465 }
466
467 if (CurrentFeature > 0) {
468 ExtraLength = -(shoe_points.back() - shoe_points.front()).Length();
469 } else if ((ExtraLength == 0) && (CurrentAngle > StartingAngle)) {
470 ExtraLength = (shoe_points.back() - shoe_points.front()).Length();
471 } else if (ExtraLength == 0) {
472 ExtraLength = -(shoe_points.back() - shoe_points.front()).Length();
473 }
474
475 if (std::abs(ExtraLength) < Tolerance) {
476 GetLog() << "Belt Wrap Algorithm Conveged after " << iter << " iterations - Extra Length: " << ExtraLength
477 << " - Length Tolerance: " << Tolerance << "\n";
478 break;
479 }
480
481 if (ExtraLength > 0) {
482 if (DeltaRadius > ScaleMin) {
483 ScaleMin = DeltaRadius;
484 }
485 } else {
486 if ((DeltaRadius < ScaleMax) || (ScaleMax == 0)) {
487 ScaleMax = DeltaRadius;
488 }
489 }
490
491 if (iter == 0) {
492 DeltaRadius = DeltaRadius + ExtraLength * 10;
493 } else {
494 DeltaRadius = (ScaleMin + ScaleMax) / 2;
495 }
496 }
497
498 ////utils::CSV_writer csv;
499 ////for (int i = 0; i < shoe_points.size(); i++) {
500 //// csv << shoe_points[i].x() << shoe_points[i].y() << std::endl;
501 ////}
502 ////csv.write_to_file("points.txt");
503
504 //// TODO: right now only counter-clock-wise
505 return true;
506 }
507
FindCircleTangentPoints(ChVector2<> Circle1Pos,double Circle1Rad,ChVector2<> Circle2Pos,double Circle2Rad,ChVector2<> & Tan1Pnt1,ChVector2<> & Tan1Pnt2,ChVector2<> & Tan2Pnt1,ChVector2<> & Tan2Pnt2)508 void ChTrackAssemblyBand::FindCircleTangentPoints(ChVector2<> Circle1Pos,
509 double Circle1Rad,
510 ChVector2<> Circle2Pos,
511 double Circle2Rad,
512 ChVector2<>& Tan1Pnt1,
513 ChVector2<>& Tan1Pnt2,
514 ChVector2<>& Tan2Pnt1,
515 ChVector2<>& Tan2Pnt2) {
516 // Based on https://en.wikipedia.org/wiki/Tangent_lines_to_circles with modifications
517
518 double x1 = Circle1Pos.x();
519 double x2 = Circle2Pos.x();
520 double y1 = Circle1Pos.y();
521 double y2 = Circle2Pos.y();
522 double r1 = Circle1Rad;
523 double r2 = Circle2Rad;
524
525 double beta = std::asin((r2 - r1) / std::sqrt(std::pow(x2 - x1, 2) + std::pow(y2 - y1, 2)));
526 double gamma = std::atan2((y2 - y1), (x2 - x1));
527
528 Tan1Pnt1 =
529 ChVector2<>(x1 + r1 * std::cos(gamma + (CH_C_PI_2 + beta)), y1 + r1 * std::sin(gamma + (CH_C_PI_2 + beta)));
530 Tan1Pnt2 =
531 ChVector2<>(x2 + r2 * std::cos(gamma + (CH_C_PI_2 + beta)), y2 + r2 * std::sin(gamma + (CH_C_PI_2 + beta)));
532
533 // Pick the external tangent on the other side of the circle
534 Tan2Pnt1 =
535 ChVector2<>(x1 + r1 * std::cos(gamma - (CH_C_PI_2 + beta)), y1 + r1 * std::sin(gamma - (CH_C_PI_2 + beta)));
536 Tan2Pnt2 =
537 ChVector2<>(x2 + r2 * std::cos(gamma - (CH_C_PI_2 + beta)), y2 + r2 * std::sin(gamma - (CH_C_PI_2 + beta)));
538 }
539
CheckCircleCircle(bool & found,ChVector2<> & Point,ChMatrixDynamic<> & Features,int FeatureIdx,ChVector2<> & StartingPoint,double Radius)540 void ChTrackAssemblyBand::CheckCircleCircle(bool& found,
541 ChVector2<>& Point,
542 ChMatrixDynamic<>& Features,
543 int FeatureIdx,
544 ChVector2<>& StartingPoint,
545 double Radius) {
546 // Code was based on http://mathworld.wolfram.com/Circle-CircleIntersection.html
547
548 found = false;
549 Point.x() = 0;
550 Point.y() = 0;
551
552 // Check for any intersection
553 ChVector2<> ArcCenter(Features(FeatureIdx, 1), Features(FeatureIdx, 2));
554 double StartingArcAngle = Features(FeatureIdx, 3);
555 double EndingArcAngle = Features(FeatureIdx, 4);
556
557 double R = Features(FeatureIdx, 5);
558 double r = Radius;
559 ChVector2<> temp = ArcCenter - StartingPoint;
560 double d = temp.Length();
561
562 double y2 = (4 * d * d * R * R - std::pow(d * d - r * r + R * R, 2)) / (4 * d * d);
563
564 if (y2 < 0) {
565 return; // No intersection
566 }
567
568 // Now check the two possible points(could be a degenerate case of 1 point)
569
570 double theta = std::acos(((R * R + d * d) - r * r) / (2 * R * d));
571
572 ChVector2<> Ctr = StartingPoint - ArcCenter;
573 double Angle1 = std::atan2(Ctr.y(), Ctr.x()) - theta;
574 double Angle2 = std::atan2(Ctr.y(), Ctr.x()) + theta - CH_C_2PI;
575
576 while (Angle1 < StartingArcAngle) {
577 Angle1 += CH_C_2PI;
578 }
579 while (Angle2 < StartingArcAngle) {
580 Angle2 += CH_C_2PI;
581 }
582
583 double Angle0 = std::atan2(Ctr.y(), Ctr.x());
584 double Angle1_check = Angle1 - 6 * CH_C_PI;
585 double Angle2_check = Angle2 - 6 * CH_C_PI;
586
587 while (Angle0 < 0) {
588 Angle0 += CH_C_2PI;
589 }
590 while (Angle1_check < Angle0) {
591 Angle1_check += CH_C_2PI;
592 }
593 while (Angle2_check < Angle0) {
594 Angle2_check += CH_C_2PI;
595 }
596
597 if (Angle1_check < Angle2_check) {
598 if (Angle1 <= EndingArcAngle) {
599 found = true;
600 Point.x() = ArcCenter.x() + R * std::cos(Angle1);
601 Point.y() = ArcCenter.y() + R * std::sin(Angle1);
602 }
603 } else {
604 if (Angle2 <= EndingArcAngle) {
605 found = true;
606 Point.x() = ArcCenter.x() + R * std::cos(Angle2);
607 Point.y() = ArcCenter.y() + R * std::sin(Angle2);
608 }
609 }
610 }
611
CheckCircleLine(bool & found,ChVector2<> & Point,ChMatrixDynamic<> & Features,int FeatureIdx,ChVector2<> & StartingPoint,double Radius)612 void ChTrackAssemblyBand::CheckCircleLine(bool& found,
613 ChVector2<>& Point,
614 ChMatrixDynamic<>& Features,
615 int FeatureIdx,
616 ChVector2<>& StartingPoint,
617 double Radius) {
618 // Code was based on http://mathworld.wolfram.com/Circle-LineIntersection.html
619
620 found = false;
621 Point.x() = 0;
622 Point.y() = 0;
623
624 double x1 = Features(FeatureIdx, 1) - StartingPoint.x();
625 double y1 = Features(FeatureIdx, 2) - StartingPoint.y();
626 double x2 = Features(FeatureIdx, 3) - StartingPoint.x();
627 double y2 = Features(FeatureIdx, 4) - StartingPoint.y();
628 ChVector2<> LinePnt1(x1, y1);
629 ChVector2<> LinePnt2(x2, y2);
630 double r = Radius;
631
632 double dx = x2 - x1;
633 double dy = y2 - y1;
634 double dr = std::sqrt(dx * dx + dy * dy);
635 double D = x1 * y2 - x2 * y1;
636
637 double discriminant = (r * r * dr * dr) - (D * D);
638
639 if (discriminant < 0) {
640 return; // No intersection
641 }
642
643 double xint_1;
644 double xint_2;
645
646 if (dy < 0) {
647 xint_1 = (D * dy - dx * std::sqrt(discriminant)) / (dr * dr);
648 xint_2 = (D * dy + dx * std::sqrt(discriminant)) / (dr * dr);
649 } else {
650 xint_1 = (D * dy + dx * std::sqrt(discriminant)) / (dr * dr);
651 xint_2 = (D * dy - dx * std::sqrt(discriminant)) / (dr * dr);
652 }
653
654 double yint_1 = (-D * dx + std::abs(dy) * std::sqrt(discriminant)) / (dr * dr);
655 double yint_2 = (-D * dx - std::abs(dy) * std::sqrt(discriminant)) / (dr * dr);
656
657 ChVector2<> intPnt1(xint_1, yint_1);
658 ChVector2<> intPnt2(xint_2, yint_2);
659
660 ChVector2<> Line = LinePnt2 - LinePnt1;
661 if (Line.Dot(intPnt1 - LinePnt1) > Line.Dot(intPnt2 - LinePnt1)) {
662 auto temp = intPnt1;
663 intPnt1 = intPnt2;
664 intPnt2 = temp;
665 }
666
667 ChVector2<> intPnt2mLinePnt1 = intPnt2 - LinePnt1;
668 ChVector2<> intPnt2mLinePnt2 = intPnt2 - LinePnt2;
669 if ((intPnt2mLinePnt1.Length() <= dr) && (intPnt2mLinePnt2.Length() <= dr)) {
670 found = true;
671 Point = intPnt2 + StartingPoint;
672 }
673 }
674
675 } // end namespace vehicle
676 } // end namespace chrono
677