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

Last change on this file since 1032 was 1032, checked in by Maciej Komosinski, 3 months ago
  • fS: comma as an intuitive separator in genotype instead of weird symbols ;'
  • other minor refactorizations
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
10class PartDistanceEstimator
11{
12public:
13
14        static Part *buildTemporaryPart(Part::Shape shape, const Pt3D &scale, const Pt3D &rotation)
15        {
16                Part *tmpPart1 = new Part(shape);
17                tmpPart1->scale = scale;
18                tmpPart1->setRot(rotation);
19                return tmpPart1;
20        }
21
22        /// Get some of the points from the surface of the part
23        static vector <Pt3D> findSurfacePoints(Part *part, double  relativeDensity)
24        {
25                // Divide by maximal radius to avoid long computations
26                MeshBuilder::PartSurface surface(relativeDensity / part->scale.maxComponentValue());
27                surface.initialize(part);
28
29                vector <Pt3D> points;
30                Pt3D point;
31                while (surface.tryGetNext(point))
32                {
33                        points.push_back(point);
34                }
35                return points;
36        }
37
38        /// Check if there is a collision between the parts
39        static bool isCollision(Part *part, vector <Pt3D> &points, Pt3D &vectorBetweenParts)
40        {
41                static double CBRT_3 = std::cbrt(3);
42                double maxPartReachSq = pow(CBRT_3 * part->scale.maxComponentValue(), 2);
43                for (int i = 0; i < int(points.size()); i++)
44                {
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))
48                                return true;
49                }
50                return false;
51        }
52
53
54        static double calculateDistance(Part tmpPart1, Part tmpPart2, double distanceTolerance, double relativeDensity)
55        {
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                /// tmpPart1 will be approximated by surface points.
59                /// The collision between the parts is detected when any of those points is inside tmpPart2
60                /// If tmpPart1 and tmpPart2 are swapped, the calculated distance may slightly differ
61                Pt3D directionVersor = tmpPart1.p - tmpPart2.p;
62                directionVersor.normalize();
63
64                tmpPart1.p = Pt3D_0;
65                tmpPart2.p = Pt3D_0;
66
67                static double CBRT_3 = std::cbrt(3);
68                vector <Pt3D> points = PartDistanceEstimator::findSurfacePoints(&tmpPart1, relativeDensity);
69
70                double minDistance = tmpPart2.scale.minComponentValue() + tmpPart1.scale.minComponentValue();
71                double maxDistance = CBRT_3 * (tmpPart2.scale.maxComponentValue() + tmpPart1.scale.maxComponentValue());
72                double currentDistance = 0.5 * (maxDistance + minDistance);
73                int collisionDetected = false;
74                while (maxDistance - minDistance > distanceTolerance)
75                {
76                        Pt3D vectorBetweenParts = directionVersor * currentDistance;
77                        collisionDetected = PartDistanceEstimator::isCollision(&tmpPart2, points, vectorBetweenParts);
78
79                        if (collisionDetected)
80                        {
81                                minDistance = currentDistance;
82                                currentDistance = 0.5 * (maxDistance + currentDistance);
83                        } else
84                        {
85                                maxDistance = currentDistance;
86                                currentDistance = 0.5 * (currentDistance + minDistance);
87                        }
88                }
89                return currentDistance;
90        }
91};
92
93
94#endif //_PART_DISTANCE_ESTIMATOR_H_
Note: See TracBrowser for help on using the repository browser.