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

Last change on this file was 1056, checked in by Maciej Komosinski, 3 years ago

Cosmetic

File size: 3.1 KB
RevLine 
[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"
[1056]9#include <cmath>
[1017]10
[1006]11class PartDistanceEstimator
12{
[1017]13public:
[1006]14
[1030]15        static Part *buildTemporaryPart(Part::Shape shape, const Pt3D &scale, const Pt3D &rotation)
[1006]16        {
[1030]17                Part *tmpPart1 = new Part(shape);
18                tmpPart1->scale = scale;
19                tmpPart1->setRot(rotation);
20                return tmpPart1;
[1006]21        }
22
[1030]23        /// Get some of the points from the surface of the part
24        static vector <Pt3D> findSurfacePoints(Part *part, double  relativeDensity)
[1006]25        {
[1017]26                // Divide by maximal radius to avoid long computations
[1030]27                MeshBuilder::PartSurface surface(relativeDensity / part->scale.maxComponentValue());
[1017]28                surface.initialize(part);
[1006]29
[1030]30                vector <Pt3D> points;
[1017]31                Pt3D point;
32                while (surface.tryGetNext(point))
[1006]33                {
[1030]34                        points.push_back(point);
[1006]35                }
[1030]36                return points;
[1006]37        }
38
[1030]39        /// Check if there is a collision between the parts
40        static bool isCollision(Part *part, vector <Pt3D> &points, Pt3D &vectorBetweenParts)
[1006]41        {
[1017]42                static double CBRT_3 = std::cbrt(3);
[1030]43                double maxPartReachSq = pow(CBRT_3 * part->scale.maxComponentValue(), 2);
44                for (int i = 0; i < int(points.size()); i++)
[1006]45                {
[1030]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))
[1017]49                                return true;
[1006]50                }
[1017]51                return false;
[1006]52        }
53
54
[1030]55        static double calculateDistance(Part tmpPart1, Part tmpPart2, double distanceTolerance, double relativeDensity)
[1006]56        {
[1056]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.
[1032]59                /// tmpPart1 will be approximated by surface points.
[1056]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.
[1030]62                Pt3D directionVersor = tmpPart1.p - tmpPart2.p;
63                directionVersor.normalize();
[1006]64
[1032]65                tmpPart1.p = Pt3D_0;
66                tmpPart2.p = Pt3D_0;
[1030]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)
[1006]76                {
[1030]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                        }
[1006]89                }
[1030]90                return currentDistance;
[1006]91        }
[1030]92};
[1006]93
[1030]94
[1006]95#endif //_PART_DISTANCE_ESTIMATOR_H_
Note: See TracBrowser for help on using the repository browser.