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

Last change on this file since 1281 was 1173, checked in by Maciej Komosinski, 3 years ago

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