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 | |
---|
10 | class fS_Utils |
---|
11 | { |
---|
12 | public: |
---|
13 | static void rotateVector(Pt3D &vector, const Pt3D &rotation) |
---|
14 | { |
---|
15 | Orient rotmatrix = Orient_1; |
---|
16 | rotmatrix.rotate(rotation); |
---|
17 | vector = rotmatrix.transform(vector); |
---|
18 | } |
---|
19 | |
---|
20 | static double avg(double a, double b) |
---|
21 | { |
---|
22 | return 0.5 * (a + b); |
---|
23 | } |
---|
24 | |
---|
25 | static double min3(const Pt3D &p) |
---|
26 | { |
---|
27 | double tmp = p.x; |
---|
28 | if (p.y < tmp) |
---|
29 | tmp = p.y; |
---|
30 | if (p.z < tmp) |
---|
31 | tmp = p.z; |
---|
32 | return tmp; |
---|
33 | } |
---|
34 | |
---|
35 | static double max3(const Pt3D &p) |
---|
36 | { |
---|
37 | double tmp = p.x; |
---|
38 | if (p.y > tmp) |
---|
39 | tmp = p.y; |
---|
40 | if (p.z > tmp) |
---|
41 | tmp = p.z; |
---|
42 | return tmp; |
---|
43 | } |
---|
44 | }; |
---|
45 | |
---|
46 | class PartDistanceEstimator |
---|
47 | { |
---|
48 | |
---|
49 | public: |
---|
50 | static constexpr double PRECISION = 0.05; |
---|
51 | static constexpr double RELATIVE_DENSITY = 5.0; |
---|
52 | |
---|
53 | |
---|
54 | static Part *buildTemporaryPart(Part::Shape shape, const Pt3D &scale, const Pt3D &rotations) |
---|
55 | { |
---|
56 | Part *tmpPart = new Part(shape); |
---|
57 | tmpPart->scale = scale; |
---|
58 | tmpPart->setRot(rotations); |
---|
59 | return tmpPart; |
---|
60 | } |
---|
61 | |
---|
62 | static vector <Pt3D> findSphereCenters(Part *part) |
---|
63 | { |
---|
64 | // Divide by maximal radius to avoid long computations |
---|
65 | MeshBuilder::PartSurface surface(RELATIVE_DENSITY / fS_Utils::max3(part->scale)); |
---|
66 | surface.initialize(part); |
---|
67 | |
---|
68 | vector <Pt3D> centers; |
---|
69 | Pt3D point; |
---|
70 | while (surface.tryGetNext(point)) |
---|
71 | { |
---|
72 | centers.push_back(point); |
---|
73 | } |
---|
74 | return centers; |
---|
75 | } |
---|
76 | |
---|
77 | static bool isCollision(Part *parentPart, vector <Pt3D> ¢ers, Pt3D &vectorBetweenParts) |
---|
78 | { |
---|
79 | static double CBRT_3 = std::cbrt(3); |
---|
80 | double maxParentReachSq = pow(CBRT_3 * fS_Utils::max3(parentPart->scale), 2); |
---|
81 | for (int i = 0; i < int(centers.size()); i++) |
---|
82 | { |
---|
83 | Pt3D shifted = centers[i] + vectorBetweenParts; |
---|
84 | double distanceToCenterSq = shifted.x * shifted.x + shifted.y * shifted.y + shifted.z * shifted.z; |
---|
85 | if (distanceToCenterSq <= maxParentReachSq && GeometryUtils::isPointInsidePart(shifted, parentPart)) |
---|
86 | return true; |
---|
87 | } |
---|
88 | return false; |
---|
89 | } |
---|
90 | }; |
---|
91 | |
---|
92 | double Node::getDistance() |
---|
93 | { |
---|
94 | Pt3D size; |
---|
95 | calculateSize(size); |
---|
96 | Pt3D parentSize; |
---|
97 | parent->calculateSize(parentSize); // Here we are sure that parent is not nullptr |
---|
98 | Part *tmpPart = PartDistanceEstimator::buildTemporaryPart(partType, size, getRotation()); |
---|
99 | Part *parentTmpPart = PartDistanceEstimator::buildTemporaryPart(parent->partType, parentSize, parent->getRotation()); |
---|
100 | |
---|
101 | vector <Pt3D> centers = PartDistanceEstimator::findSphereCenters(tmpPart); |
---|
102 | |
---|
103 | double minDistance = 0.0; |
---|
104 | double maxDistance = 2 * (fS_Utils::max3(parentSize) + fS_Utils::max3(size)); |
---|
105 | double currentDistance = fS_Utils::avg(maxDistance, minDistance); |
---|
106 | int collisionDetected = false; |
---|
107 | while (maxDistance - minDistance > PartDistanceEstimator::PRECISION) |
---|
108 | { |
---|
109 | Pt3D vectorBetweenParts = state->v * currentDistance; |
---|
110 | collisionDetected = PartDistanceEstimator::isCollision(parentTmpPart, centers, vectorBetweenParts); |
---|
111 | |
---|
112 | if (collisionDetected) |
---|
113 | { |
---|
114 | minDistance = currentDistance; |
---|
115 | currentDistance = fS_Utils::avg(maxDistance, currentDistance); |
---|
116 | } else |
---|
117 | { |
---|
118 | maxDistance = currentDistance; |
---|
119 | currentDistance = fS_Utils::avg(currentDistance, minDistance); |
---|
120 | } |
---|
121 | } |
---|
122 | delete tmpPart; |
---|
123 | delete parentTmpPart; |
---|
124 | return currentDistance; |
---|
125 | } |
---|
126 | |
---|
127 | #endif //_PART_DISTANCE_ESTIMATOR_H_ |
---|