source: cpp/frams/genetics/fS/part_distance_estimator.h @ 1017

Last change on this file since 1017 was 1017, checked in by Maciej Komosinski, 4 years ago

fS: faster collision detection, depends on "geometry" algorithms
File size: 3.3 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 fS_Utils
11{
12public:
13        static void rotateVector(Pt3D &vector, const Pt3D &rotation)
14        {
15                Orient rotmatrix = Orient_1;
16                rotmatrix.rotate(rotation);
17                vector = rotmatrix.transform(vector);
18        }
19
20        static double avg(double a, double b)
21        {
22                return 0.5 * (a + b);
23        }
24
25        static double min3(const Pt3D &p)
26        {
27                double tmp = p.x;
28                if (p.y < tmp)
29                        tmp = p.y;
30                if (p.z < tmp)
31                        tmp = p.z;
32                return tmp;
33        }
34
35        static double max3(const Pt3D &p)
36        {
37                double tmp = p.x;
38                if (p.y > tmp)
39                        tmp = p.y;
40                if (p.z > tmp)
41                        tmp = p.z;
42                return tmp;
43        }
44};
45
46class PartDistanceEstimator
47{
48
49public:
50        static constexpr double PRECISION = 0.05;
51        static constexpr double RELATIVE_DENSITY = 5.0;
52
53
54        static Part *buildTemporaryPart(Part::Shape shape, const Pt3D &scale, const Pt3D &rotations)
55        {
56                Part *tmpPart = new Part(shape);
57                tmpPart->scale = scale;
58                tmpPart->setRot(rotations);
59                return tmpPart;
60        }
61
62        static vector <Pt3D> findSphereCenters(Part *part)
63        {
64                // Divide by maximal radius to avoid long computations
65                MeshBuilder::PartSurface surface(RELATIVE_DENSITY / fS_Utils::max3(part->scale));
66                surface.initialize(part);
67
68                vector <Pt3D> centers;
69                Pt3D point;
70                while (surface.tryGetNext(point))
71                {
72                        centers.push_back(point);
73                }
74                return centers;
75        }
76
77        static bool isCollision(Part *parentPart, vector <Pt3D> &centers, Pt3D &vectorBetweenParts)
78        {
79                static double CBRT_3 = std::cbrt(3);
80                double maxParentReachSq = pow(CBRT_3 * fS_Utils::max3(parentPart->scale), 2);
81                for (int i = 0; i < int(centers.size()); i++)
82                {
83                        Pt3D shifted = centers[i] + vectorBetweenParts;
84                        double distanceToCenterSq = shifted.x * shifted.x + shifted.y * shifted.y + shifted.z * shifted.z;
85                        if (distanceToCenterSq <= maxParentReachSq && GeometryUtils::isPointInsidePart(shifted, parentPart))
86                                return true;
87                }
88                return false;
89        }
90};
91
92double Node::getDistance()
93{
94        Pt3D size;
95        calculateSize(size);
96        Pt3D parentSize;
97        parent->calculateSize(parentSize);    // Here we are sure that parent is not nullptr
98        Part *tmpPart = PartDistanceEstimator::buildTemporaryPart(partType, size, getRotation());
99        Part *parentTmpPart = PartDistanceEstimator::buildTemporaryPart(parent->partType, parentSize, parent->getRotation());
100
101        vector <Pt3D> centers = PartDistanceEstimator::findSphereCenters(tmpPart);
102
103        double minDistance = 0.0;
104        double maxDistance = 2 * (fS_Utils::max3(parentSize) + fS_Utils::max3(size));
105        double currentDistance = fS_Utils::avg(maxDistance, minDistance);
106        int collisionDetected = false;
107        while (maxDistance - minDistance > PartDistanceEstimator::PRECISION)
108        {
109                Pt3D vectorBetweenParts = state->v * currentDistance;
110                collisionDetected = PartDistanceEstimator::isCollision(parentTmpPart, centers, vectorBetweenParts);
111
112                if (collisionDetected)
113                {
114                        minDistance = currentDistance;
115                        currentDistance = fS_Utils::avg(maxDistance, currentDistance);
116                } else
117                {
118                        maxDistance = currentDistance;
119                        currentDistance = fS_Utils::avg(currentDistance, minDistance);
120                }
121        }
122        delete tmpPart;
123        delete parentTmpPart;
124        return currentDistance;
125}
126
127#endif //_PART_DISTANCE_ESTIMATOR_H_
Note: See TracBrowser for help on using the repository browser.