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