Changeset 1030 for cpp/frams/genetics/fS/part_distance_estimator.h

Timestamp:

11/26/20 01:30:40 (3 years ago)

Author:

Maciej Komosinski

Message:

fS: refactoring

File:

• cpp/frams/genetics/fS/part_distance_estimator.h (modified) (1 diff)

cpp/frams/genetics/fS/part_distance_estimator.h

@@ -8 +8 @@
 #include "frams/model/geometry/meshbuilder.h"
 
-class fS_Utils
+class PartDistanceEstimator
 {
 public:
-        static void rotateVector(Pt3D &vector, const Pt3D &rotation)
+
+        static Part *buildTemporaryPart(Part::Shape shape, const Pt3D &scale, const Pt3D &rotation)
         {
-                Orient rotmatrix = Orient_1;
-                rotmatrix.rotate(rotation);
-                vector = rotmatrix.transform(vector);
+                Part *tmpPart1 = new Part(shape);
+                tmpPart1->scale = scale;
+                tmpPart1->setRot(rotation);
+                return tmpPart1;
         }
 
-        static double avg(double a, double b)
-        {
-                return 0.5 * (a + b);
-        }
-
-        static double min3(const Pt3D &p)
-        {
-                double tmp = p.x;
-                if (p.y < tmp)
-                        tmp = p.y;
-                if (p.z < tmp)
-                        tmp = p.z;
-                return tmp;
-        }
-
-        static double max3(const Pt3D &p)
-        {
-                double tmp = p.x;
-                if (p.y > tmp)
-                        tmp = p.y;
-                if (p.z > tmp)
-                        tmp = p.z;
-                return tmp;
-        }
-};
-
-class PartDistanceEstimator
-{
-
-public:
-        static constexpr double PRECISION = 0.05;
-        static constexpr double RELATIVE_DENSITY = 5.0;
-
-
-        static Part *buildTemporaryPart(Part::Shape shape, const Pt3D &scale, const Pt3D &rotations)
-        {
-                Part *tmpPart = new Part(shape);
-                tmpPart->scale = scale;
-                tmpPart->setRot(rotations);
-                return tmpPart;
-        }
-
-        static vector <Pt3D> findSphereCenters(Part *part)
+        /// Get some of the points from the surface of the part
+        static vector <Pt3D> findSurfacePoints(Part *part, double  relativeDensity)
         {
                 // Divide by maximal radius to avoid long computations
-                MeshBuilder::PartSurface surface(RELATIVE_DENSITY / fS_Utils::max3(part->scale));
+                MeshBuilder::PartSurface surface(relativeDensity / part->scale.maxComponentValue());
                 surface.initialize(part);
 
-                vector <Pt3D> centers;
+                vector <Pt3D> points;
                 Pt3D point;
                 while (surface.tryGetNext(point))
                 {
-                        centers.push_back(point);
+                        points.push_back(point);
                 }
-                return centers;
+                return points;
         }
 
-        static bool isCollision(Part *parentPart, vector <Pt3D> &centers, Pt3D &vectorBetweenParts)
+        /// Check if there is a collision between the parts
+        static bool isCollision(Part *part, vector <Pt3D> &points, Pt3D &vectorBetweenParts)
         {
                 static double CBRT_3 = std::cbrt(3);
-                double maxParentReachSq = pow(CBRT_3 * fS_Utils::max3(parentPart->scale), 2);
-                for (int i = 0; i < int(centers.size()); i++)
+                double maxPartReachSq = pow(CBRT_3 * part->scale.maxComponentValue(), 2);
+                for (int i = 0; i < int(points.size()); i++)
                 {
-                        Pt3D shifted = centers[i] + vectorBetweenParts;
-                        double distanceToCenterSq = shifted.x * shifted.x + shifted.y * shifted.y + shifted.z * shifted.z;
-                        if (distanceToCenterSq <= maxParentReachSq && GeometryUtils::isPointInsidePart(shifted, parentPart))
+                        Pt3D shifted = points[i] + vectorBetweenParts;
+                        double distanceToPointSq = shifted.x * shifted.x + shifted.y * shifted.y + shifted.z * shifted.z;
+                        if (distanceToPointSq <= maxPartReachSq && GeometryUtils::isPointInsidePart(shifted, part))
                                 return true;
                 }
                 return false;
         }
+
+
+        static double calculateDistance(Part tmpPart1, Part tmpPart2, double distanceTolerance, double relativeDensity)
+        {
+                /// tmpPart1 and tmpPart2 are copied for purpose and should not be passed as reference
+                /// This function can change some of the properties of those parts
+                Pt3D directionVersor = tmpPart1.p - tmpPart2.p;
+                directionVersor.normalize();
+
+                tmpPart1.p = Pt3D(0);
+                tmpPart2.p = Pt3D(0);
+
+                static double CBRT_3 = std::cbrt(3);
+                vector <Pt3D> points = PartDistanceEstimator::findSurfacePoints(&tmpPart1, relativeDensity);
+
+                double minDistance = tmpPart2.scale.minComponentValue() + tmpPart1.scale.minComponentValue();
+                double maxDistance = CBRT_3 * (tmpPart2.scale.maxComponentValue() + tmpPart1.scale.maxComponentValue());
+                double currentDistance = 0.5 * (maxDistance + minDistance);
+                int collisionDetected = false;
+                while (maxDistance - minDistance > distanceTolerance)
+                {
+                        Pt3D vectorBetweenParts = directionVersor * currentDistance;
+                        collisionDetected = PartDistanceEstimator::isCollision(&tmpPart2, points, vectorBetweenParts);
+
+                        if (collisionDetected)
+                        {
+                                minDistance = currentDistance;
+                                currentDistance = 0.5 * (maxDistance + currentDistance);
+                        } else
+                        {
+                                maxDistance = currentDistance;
+                                currentDistance = 0.5 * (currentDistance + minDistance);
+                        }
+                }
+                return currentDistance;
+        }
 };
 
-double Node::getDistance()
-{
-        Pt3D size;
-        calculateSize(size);
-        Pt3D parentSize;
-        parent->calculateSize(parentSize);    // Here we are sure that parent is not nullptr
-        Part *tmpPart = PartDistanceEstimator::buildTemporaryPart(partType, size, getRotation());
-        Part *parentTmpPart = PartDistanceEstimator::buildTemporaryPart(parent->partType, parentSize, parent->getRotation());
-
-        vector <Pt3D> centers = PartDistanceEstimator::findSphereCenters(tmpPart);
-
-        double minDistance = 0.0;
-        double maxDistance = 2 * (fS_Utils::max3(parentSize) + fS_Utils::max3(size));
-        double currentDistance = fS_Utils::avg(maxDistance, minDistance);
-        int collisionDetected = false;
-        while (maxDistance - minDistance > PartDistanceEstimator::PRECISION)
-        {
-                Pt3D vectorBetweenParts = state->v * currentDistance;
-                collisionDetected = PartDistanceEstimator::isCollision(parentTmpPart, centers, vectorBetweenParts);
-
-                if (collisionDetected)
-                {
-                        minDistance = currentDistance;
-                        currentDistance = fS_Utils::avg(maxDistance, currentDistance);
-                } else
-                {
-                        maxDistance = currentDistance;
-                        currentDistance = fS_Utils::avg(currentDistance, minDistance);
-                }
-        }
-        delete tmpPart;
-        delete parentTmpPart;
-        return currentDistance;
-}
 
 #endif //_PART_DISTANCE_ESTIMATOR_H_