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

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

Improved the fS encoding
File size: 6.9 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
8class fS_Utils
9{
10public:
11        static void rotateVector(Pt3D &vector, const Pt3D &rotation)
12        {
13                Orient rotmatrix = Orient_1;
14                rotmatrix.rotate(rotation);
15                vector = rotmatrix.transform(vector);
16        }
17
18        static double avg(double a, double b)
19        {
20                return 0.5 * (a + b);
21        }
22
23        static double min3(const Pt3D &p)
24        {
25                double tmp = p.x;
26                if (p.y < tmp)
27                        tmp = p.y;
28                if (p.z < tmp)
29                        tmp = p.z;
30                return tmp;
31        }
32
33        static double max3(const Pt3D &p)
34        {
35                double tmp = p.x;
36                if (p.y > tmp)
37                        tmp = p.y;
38                if (p.z > tmp)
39                        tmp = p.z;
40                return tmp;
41        }
42};
43
44class PartDistanceEstimator
45{
46        /**
47         * Used in finding the proper distance between the parts
48         * distance between spheres / sphere radius
49         * That default value can be changed in certain cases
50         * */
51        static constexpr float SPHERE_RELATIVE_DISTANCE = 0.5;
52        /**
53         * Used in finding the proper distance between the parts
54         * The maximal allowed value for
55         * maximal radius of the node / sphere radius
56         */
57        static const int MAX_DIAMETER_QUOTIENT = 30;
58        /**
59         * The tolerance of the value of distance between parts
60         */
61        static constexpr double SPHERE_DISTANCE_TOLERANCE = 0.96;
62
63        static constexpr double SPHERE_SIZE_QUOTIENT = 0.25;
64
65
66        static bool isInsidePart(Part::Shape shape, const Pt3D &partRadii, const Pt3D &center, double sphereRadius)
67        {
68                if(sphereRadius >= fS_Utils::min3(partRadii))
69                        return true; // TODO Special case
70                Pt3D mostRemote = Pt3D(fabs(center.x), fabs(center.y), fabs(center.z)) + Pt3D(sphereRadius);
71                bool isInEllipsis;
72
73                bool result = false;
74
75                switch (shape)
76                {
77                        case Part::Shape::SHAPE_CUBOID:
78                                if(mostRemote.x <= partRadii.x && mostRemote.y <= partRadii.y && mostRemote.z <= partRadii.z)
79                                        result = true;
80                                break;
81                        case Part::Shape::SHAPE_CYLINDER:
82                                isInEllipsis = pow(center.y / (partRadii.y - sphereRadius), 2) + pow(center.z / (partRadii.z - sphereRadius), 2) <= 1.0;
83                                if (mostRemote.x <= partRadii.x && isInEllipsis)
84                                        result = true;
85                                break;
86                        case Part::Shape::SHAPE_ELLIPSOID:
87                                if(pow(center.x / (partRadii.x - sphereRadius), 2) + pow(center.y / (partRadii.y - sphereRadius), 2) + pow(center.z / (partRadii.z - sphereRadius), 2) <= 1.0)
88                                        result = true;
89                                break;
90                        default:
91                                logMessage("fS", "calculateVolume", LOG_ERROR, "Invalid part type");
92                }
93                return result;
94        }
95
96public:
97        static vector<Pt3D> findSphereCenters(Part::Shape shape, double &sphereRadius, const Pt3D &radii, const Pt3D &rotations)
98        {
99                double sphereRelativeDistance = SPHERE_RELATIVE_DISTANCE;
100                double minRadius = fS_Utils::min3(radii);
101                if(minRadius <= 0)
102                        throw fS_Exception("Invalid part size in PartDistanceEstimator", 0);
103                double maxRadius = fS_Utils::max3(radii);
104                sphereRadius = SPHERE_SIZE_QUOTIENT * minRadius;
105                if (MAX_DIAMETER_QUOTIENT < maxRadius / sphereRadius)
106                {
107                        // When max radius is much bigger than sphereRadius and there are to many spheresZ
108                        sphereRelativeDistance = 1.0;   // Make the spheres adjacent to speed up the computation
109                        sphereRadius = maxRadius / MAX_DIAMETER_QUOTIENT;
110                }
111                else if(shape == Part::Shape::SHAPE_ELLIPSOID)
112                        sphereRadius = minRadius;
113
114                double sphereDiameter = 2 * sphereRadius;
115
116                double radiiArr[3]{radii.x, radii.y, radii.z};
117                vector<double> coordinates[3];
118                for (int dim = 0; dim < 3; dim++)
119                {
120                        double spaceForSphereCenters = 2 * radiiArr[dim] - sphereDiameter;
121                        if (spaceForSphereCenters > 0)
122                        {
123                                int lastIndex = ceil(spaceForSphereCenters / (sphereDiameter * sphereRelativeDistance));
124                                for (int i = 0; i <= lastIndex; i++)
125                                {
126                                        coordinates[dim].push_back(spaceForSphereCenters * (double(i) / lastIndex - 0.5));
127                                }
128                        }
129                        else
130                                coordinates[dim].push_back(0.0);
131                }
132
133                vector<Pt3D> centers;
134                for (int xi = 0; xi < int(coordinates[0].size()); xi++)
135                {
136                        for (int yi = 0; yi < int(coordinates[1].size()); yi++)
137                        {
138                                for (int zi = 0; zi < int(coordinates[2].size()); zi++)
139                                {
140                                                Pt3D center = Pt3D(coordinates[0][xi], coordinates[1][yi], coordinates[2][zi]);
141                                                if (isInsidePart(shape, radii, center, sphereRadius))
142                                                {
143                                                        fS_Utils::rotateVector(center, rotations);
144                                                        centers.push_back(center);
145                                                }
146                                }
147                        }
148                }
149                return centers;
150        }
151
152        static int isCollision(vector<Pt3D> &centersParent, vector<Pt3D> &centers, Pt3D &vectorBetweenParts,
153                                        double distanceThreshold)
154        {
155                double upperThresholdSq = distanceThreshold * distanceThreshold;
156                double lowerThresholdSq = pow(SPHERE_DISTANCE_TOLERANCE * distanceThreshold, 2);
157                double distanceSq;
158                double dx, dy, dz;
159                bool existsAdjacent = false;
160                Pt3D *tmpPoint;
161                for (int sc = 0; sc < int(centers.size()); sc++)
162                {
163                        Pt3D shiftedSphere = Pt3D(centers[sc]);
164                        shiftedSphere += vectorBetweenParts;
165                        for (int psc = 0; psc < int(centersParent.size()); psc++)
166                        {
167                                tmpPoint = &centersParent[psc];
168                                dx = shiftedSphere.x - tmpPoint->x;
169                                dy = shiftedSphere.y - tmpPoint->y;
170                                dz = shiftedSphere.z - tmpPoint->z;
171                                distanceSq = dx * dx + dy * dy + dz * dz;
172
173                                if (distanceSq <= upperThresholdSq)
174                                {
175                                        if (distanceSq >= lowerThresholdSq)
176                                                existsAdjacent = true;
177                                        else
178                                        {
179                                                return COLLISION;
180                                        }
181                                }
182                        }
183                }
184                if (existsAdjacent)
185                        return ADJACENT;
186                else
187                        return DISJOINT;
188        }
189};
190
191double Node::getDistance()
192{
193        Pt3D size = calculateSize();
194        Pt3D parentSize = parent->calculateSize();      // Here we are sure that parent is not nullptr
195        double parentSphereRadius, sphereRadius;
196        vector<Pt3D> centersParent = PartDistanceEstimator::findSphereCenters(parent->partType, parentSphereRadius, parentSize, parent->getRotation());
197        vector<Pt3D> centers = PartDistanceEstimator::findSphereCenters(partType, sphereRadius, size, getRotation());
198
199        double distanceThreshold = sphereRadius + parentSphereRadius;
200        double minDistance = 0.0;
201        double maxDistance = 2 * (fS_Utils::max3(parentSize) + fS_Utils::max3(size));
202        double currentDistance = fS_Utils::avg(maxDistance, minDistance);
203        int result = -1;
204        int iterationNo = 0;
205        while (result != ADJACENT)
206        {
207                iterationNo++;
208                Pt3D vectorBetweenParts = state->v * currentDistance;
209                result = PartDistanceEstimator::isCollision(centersParent, centers, vectorBetweenParts, distanceThreshold);
210
211                if (result == DISJOINT)
212                {
213                        maxDistance = currentDistance;
214                        currentDistance = fS_Utils::avg(currentDistance, minDistance);
215                } else if (result == COLLISION)
216                {
217                        minDistance = currentDistance;
218                        currentDistance = fS_Utils::avg(maxDistance, currentDistance);
219                }
220
221                if(maxDistance <= 0 || iterationNo > 1000)
222                        throw fS_Exception("Computing of distances between parts failed", 0);
223                if (currentDistance > maxDistance)
224                {
225                        throw fS_Exception("Internal error; the maximal distance between parts exceeded.", 0);
226                }
227                if (currentDistance < minDistance)
228                        throw fS_Exception("Internal error; the minimal distance between parts exceeded.", 0);
229
230        }
231
232        return currentDistance;
233}
234
235#endif //_PART_DISTANCE_ESTIMATOR_H_
Note: See TracBrowser for help on using the repository browser.