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

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

Cosmetic
  • Property svn:eol-style set to native
File size: 13.2 KB
Line 
1// This file is a part of Framsticks SDK.  http://www.framsticks.com/
2// Copyright (C) 1999-2020  Maciej Komosinski and Szymon Ulatowski.
3// See LICENSE.txt for details.
4
5#include "geometryutils.h"
6#include <math.h>
7
8double GeometryUtils::pointPosition(const int pointIndex, const int numberOfPoints)
9{
10        if (numberOfPoints == 1)
11                return 0;
12        else
13                return pointIndex / (numberOfPoints-1.0);
14}
15
16double GeometryUtils::pointOnAxis(const double scale, const double position)
17{
18        return (position-0.5) * scale;
19}
20
21double GeometryUtils::pointOnAxis(const double scale, const int pointIndex, const int numberOfPoints)
22{
23        return pointOnAxis(scale, pointPosition(pointIndex, numberOfPoints));
24}
25
26double GeometryUtils::combination(const double value1, const double value2, const double position)
27{
28        return value1 + position * (value2-value1);
29}
30
31double GeometryUtils::combination(const double value1, const double value2, const int pointIndex, const int numberOfPoints)
32{
33        return combination(value1, value2, pointPosition(pointIndex, numberOfPoints));
34}
35
36bool GeometryUtils::isPointInsideModelExcludingPart(const Pt3D &point, const Model *model, const int excludedPartIndex)
37{
38        for (int i = 0; i < excludedPartIndex; i++)
39        {
40                if (isPointInsidePart(point, model->getPart(i)))
41                {
42                        return true;
43                }
44        }
45       
46        for (int i = excludedPartIndex+1; i < model->getPartCount(); i++)
47        {
48                if (isPointStrictlyInsidePart(point, model->getPart(i)))
49                {
50                        return true;
51                }
52        }
53       
54        return false;
55}
56
57bool GeometryUtils::isPointInsideModel(const Pt3D &point, const Model &model)
58{
59        for (int i = 0; i < model.getPartCount(); i++)
60        {
61                if (isPointInsidePart(point, model.getPart(i)))
62                {
63                        return true;
64                }
65        }
66       
67        return false;
68}
69
70bool GeometryUtils::isPointInsidePart(const Pt3D &point, const Part *part)
71{
72        switch (part->shape)
73        {
74                case Part::SHAPE_ELLIPSOID:
75                        return isPointInsideEllipsoid(point, part);
76                        break;
77                       
78                case Part::SHAPE_CUBOID:
79                        return isPointInsideCuboid(point, part);
80                        break;
81                       
82                case Part::SHAPE_CYLINDER:
83                        return isPointInsideCylinder(point, part);
84                        break;
85        }
86        logPrintf("GeometryUtils", "isPointInsidePart", LOG_ERROR, "Part shape=%d not supported", part->shape);
87        return false;
88}
89
90bool GeometryUtils::isPointStrictlyInsidePart(const Pt3D &point, const Part *part)
91{
92        switch (part->shape)
93        {
94                case Part::SHAPE_ELLIPSOID:
95                        return isPointStrictlyInsideEllipsoid(point, part);
96                        break;
97                       
98                case Part::SHAPE_CUBOID:
99                        return isPointStrictlyInsideCuboid(point, part);
100                        break;
101                       
102                case Part::SHAPE_CYLINDER:
103                        return isPointStrictlyInsideCylinder(point, part);
104                        break;
105        }
106        logPrintf("GeometryUtils", "isPointStrictlyInsidePart", LOG_ERROR, "Part shape=%d not supported", part->shape);
107        return false;
108}
109
110bool GeometryUtils::isPointInsideEllipsoid(const Pt3D &point, const Part *part)
111{
112        Pt3D moved = point - part->p;
113        Pt3D rotated;
114        part->o.revTransform(rotated, moved);
115       
116        double r
117                = (pow(rotated.x, 2.0) / pow(part->scale.x, 2.0))
118                + (pow(rotated.y, 2.0) / pow(part->scale.y, 2.0))
119                + (pow(rotated.z, 2.0) / pow(part->scale.z, 2.0));
120       
121        return r <= 1.0;
122}
123
124bool GeometryUtils::isPointStrictlyInsideEllipsoid(const Pt3D &point, const Part *part)
125{
126        Pt3D moved = point - part->p;
127        Pt3D rotated;
128        part->o.revTransform(rotated, moved);
129       
130        double r
131                = (pow(rotated.x, 2.0) / pow(part->scale.x, 2.0))
132                + (pow(rotated.y, 2.0) / pow(part->scale.y, 2.0))
133                + (pow(rotated.z, 2.0) / pow(part->scale.z, 2.0));
134       
135        return r < 1.0;
136}
137
138bool GeometryUtils::isPointInsideCuboid(const Pt3D &point, const Part *part)
139{
140        Pt3D moved = point - part->p;
141        Pt3D rotated;
142        part->o.revTransform(rotated, moved);
143       
144        return (fabs(rotated.x) <= part->scale.x)
145                && (fabs(rotated.y) <= part->scale.y)
146                && (fabs(rotated.z) <= part->scale.z);
147}
148
149bool GeometryUtils::isPointStrictlyInsideCuboid(const Pt3D &point, const Part *part)
150{
151        Pt3D moved = point - part->p;
152        Pt3D rotated;
153        part->o.revTransform(rotated, moved);
154       
155        return (fabs(rotated.x) < part->scale.x)
156                && (fabs(rotated.y) < part->scale.y)
157                && (fabs(rotated.z) < part->scale.z);
158}
159
160bool GeometryUtils::isPointInsideCylinder(const Pt3D &point, const Part *part)
161{
162        Pt3D moved = point - part->p;
163        Pt3D rotated;
164        part->o.revTransform(rotated, moved);
165       
166        double r
167                = (pow(rotated.y, 2.0) / pow(part->scale.y, 2.0))
168                + (pow(rotated.z, 2.0) / pow(part->scale.z, 2.0));
169       
170        return (fabs(rotated.x) <= part->scale.x) && (r <= 1.0);
171}
172
173bool GeometryUtils::isPointStrictlyInsideCylinder(const Pt3D &point, const Part *part)
174{
175        Pt3D moved = point - part->p;
176        Pt3D rotated;
177        part->o.revTransform(rotated, moved);
178       
179        double r
180                = (pow(rotated.y, 2.0) / pow(part->scale.y, 2.0))
181                + (pow(rotated.z, 2.0) / pow(part->scale.z, 2.0));
182       
183        return (fabs(rotated.x) < part->scale.x) && (r < 1.0);
184}
185
186void GeometryUtils::findSizesAndAxesOfPointsGroup(SListTempl<Pt3D> &points, Pt3D &sizes,
187        Orient &axes)
188{
189        findSizeAndAxisOfPointsGroup(points, sizes.x, axes.x);
190        orthographicProjectionToPlane(points, axes.x);
191        findSizeAndAxisOfPointsGroup(points, sizes.y, axes.y);
192        orthographicProjectionToPlane(points, axes.y);
193       
194        Pt3D minimal(points.get(0)), maximal(points.get(0));
195       
196        for (int i = 1; i < points.size(); i++)
197        {
198                minimal.getMin(points.get(i));
199                maximal.getMax(points.get(i));
200        }
201       
202        sizes.z = minimal.distanceTo(maximal);
203        axes.z.vectorProduct(axes.x, axes.y);
204}
205
206void GeometryUtils::findSizeAndAxisOfPointsGroup(const SListTempl<Pt3D> &points, double &size,
207        Pt3D &axis)
208{
209        int index1, index2;
210        size = findTwoFurthestPoints(points, index1, index2);
211        createAxisFromTwoPoints(axis, points.get(index1), points.get(index2));
212}
213
214double GeometryUtils::findTwoFurthestPoints(const SListTempl<Pt3D> &points, int &index1,
215        int &index2)
216{
217        double distance = 0;
218        index1 = index2 = 0;
219       
220        for (int i = 0; i < points.size()-1; i++)
221        {
222                Pt3D p1 = points.get(i);
223               
224                for (int j = i+1; j < points.size(); j++)
225                {
226                        Pt3D p2 = points.get(j);
227                        double d = p1.distanceTo(p2);
228                       
229                        if (d > distance)
230                        {
231                                distance = d;
232                                index1 = i;
233                                index2 = j;
234                        }
235                }
236        }
237       
238        return distance;
239}
240
241void GeometryUtils::createAxisFromTwoPoints(Pt3D &axis, const Pt3D &point1, const Pt3D &point2)
242{
243        Pt3D vector = point2 - point1;
244        vector.normalize();
245       
246        axis.x = vector.x;
247        axis.y = vector.y;
248        axis.z = vector.z;
249}
250
251void GeometryUtils::orthographicProjectionToPlane(SListTempl<Pt3D> &points,
252        const Pt3D &planeNormalVector)
253{
254        for (int i = 0; i < points.size(); i++)
255        {
256                Pt3D &point = points.get(i);
257               
258                double distance = pointDistanceToPlane(point, planeNormalVector);
259               
260                point.x -= planeNormalVector.x * distance;
261                point.y -= planeNormalVector.y * distance;
262                point.z -= planeNormalVector.z * distance;
263        }
264}
265
266double GeometryUtils::pointDistanceToPlane(const Pt3D &point, const Pt3D &planeNormalVector)
267{
268        return planeNormalVector.x*point.x + planeNormalVector.y*point.y + planeNormalVector.z*point.z;
269}
270
271void GeometryUtils::getRectangleApicesFromCuboid(const Part *part, const CuboidFaces::Face face, Pt3D &apex1, Pt3D &apex2, Pt3D &apex3, Pt3D &apex4)
272{
273        Pt3D temp1(part->scale), temp2(part->scale), temp3(part->scale), temp4(part->scale);
274       
275        if (CuboidFaces::isX(face))
276        {
277                temp2.z *= -1;
278                temp3.y *= -1;
279                temp4.z *= -1;
280                temp4.y *= -1;
281        }
282        else if (CuboidFaces::isY(face))
283        {
284                temp2.x *= -1;
285                temp3.z *= -1;
286                temp4.x *= -1;
287                temp4.z *= -1;
288        }
289        else if (CuboidFaces::isZ(face))
290        {
291                temp2.y *= -1;
292                temp3.x *= -1;
293                temp4.y *= -1;
294                temp4.x *= -1;
295        }
296       
297        if (CuboidFaces::isNegative(face))
298        {
299                temp1 *= -1;
300                temp2 *= -1;
301                temp3 *= -1;
302                temp4 *= -1;
303        }
304
305        part->o.transform(apex1, temp1);
306        part->o.transform(apex2, temp2);
307        part->o.transform(apex3, temp3);
308        part->o.transform(apex4, temp4);
309
310        apex1 += part->p;
311        apex2 += part->p;
312        apex3 += part->p;
313        apex4 += part->p;
314}
315
316void GeometryUtils::getRectangleApices(const double width, const double height, const Pt3D &position, const Orient &orient, Pt3D &apex1, Pt3D &apex2, Pt3D &apex3, Pt3D &apex4)
317{
318        Pt3D temp1(0.0, +width, +height);
319        Pt3D temp2(0.0, +width, -height);
320        Pt3D temp3(0.0, -width, +height);
321        Pt3D temp4(0.0, -width, -height);
322
323        orient.transform(apex1, temp1);
324        orient.transform(apex2, temp2);
325        orient.transform(apex3, temp3);
326        orient.transform(apex4, temp4);
327
328        apex1 += position;
329        apex2 += position;
330        apex3 += position;
331        apex4 += position;
332}
333
334void GeometryUtils::getNextEllipseSegmentationPoint(const double d, const double a, const double b, double &x, double &y)
335{
336        x += d / sqrt(1.0 + (b*b * x*x) / (a*a * (a*a - x*x)));
337        double sqrt_arg = 1.0 - (x*x) / (a*a);
338        if (sqrt_arg >= 0)
339                y = b * sqrt(sqrt_arg);
340        else
341#ifdef __BORLANDC__ //compiler bug: embarcadero 10.3u3 raises "invalid fp operation exception" even when the execution does not enter the "signaling_NaN()" branch of "if" (i.e. when sqrt_arg >= 0) so we avoid using signaling_NaN().
342                y = std::numeric_limits<double>::max();
343#else
344                y = std::numeric_limits<double>::signaling_NaN();
345#endif
346        //This function is called from MeshBuilder::EllipsoidSurface::findNextAreaEdgeAndPhase().
347        //y=NaN set above co-occurs with the value of x that doesn't meet the condition tested in findNextAreaEdgeAndPhase().
348        //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.
349        //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.
350}
351
352double GeometryUtils::ellipsoidArea(const Pt3D &sizes)
353{
354        return ellipsoidArea(sizes.x, sizes.y, sizes.z);
355}
356
357double GeometryUtils::ellipsoidArea(const double a, const double b, const double c)
358{
359        double p = 1.6075;
360        double ap = pow(a, p);
361        double bp = pow(b, p);
362        double cp = pow(c, p);
363        return 4*M_PI * pow((ap*bp + bp*cp + cp*ap) / 3.0, 1.0 / p);
364}
365
366double GeometryUtils::ellipsePerimeter(const double a, const double b)
367{
368        return M_PI * ((3 * (a+b)) - sqrt((3*a + b) * (a + 3*b)));
369}
370
371double GeometryUtils::calculateSolidVolume(const Part * part)
372{
373        double radiiProduct = part->scale.x * part->scale.y * part->scale.z;
374        switch (part->shape)
375        {
376                case Part::Shape::SHAPE_CUBOID:
377                        return 8.0 * radiiProduct;
378                case Part::Shape::SHAPE_CYLINDER:
379                        return  2.0 * M_PI * radiiProduct;
380                case Part::Shape::SHAPE_ELLIPSOID:
381                        return  (4.0 / 3.0) * M_PI * radiiProduct;
382                default:
383                        logMessage("GeometryUtils", "calculateSolidVolume", LOG_ERROR, "Unsupported part shape");
384                        return -1;
385        }
386}
387
388bool GeometryUtils::isSolidPartScaleValid(const Part::Shape &partShape, const Pt3D &scale, bool ensureCircleSection)
389{
390        Part *tmpPart = new Part(partShape);
391        tmpPart->scale = scale;
392        double volume = GeometryUtils::calculateSolidVolume(tmpPart);
393
394        Part_MinMaxDef minP = Model::getMinPart();
395        Part_MinMaxDef maxP = Model::getMaxPart();
396
397        if (volume > maxP.volume || minP.volume > volume)
398                return false;
399        if (scale.x < minP.scale.x || scale.y < minP.scale.y || scale.z < minP.scale.z)
400                return false;
401        if (scale.x > maxP.scale.x || scale.y > maxP.scale.y || scale.z > maxP.scale.z)
402                return false;
403
404        if (ensureCircleSection)
405        {
406                if (partShape == Part::Shape::SHAPE_ELLIPSOID && scale.maxComponentValue() != scale.minComponentValue()) // Any radius has a different value than the other ones?
407                        return false;
408                if (partShape == Part::Shape::SHAPE_CYLINDER && scale.y != scale.z) // Base radii have different values?
409                        return false;
410        }
411        return true;
412}
413
414void GeometryUtils::addAnchorToModel(Model &model)
415{
416    Part *part = model.addNewPart(Part::SHAPE_ELLIPSOID);
417
418    part->p = Pt3D(0);
419    part->scale = Pt3D(0.1);
420    part->vcolor = Pt3D(1.0, 0.0, 1.0);
421
422    addAxesToModel(Pt3D(0.5), Orient(Orient_1), Pt3D(0.0), model);
423}
424
425void GeometryUtils::addPointToModel(const Pt3D &markerLocation, Model &model)
426{
427    Part *anchor = model.getPart(0);
428    Part *part = model.addNewPart(Part::SHAPE_ELLIPSOID);
429
430    part->p = Pt3D(markerLocation);
431    part->scale = Pt3D(0.05);
432    part->vcolor = Pt3D(1.0, 1.0, 0.0);
433
434    model.addNewJoint(anchor, part, Joint::SHAPE_FIXED);
435}
436
437void GeometryUtils::addAxesToModel(const Pt3D &sizes, const Orient &axes, const Pt3D &center,
438                                   Model &model)
439{
440    Part *anchor = model.getPart(0);
441    Part *part;
442
443    part = model.addNewPart(Part::SHAPE_CUBOID);
444    part->scale = Pt3D(sizes.x, 0.05, 0.05);
445    part->setOrient(axes);
446    part->p = center;
447    part->vcolor = Pt3D(1.0, 0.0, 0.0);
448    model.addNewJoint(anchor, part, Joint::SHAPE_FIXED);
449
450    part = model.addNewPart(Part::SHAPE_CUBOID);
451    part->scale = Pt3D(0.05, sizes.y, 0.05);
452    part->setOrient(axes);
453    part->p = center;
454    part->vcolor = Pt3D(0.0, 1.0, 0.0);
455    model.addNewJoint(anchor, part, Joint::SHAPE_FIXED);
456
457    part = model.addNewPart(Part::SHAPE_CUBOID);
458    part->scale = Pt3D(0.05, 0.05, sizes.z);
459    part->setOrient(axes);
460    part->p = center;
461    part->vcolor = Pt3D(0.0, 0.0, 1.0);
462    model.addNewJoint(anchor, part, Joint::SHAPE_FIXED);
463}
464
465void GeometryUtils::mergeModels(Model &target, Model &source)
466{
467    Part *targetAnchor = target.getPart(0);
468    Part *sourceAnchor = source.getPart(0);
469
470    target.moveElementsFrom(source);
471
472    target.addNewJoint(targetAnchor, sourceAnchor, Joint::SHAPE_FIXED);
473}
474
475double frand(double from, double width)
476{
477    return from + width * ((rand() % 10000) / 10000.0);
478}
479
480void GeometryUtils::randomizePositionScaleAndOrient(Part *part)
481{
482    part->p = Pt3D(frand(1.5, 1.0), frand(1.5, 1.0), frand(1.5, 1.0));
483    part->scale = Pt3D(frand(0.1, 0.9), frand(0.1, 0.9), frand(0.1, 0.9));
484    part->setRot(Pt3D(frand(0.0, M_PI), frand(0.0, M_PI), frand(0.0, M_PI)));
485}
Note: See TracBrowser for help on using the repository browser.