source: cpp/frams/model/geometry/geometryutils.cpp @ 896

Last change on this file since 896 was 810, checked in by Maciej Komosinski, 6 years ago

Don't unnecessarily calculate sqrt(negative_value) [fixes #51]
  • Property svn:eol-style set to native
File size: 9.4 KB
Line 
1// This file is a part of Framsticks SDK.  http://www.framsticks.com/
2// Copyright (C) 1999-2015  Maciej Komosinski and Szymon Ulatowski.
3// See LICENSE.txt for details.
4
5#include "geometryutils.h"
6
7#include <math.h>
8
9double GeometryUtils::pointPosition(const int pointIndex, const int numberOfPoints)
10{
11        if (numberOfPoints == 1)
12                return 0;
13        else
14                return pointIndex / (numberOfPoints-1.0);
15}
16
17double GeometryUtils::pointOnAxis(const double scale, const double position)
18{
19        return (position-0.5) * scale;
20}
21
22double GeometryUtils::pointOnAxis(const double scale, const int pointIndex, const int numberOfPoints)
23{
24        return pointOnAxis(scale, pointPosition(pointIndex, numberOfPoints));
25}
26
27double GeometryUtils::combination(const double value1, const double value2, const double position)
28{
29        return value1 + position * (value2-value1);
30}
31
32double GeometryUtils::combination(const double value1, const double value2, const int pointIndex, const int numberOfPoints)
33{
34        return combination(value1, value2, pointPosition(pointIndex, numberOfPoints));
35}
36
37bool GeometryUtils::isPointInsideModelExcludingPart(const Pt3D &point, const Model *model, const int excludedPartIndex)
38{
39        for (int i = 0; i < excludedPartIndex; i++)
40        {
41                if (isPointInsidePart(point, model->getPart(i)))
42                {
43                        return true;
44                }
45        }
46       
47        for (int i = excludedPartIndex+1; i < model->getPartCount(); i++)
48        {
49                if (isPointStrictlyInsidePart(point, model->getPart(i)))
50                {
51                        return true;
52                }
53        }
54       
55        return false;
56}
57
58bool GeometryUtils::isPointInsideModel(const Pt3D &point, const Model &model)
59{
60        for (int i = 0; i < model.getPartCount(); i++)
61        {
62                if (isPointInsidePart(point, model.getPart(i)))
63                {
64                        return true;
65                }
66        }
67       
68        return false;
69}
70
71bool GeometryUtils::isPointInsidePart(const Pt3D &point, const Part *part)
72{
73        switch (part->shape)
74        {
75                case Part::SHAPE_ELLIPSOID:
76                        return isPointInsideEllipsoid(point, part);
77                        break;
78                       
79                case Part::SHAPE_CUBOID:
80                        return isPointInsideCuboid(point, part);
81                        break;
82                       
83                case Part::SHAPE_CYLINDER:
84                        return isPointInsideCylinder(point, part);
85                        break;
86        }
87        logPrintf("GeometryUtils", "isPointInsidePart", LOG_ERROR, "Part shape=%d not supported", part->shape);
88        return false;
89}
90
91bool GeometryUtils::isPointStrictlyInsidePart(const Pt3D &point, const Part *part)
92{
93        switch (part->shape)
94        {
95                case Part::SHAPE_ELLIPSOID:
96                        return isPointStrictlyInsideEllipsoid(point, part);
97                        break;
98                       
99                case Part::SHAPE_CUBOID:
100                        return isPointStrictlyInsideCuboid(point, part);
101                        break;
102                       
103                case Part::SHAPE_CYLINDER:
104                        return isPointStrictlyInsideCylinder(point, part);
105                        break;
106        }
107        logPrintf("GeometryUtils", "isPointStrictlyInsidePart", LOG_ERROR, "Part shape=%d not supported", part->shape);
108        return false;
109}
110
111bool GeometryUtils::isPointInsideEllipsoid(const Pt3D &point, const Part *part)
112{
113        Pt3D moved = point - part->p;
114        Pt3D rotated;
115        part->o.revTransform(rotated, moved);
116       
117        double r
118                = (pow(rotated.x, 2.0) / pow(part->scale.x, 2.0))
119                + (pow(rotated.y, 2.0) / pow(part->scale.y, 2.0))
120                + (pow(rotated.z, 2.0) / pow(part->scale.z, 2.0));
121       
122        return r <= 1.0;
123}
124
125bool GeometryUtils::isPointStrictlyInsideEllipsoid(const Pt3D &point, const Part *part)
126{
127        Pt3D moved = point - part->p;
128        Pt3D rotated;
129        part->o.revTransform(rotated, moved);
130       
131        double r
132                = (pow(rotated.x, 2.0) / pow(part->scale.x, 2.0))
133                + (pow(rotated.y, 2.0) / pow(part->scale.y, 2.0))
134                + (pow(rotated.z, 2.0) / pow(part->scale.z, 2.0));
135       
136        return r < 1.0;
137}
138
139bool GeometryUtils::isPointInsideCuboid(const Pt3D &point, const Part *part)
140{
141        Pt3D moved = point - part->p;
142        Pt3D rotated;
143        part->o.revTransform(rotated, moved);
144       
145        return (fabs(rotated.x) <= part->scale.x)
146                && (fabs(rotated.y) <= part->scale.y)
147                && (fabs(rotated.z) <= part->scale.z);
148}
149
150bool GeometryUtils::isPointStrictlyInsideCuboid(const Pt3D &point, const Part *part)
151{
152        Pt3D moved = point - part->p;
153        Pt3D rotated;
154        part->o.revTransform(rotated, moved);
155       
156        return (fabs(rotated.x) < part->scale.x)
157                && (fabs(rotated.y) < part->scale.y)
158                && (fabs(rotated.z) < part->scale.z);
159}
160
161bool GeometryUtils::isPointInsideCylinder(const Pt3D &point, const Part *part)
162{
163        Pt3D moved = point - part->p;
164        Pt3D rotated;
165        part->o.revTransform(rotated, moved);
166       
167        double r
168                = (pow(rotated.y, 2.0) / pow(part->scale.y, 2.0))
169                + (pow(rotated.z, 2.0) / pow(part->scale.z, 2.0));
170       
171        return (fabs(rotated.x) <= part->scale.x) && (r <= 1.0);
172}
173
174bool GeometryUtils::isPointStrictlyInsideCylinder(const Pt3D &point, const Part *part)
175{
176        Pt3D moved = point - part->p;
177        Pt3D rotated;
178        part->o.revTransform(rotated, moved);
179       
180        double r
181                = (pow(rotated.y, 2.0) / pow(part->scale.y, 2.0))
182                + (pow(rotated.z, 2.0) / pow(part->scale.z, 2.0));
183       
184        return (fabs(rotated.x) < part->scale.x) && (r < 1.0);
185}
186
187void GeometryUtils::findSizesAndAxesOfPointsGroup(SListTempl<Pt3D> &points, Pt3D &sizes,
188        Orient &axes)
189{
190        findSizeAndAxisOfPointsGroup(points, sizes.x, axes.x);
191        orthographicProjectionToPlane(points, axes.x);
192        findSizeAndAxisOfPointsGroup(points, sizes.y, axes.y);
193        orthographicProjectionToPlane(points, axes.y);
194       
195        Pt3D minimal(points.get(0)), maximal(points.get(0));
196       
197        for (int i = 1; i < points.size(); i++)
198        {
199                minimal.getMin(points.get(i));
200                maximal.getMax(points.get(i));
201        }
202       
203        sizes.z = minimal.distanceTo(maximal);
204        axes.z.vectorProduct(axes.x, axes.y);
205}
206
207void GeometryUtils::findSizeAndAxisOfPointsGroup(const SListTempl<Pt3D> &points, double &size,
208        Pt3D &axis)
209{
210        int index1, index2;
211        size = findTwoFurthestPoints(points, index1, index2);
212        createAxisFromTwoPoints(axis, points.get(index1), points.get(index2));
213}
214
215double GeometryUtils::findTwoFurthestPoints(const SListTempl<Pt3D> &points, int &index1,
216        int &index2)
217{
218        double distance = 0;
219        index1 = index2 = 0;
220       
221        for (int i = 0; i < points.size()-1; i++)
222        {
223                Pt3D p1 = points.get(i);
224               
225                for (int j = i+1; j < points.size(); j++)
226                {
227                        Pt3D p2 = points.get(j);
228                        double d = p1.distanceTo(p2);
229                       
230                        if (d > distance)
231                        {
232                                distance = d;
233                                index1 = i;
234                                index2 = j;
235                        }
236                }
237        }
238       
239        return distance;
240}
241
242void GeometryUtils::createAxisFromTwoPoints(Pt3D &axis, const Pt3D &point1, const Pt3D &point2)
243{
244        Pt3D vector = point2 - point1;
245        vector.normalize();
246       
247        axis.x = vector.x;
248        axis.y = vector.y;
249        axis.z = vector.z;
250}
251
252void GeometryUtils::orthographicProjectionToPlane(SListTempl<Pt3D> &points,
253        const Pt3D &planeNormalVector)
254{
255        for (int i = 0; i < points.size(); i++)
256        {
257                Pt3D &point = points.get(i);
258               
259                double distance = pointDistanceToPlane(point, planeNormalVector);
260               
261                point.x -= planeNormalVector.x * distance;
262                point.y -= planeNormalVector.y * distance;
263                point.z -= planeNormalVector.z * distance;
264        }
265}
266
267double GeometryUtils::pointDistanceToPlane(const Pt3D &point, const Pt3D &planeNormalVector)
268{
269        return planeNormalVector.x*point.x + planeNormalVector.y*point.y + planeNormalVector.z*point.z;
270}
271
272void GeometryUtils::getRectangleApicesFromCuboid(const Part *part, const CuboidFaces::Face face, Pt3D &apex1, Pt3D &apex2, Pt3D &apex3, Pt3D &apex4)
273{
274        Pt3D temp1(part->scale), temp2(part->scale), temp3(part->scale), temp4(part->scale);
275       
276        if (CuboidFaces::isX(face))
277        {
278                temp2.z *= -1;
279                temp3.y *= -1;
280                temp4.z *= -1;
281                temp4.y *= -1;
282        }
283        else if (CuboidFaces::isY(face))
284        {
285                temp2.x *= -1;
286                temp3.z *= -1;
287                temp4.x *= -1;
288                temp4.z *= -1;
289        }
290        else if (CuboidFaces::isZ(face))
291        {
292                temp2.y *= -1;
293                temp3.x *= -1;
294                temp4.y *= -1;
295                temp4.x *= -1;
296        }
297       
298        if (CuboidFaces::isNegative(face))
299        {
300                temp1 *= -1;
301                temp2 *= -1;
302                temp3 *= -1;
303                temp4 *= -1;
304        }
305
306        part->o.transform(apex1, temp1);
307        part->o.transform(apex2, temp2);
308        part->o.transform(apex3, temp3);
309        part->o.transform(apex4, temp4);
310
311        apex1 += part->p;
312        apex2 += part->p;
313        apex3 += part->p;
314        apex4 += part->p;
315}
316
317void GeometryUtils::getRectangleApices(const double width, const double height, const Pt3D &position, const Orient &orient, Pt3D &apex1, Pt3D &apex2, Pt3D &apex3, Pt3D &apex4)
318{
319        Pt3D temp1(0.0, +width, +height);
320        Pt3D temp2(0.0, +width, -height);
321        Pt3D temp3(0.0, -width, +height);
322        Pt3D temp4(0.0, -width, -height);
323
324        orient.transform(apex1, temp1);
325        orient.transform(apex2, temp2);
326        orient.transform(apex3, temp3);
327        orient.transform(apex4, temp4);
328
329        apex1 += position;
330        apex2 += position;
331        apex3 += position;
332        apex4 += position;
333}
334
335void GeometryUtils::getNextEllipseSegmentationPoint(const double d, const double a, const double b, double &x, double &y)
336{
337        x += d / sqrt(1.0 + (b*b * x*x) / (a*a * (a*a - x*x)));
338        double sqrt_arg = 1.0 - (x*x) / (a*a);
339        if (sqrt_arg >= 0)
340                y = b * sqrt(sqrt_arg);
341        else
342                y = std::numeric_limits<double>::signaling_NaN();
343        //This function is called from MeshBuilder::EllipsoidSurface::findNextAreaEdgeAndPhase().
344        //y=NaN set above co-occurs with the value of x that doesn't meet the condition tested in findNextAreaEdgeAndPhase().
345        //If the condition is true (i.e., x exceeds the allowed range), entirely new values of x and y are set in the next step anyway.
346        //An impossible-to-calculate y should never be used for invalid x, hence y=NaN is set here to indicate this specific situation and signal just in case anyone would try to use such y.
347}
348
349double GeometryUtils::ellipsoidArea(const Pt3D &sizes)
350{
351        return ellipsoidArea(sizes.x, sizes.y, sizes.z);
352}
353
354double GeometryUtils::ellipsoidArea(const double a, const double b, const double c)
355{
356        double p = 1.6075;
357        double ap = pow(a, p);
358        double bp = pow(b, p);
359        double cp = pow(c, p);
360        return 4*M_PI * pow((ap*bp + bp*cp + cp*ap) / 3.0, 1.0 / p);
361}
362
363double GeometryUtils::ellipsePerimeter(const double a, const double b)
364{
365        return M_PI * ((3 * (a+b)) - sqrt((3*a + b) * (a + 3*b)));
366}
Note: See TracBrowser for help on using the repository browser.