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 | class fS_Utils |
---|
9 | { |
---|
10 | public: |
---|
11 | static void rotateVector(Pt3D &vector, const Pt3D &rotation) |
---|
12 | { |
---|
13 | Orient rotmatrix = Orient_1; |
---|
14 | rotmatrix.rotate(rotation); |
---|
15 | vector = rotmatrix.transform(vector); |
---|
16 | } |
---|
17 | |
---|
18 | static double avg(double a, double b) |
---|
19 | { |
---|
20 | return 0.5 * (a + b); |
---|
21 | } |
---|
22 | |
---|
23 | static double min3(const Pt3D &p) |
---|
24 | { |
---|
25 | double tmp = p.x; |
---|
26 | if (p.y < tmp) |
---|
27 | tmp = p.y; |
---|
28 | if (p.z < tmp) |
---|
29 | tmp = p.z; |
---|
30 | return tmp; |
---|
31 | } |
---|
32 | |
---|
33 | static double max3(const Pt3D &p) |
---|
34 | { |
---|
35 | double tmp = p.x; |
---|
36 | if (p.y > tmp) |
---|
37 | tmp = p.y; |
---|
38 | if (p.z > tmp) |
---|
39 | tmp = p.z; |
---|
40 | return tmp; |
---|
41 | } |
---|
42 | }; |
---|
43 | |
---|
44 | class PartDistanceEstimator |
---|
45 | { |
---|
46 | /** |
---|
47 | * Used in finding the proper distance between the parts |
---|
48 | * distance between spheres / sphere radius |
---|
49 | * That default value can be changed in certain cases |
---|
50 | * */ |
---|
51 | static constexpr float SPHERE_RELATIVE_DISTANCE = 0.5; |
---|
52 | /** |
---|
53 | * Used in finding the proper distance between the parts |
---|
54 | * The maximal allowed value for |
---|
55 | * maximal radius of the node / sphere radius |
---|
56 | */ |
---|
57 | static const int MAX_DIAMETER_QUOTIENT = 30; |
---|
58 | /** |
---|
59 | * The tolerance of the value of distance between parts |
---|
60 | */ |
---|
61 | static constexpr double SPHERE_DISTANCE_TOLERANCE = 0.96; |
---|
62 | |
---|
63 | static constexpr double SPHERE_SIZE_QUOTIENT = 0.25; |
---|
64 | |
---|
65 | |
---|
66 | static bool isInsidePart(Part::Shape shape, const Pt3D &partRadii, const Pt3D ¢er, double sphereRadius) |
---|
67 | { |
---|
68 | if(sphereRadius >= fS_Utils::min3(partRadii)) |
---|
69 | return true; // TODO Special case |
---|
70 | Pt3D mostRemote = Pt3D(fabs(center.x), fabs(center.y), fabs(center.z)) + Pt3D(sphereRadius); |
---|
71 | bool isInEllipsis; |
---|
72 | |
---|
73 | bool result = false; |
---|
74 | |
---|
75 | switch (shape) |
---|
76 | { |
---|
77 | case Part::Shape::SHAPE_CUBOID: |
---|
78 | if(mostRemote.x <= partRadii.x && mostRemote.y <= partRadii.y && mostRemote.z <= partRadii.z) |
---|
79 | result = true; |
---|
80 | break; |
---|
81 | case Part::Shape::SHAPE_CYLINDER: |
---|
82 | isInEllipsis = pow(center.y / (partRadii.y - sphereRadius), 2) + pow(center.z / (partRadii.z - sphereRadius), 2) <= 1.0; |
---|
83 | if (mostRemote.x <= partRadii.x && isInEllipsis) |
---|
84 | result = true; |
---|
85 | break; |
---|
86 | case Part::Shape::SHAPE_ELLIPSOID: |
---|
87 | if(pow(center.x / (partRadii.x - sphereRadius), 2) + pow(center.y / (partRadii.y - sphereRadius), 2) + pow(center.z / (partRadii.z - sphereRadius), 2) <= 1.0) |
---|
88 | result = true; |
---|
89 | break; |
---|
90 | default: |
---|
91 | logMessage("fS", "calculateVolume", LOG_ERROR, "Invalid part type"); |
---|
92 | } |
---|
93 | return result; |
---|
94 | } |
---|
95 | |
---|
96 | public: |
---|
97 | static vector<Pt3D> findSphereCenters(Part::Shape shape, double &sphereRadius, const Pt3D &radii, const Pt3D &rotations) |
---|
98 | { |
---|
99 | double sphereRelativeDistance = SPHERE_RELATIVE_DISTANCE; |
---|
100 | double minRadius = fS_Utils::min3(radii); |
---|
101 | if(minRadius <= 0) |
---|
102 | throw fS_Exception("Invalid part size in PartDistanceEstimator", 0); |
---|
103 | double maxRadius = fS_Utils::max3(radii); |
---|
104 | sphereRadius = SPHERE_SIZE_QUOTIENT * minRadius; |
---|
105 | if (MAX_DIAMETER_QUOTIENT < maxRadius / sphereRadius) |
---|
106 | { |
---|
107 | // When max radius is much bigger than sphereRadius and there are to many spheresZ |
---|
108 | sphereRelativeDistance = 1.0; // Make the spheres adjacent to speed up the computation |
---|
109 | sphereRadius = maxRadius / MAX_DIAMETER_QUOTIENT; |
---|
110 | } |
---|
111 | else if(shape == Part::Shape::SHAPE_ELLIPSOID) |
---|
112 | sphereRadius = minRadius; |
---|
113 | |
---|
114 | double sphereDiameter = 2 * sphereRadius; |
---|
115 | |
---|
116 | double radiiArr[3]{radii.x, radii.y, radii.z}; |
---|
117 | vector<double> coordinates[3]; |
---|
118 | for (int dim = 0; dim < 3; dim++) |
---|
119 | { |
---|
120 | double spaceForSphereCenters = 2 * radiiArr[dim] - sphereDiameter; |
---|
121 | if (spaceForSphereCenters > 0) |
---|
122 | { |
---|
123 | int lastIndex = ceil(spaceForSphereCenters / (sphereDiameter * sphereRelativeDistance)); |
---|
124 | for (int i = 0; i <= lastIndex; i++) |
---|
125 | { |
---|
126 | coordinates[dim].push_back(spaceForSphereCenters * (double(i) / lastIndex - 0.5)); |
---|
127 | } |
---|
128 | } |
---|
129 | else |
---|
130 | coordinates[dim].push_back(0.0); |
---|
131 | } |
---|
132 | |
---|
133 | vector<Pt3D> centers; |
---|
134 | for (int xi = 0; xi < int(coordinates[0].size()); xi++) |
---|
135 | { |
---|
136 | for (int yi = 0; yi < int(coordinates[1].size()); yi++) |
---|
137 | { |
---|
138 | for (int zi = 0; zi < int(coordinates[2].size()); zi++) |
---|
139 | { |
---|
140 | Pt3D center = Pt3D(coordinates[0][xi], coordinates[1][yi], coordinates[2][zi]); |
---|
141 | if (isInsidePart(shape, radii, center, sphereRadius)) |
---|
142 | { |
---|
143 | fS_Utils::rotateVector(center, rotations); |
---|
144 | centers.push_back(center); |
---|
145 | } |
---|
146 | } |
---|
147 | } |
---|
148 | } |
---|
149 | return centers; |
---|
150 | } |
---|
151 | |
---|
152 | static int isCollision(vector<Pt3D> ¢ersParent, vector<Pt3D> ¢ers, Pt3D &vectorBetweenParts, |
---|
153 | double distanceThreshold) |
---|
154 | { |
---|
155 | double upperThresholdSq = distanceThreshold * distanceThreshold; |
---|
156 | double lowerThresholdSq = pow(SPHERE_DISTANCE_TOLERANCE * distanceThreshold, 2); |
---|
157 | double distanceSq; |
---|
158 | double dx, dy, dz; |
---|
159 | bool existsAdjacent = false; |
---|
160 | Pt3D *tmpPoint; |
---|
161 | for (int sc = 0; sc < int(centers.size()); sc++) |
---|
162 | { |
---|
163 | Pt3D shiftedSphere = Pt3D(centers[sc]); |
---|
164 | shiftedSphere += vectorBetweenParts; |
---|
165 | for (int psc = 0; psc < int(centersParent.size()); psc++) |
---|
166 | { |
---|
167 | tmpPoint = ¢ersParent[psc]; |
---|
168 | dx = shiftedSphere.x - tmpPoint->x; |
---|
169 | dy = shiftedSphere.y - tmpPoint->y; |
---|
170 | dz = shiftedSphere.z - tmpPoint->z; |
---|
171 | distanceSq = dx * dx + dy * dy + dz * dz; |
---|
172 | |
---|
173 | if (distanceSq <= upperThresholdSq) |
---|
174 | { |
---|
175 | if (distanceSq >= lowerThresholdSq) |
---|
176 | existsAdjacent = true; |
---|
177 | else |
---|
178 | { |
---|
179 | return COLLISION; |
---|
180 | } |
---|
181 | } |
---|
182 | } |
---|
183 | } |
---|
184 | if (existsAdjacent) |
---|
185 | return ADJACENT; |
---|
186 | else |
---|
187 | return DISJOINT; |
---|
188 | } |
---|
189 | }; |
---|
190 | |
---|
191 | double Node::getDistance() |
---|
192 | { |
---|
193 | Pt3D size = calculateSize(); |
---|
194 | Pt3D parentSize = parent->calculateSize(); // Here we are sure that parent is not nullptr |
---|
195 | double parentSphereRadius, sphereRadius; |
---|
196 | vector<Pt3D> centersParent = PartDistanceEstimator::findSphereCenters(parent->partType, parentSphereRadius, parentSize, parent->getRotation()); |
---|
197 | vector<Pt3D> centers = PartDistanceEstimator::findSphereCenters(partType, sphereRadius, size, getRotation()); |
---|
198 | |
---|
199 | double distanceThreshold = sphereRadius + parentSphereRadius; |
---|
200 | double minDistance = 0.0; |
---|
201 | double maxDistance = 2 * (fS_Utils::max3(parentSize) + fS_Utils::max3(size)); |
---|
202 | double currentDistance = fS_Utils::avg(maxDistance, minDistance); |
---|
203 | int result = -1; |
---|
204 | int iterationNo = 0; |
---|
205 | while (result != ADJACENT) |
---|
206 | { |
---|
207 | iterationNo++; |
---|
208 | Pt3D vectorBetweenParts = state->v * currentDistance; |
---|
209 | result = PartDistanceEstimator::isCollision(centersParent, centers, vectorBetweenParts, distanceThreshold); |
---|
210 | |
---|
211 | if (result == DISJOINT) |
---|
212 | { |
---|
213 | maxDistance = currentDistance; |
---|
214 | currentDistance = fS_Utils::avg(currentDistance, minDistance); |
---|
215 | } else if (result == COLLISION) |
---|
216 | { |
---|
217 | minDistance = currentDistance; |
---|
218 | currentDistance = fS_Utils::avg(maxDistance, currentDistance); |
---|
219 | } |
---|
220 | |
---|
221 | if(maxDistance <= 0 || iterationNo > 1000) |
---|
222 | throw fS_Exception("Computing of distances between parts failed", 0); |
---|
223 | if (currentDistance > maxDistance) |
---|
224 | { |
---|
225 | throw fS_Exception("Internal error; the maximal distance between parts exceeded.", 0); |
---|
226 | } |
---|
227 | if (currentDistance < minDistance) |
---|
228 | throw fS_Exception("Internal error; the minimal distance between parts exceeded.", 0); |
---|
229 | |
---|
230 | } |
---|
231 | |
---|
232 | return currentDistance; |
---|
233 | } |
---|
234 | |
---|
235 | #endif //_PART_DISTANCE_ESTIMATOR_H_ |
---|