source: cpp/frams/model/geometry/part_distance_estimator.h @ 1056

Last change on this file since 1056 was 1056, checked in by Maciej Komosinski, 2 months ago

Cosmetic

File size: 3.1 KB
Line 
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
11class PartDistanceEstimator
12{
13public:
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_
Note: See TracBrowser for help on using the repository browser.