1 | // This file is a part of Framsticks SDK. http://www.framsticks.com/ |
---|
2 | // Copyright (C) 2019-2020 Maciej Komosinski and Szymon Ulatowski. |
---|
3 | // See LICENSE.txt for details. |
---|
4 | |
---|
5 | #ifndef _PART_DISTANCE_ESTIMATOR_H_ |
---|
6 | #define _PART_DISTANCE_ESTIMATOR_H_ |
---|
7 | |
---|
8 | #include "frams/model/geometry/meshbuilder.h" |
---|
9 | #include <cmath> |
---|
10 | |
---|
11 | class PartDistanceEstimator |
---|
12 | { |
---|
13 | public: |
---|
14 | |
---|
15 | static Part *buildTemporaryPart(Part::Shape shape, const Pt3D &scale, const Pt3D &rotation) |
---|
16 | { |
---|
17 | Part *tmpPart1 = new Part(shape); |
---|
18 | tmpPart1->scale = scale; |
---|
19 | tmpPart1->setRot(rotation); |
---|
20 | return tmpPart1; |
---|
21 | } |
---|
22 | |
---|
23 | /// Get some of the points from the surface of the part |
---|
24 | static vector <Pt3D> findSurfacePoints(Part *part, double relativeDensity) |
---|
25 | { |
---|
26 | // Divide by maximal radius to avoid long computations |
---|
27 | MeshBuilder::PartSurface surface(relativeDensity / part->scale.maxComponentValue()); |
---|
28 | surface.initialize(part); |
---|
29 | |
---|
30 | vector <Pt3D> points; |
---|
31 | Pt3D point; |
---|
32 | while (surface.tryGetNext(point)) |
---|
33 | { |
---|
34 | points.push_back(point); |
---|
35 | } |
---|
36 | return points; |
---|
37 | } |
---|
38 | |
---|
39 | /// Check if there is a collision between the parts |
---|
40 | static bool isCollision(Part *part, vector <Pt3D> &points, Pt3D &vectorBetweenParts) |
---|
41 | { |
---|
42 | static double CBRT_3 = std::cbrt(3); |
---|
43 | double maxPartReachSq = pow(CBRT_3 * part->scale.maxComponentValue(), 2); |
---|
44 | for (int i = 0; i < int(points.size()); i++) |
---|
45 | { |
---|
46 | Pt3D shifted = points[i] + vectorBetweenParts; |
---|
47 | double distanceToPointSq = shifted.x * shifted.x + shifted.y * shifted.y + shifted.z * shifted.z; |
---|
48 | if (distanceToPointSq <= maxPartReachSq && GeometryUtils::isPointInsidePart(shifted, part)) |
---|
49 | return true; |
---|
50 | } |
---|
51 | return false; |
---|
52 | } |
---|
53 | |
---|
54 | |
---|
55 | static double calculateDistance(Part tmpPart1, Part tmpPart2, double distanceTolerance, double relativeDensity) |
---|
56 | { |
---|
57 | /// tmpPart1 and tmpPart2 are copied for purpose and should not be passed as reference. |
---|
58 | /// This function can change some of the properties of those parts. |
---|
59 | /// tmpPart1 will be approximated by surface points. |
---|
60 | /// The collision between the parts is detected when any of these points is inside tmpPart2. |
---|
61 | /// If tmpPart1 and tmpPart2 are swapped, the calculated distance may slightly differ. |
---|
62 | Pt3D directionVersor = tmpPart1.p - tmpPart2.p; |
---|
63 | directionVersor.normalize(); |
---|
64 | |
---|
65 | tmpPart1.p = Pt3D_0; |
---|
66 | tmpPart2.p = Pt3D_0; |
---|
67 | |
---|
68 | static double CBRT_3 = std::cbrt(3); |
---|
69 | vector <Pt3D> points = PartDistanceEstimator::findSurfacePoints(&tmpPart1, relativeDensity); |
---|
70 | |
---|
71 | double minDistance = tmpPart2.scale.minComponentValue() + tmpPart1.scale.minComponentValue(); |
---|
72 | double maxDistance = CBRT_3 * (tmpPart2.scale.maxComponentValue() + tmpPart1.scale.maxComponentValue()); |
---|
73 | double currentDistance = 0.5 * (maxDistance + minDistance); |
---|
74 | int collisionDetected = false; |
---|
75 | while (maxDistance - minDistance > distanceTolerance) |
---|
76 | { |
---|
77 | Pt3D vectorBetweenParts = directionVersor * currentDistance; |
---|
78 | collisionDetected = PartDistanceEstimator::isCollision(&tmpPart2, points, vectorBetweenParts); |
---|
79 | |
---|
80 | if (collisionDetected) |
---|
81 | { |
---|
82 | minDistance = currentDistance; |
---|
83 | currentDistance = 0.5 * (maxDistance + currentDistance); |
---|
84 | } else |
---|
85 | { |
---|
86 | maxDistance = currentDistance; |
---|
87 | currentDistance = 0.5 * (currentDistance + minDistance); |
---|
88 | } |
---|
89 | } |
---|
90 | return currentDistance; |
---|
91 | } |
---|
92 | }; |
---|
93 | |
---|
94 | |
---|
95 | #endif //_PART_DISTANCE_ESTIMATOR_H_ |
---|