[1006] | 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_ |
---|