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

Last change on this file since 270 was 261, checked in by Maciej Komosinski, 10 years ago

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