FreeCAD

1
// SPDX-License-Identifier: LGPL-2.1-or-later
2

3
#include <cmath>
4

5
#include <gtest/gtest.h>
6

7
#include "Mod/Sketcher/App/planegcs/GCS.h"
8
#include "Mod/Sketcher/App/planegcs/Geo.h"
9
#include "Mod/Sketcher/App/planegcs/Constraints.h"
10

11
class SystemTest: public GCS::System
12
{
13
public:
14
    size_t getNumberOfConstraints(int tagID = -1)
15
    {
16
        return _getNumberOfConstraints(tagID);
17
    }
18
};
19

20
class ConstraintsTest: public ::testing::Test
21
{
22
protected:
23
    void SetUp() override
24
    {
25
        _system = std::make_unique<SystemTest>();
26
    }
27

28
    void TearDown() override
29
    {
30
        _system.reset();
31
    }
32

33
    SystemTest* System()
34
    {
35
        return _system.get();
36
    }
37

38
private:
39
    std::unique_ptr<SystemTest> _system;
40
};
41

42
TEST_F(ConstraintsTest, tangentBSplineAndArc)  // NOLINT
43
{
44
    // Arrange
45
    // TODO: Add arc, B-spline, and point
46
    double pointX = 3.5, arcStartX = 5.0, arcEndX = 0.0, arcCenterX = 0.0;
47
    double pointY = 3.5, arcStartY = 0.0, arcEndY = 5.0, arcCenterY = 0.0;
48
    GCS::Point point, arcStart, arcEnd, arcCenter;
49
    point.x = &pointX;
50
    point.y = &pointY;
51
    arcStart.x = &arcStartX;
52
    arcStart.y = &arcStartY;
53
    arcEnd.x = &arcEndX;
54
    arcEnd.y = &arcEndY;
55
    arcCenter.x = &arcCenterX;
56
    arcCenter.y = &arcCenterY;
57
    double arcRadius = 5.0, arcStartAngle = 0.0, arcEndAngle = M_PI / 2;
58
    double desiredAngle = M_PI;
59
    double bSplineStartX = 0.0, bSplineEndX = 16.0;
60
    double bSplineStartY = 10.0, bSplineEndY = -10.0;
61
    GCS::Point bSplineStart, bSplineEnd;
62
    bSplineStart.x = &bSplineStartX;
63
    bSplineStart.y = &bSplineStartY;
64
    bSplineEnd.x = &bSplineEndX;
65
    bSplineEnd.y = &bSplineEndY;
66
    std::vector<double> bSplineControlPointsX(5);
67
    std::vector<double> bSplineControlPointsY(5);
68
    bSplineControlPointsX[0] = 0.0;
69
    bSplineControlPointsY[0] = 10.0;
70
    bSplineControlPointsX[1] = 0.0;
71
    bSplineControlPointsY[1] = 6.0;
72
    bSplineControlPointsX[2] = 6.0;
73
    bSplineControlPointsY[2] = 0.5;
74
    bSplineControlPointsX[3] = 16.0;
75
    bSplineControlPointsY[3] = 0.5;
76
    bSplineControlPointsX[4] = 16.0;
77
    bSplineControlPointsY[4] = -10.0;
78
    std::vector<GCS::Point> bSplineControlPoints(5);
79
    for (size_t i = 0; i < bSplineControlPoints.size(); ++i) {
80
        bSplineControlPoints[i].x = &bSplineControlPointsX[i];
81
        bSplineControlPoints[i].y = &bSplineControlPointsY[i];
82
    }
83
    std::vector<double> weights(bSplineControlPoints.size(), 1.0);
84
    std::vector<double*> weightsAsPtr;
85
    std::vector<double> knots(bSplineControlPoints.size() - 2);  // Hardcoded for cubic
86
    std::vector<double*> knotsAsPtr;
87
    std::vector<int> mult(bSplineControlPoints.size() - 2, 1);  // Hardcoded for cubic
88
    mult.front() = 4;                                           // Hardcoded for cubic
89
    mult.back() = 4;                                            // Hardcoded for cubic
90
    for (size_t i = 0; i < bSplineControlPoints.size(); ++i) {
91
        weightsAsPtr.push_back(&weights[i]);
92
    }
93
    for (size_t i = 0; i < knots.size(); ++i) {
94
        knots[i] = i;
95
        knotsAsPtr.push_back(&knots[i]);
96
    }
97
    GCS::Arc arc;
98
    arc.start = arcStart;
99
    arc.end = arcEnd;
100
    arc.center = arcCenter;
101
    arc.rad = &arcRadius;
102
    arc.startAngle = &arcStartAngle;
103
    arc.endAngle = &arcEndAngle;
104
    GCS::BSpline bspline;
105
    bspline.start = bSplineStart;
106
    bspline.end = bSplineEnd;
107
    bspline.poles = bSplineControlPoints;
108
    bspline.weights = weightsAsPtr;
109
    bspline.knots = knotsAsPtr;
110
    bspline.mult = mult;
111
    bspline.degree = 3;
112
    bspline.periodic = false;
113
    double bsplineParam = 0.35;
114

115
    std::vector<double*> params = {point.x,
116
                                   point.y,
117
                                   arcStart.x,
118
                                   arcStart.y,
119
                                   arcEnd.x,
120
                                   arcEnd.y,
121
                                   arcCenter.x,
122
                                   arcCenter.y,
123
                                   &arcRadius,
124
                                   bSplineStart.x,
125
                                   bSplineStart.y,
126
                                   bSplineEnd.x,
127
                                   bSplineEnd.y,
128
                                   &bSplineControlPointsX[0],
129
                                   &bSplineControlPointsY[0],
130
                                   &bSplineControlPointsX[1],
131
                                   &bSplineControlPointsY[1],
132
                                   &bSplineControlPointsX[2],
133
                                   &bSplineControlPointsY[2],
134
                                   &bSplineControlPointsX[3],
135
                                   &bSplineControlPointsY[3],
136
                                   &bSplineControlPointsX[4],
137
                                   &bSplineControlPointsY[4],
138
                                   &desiredAngle,
139
                                   &bsplineParam};
140
    params.insert(params.end(), weightsAsPtr.begin(), weightsAsPtr.end());
141
    params.insert(params.end(), knotsAsPtr.begin(), knotsAsPtr.end());
142

143
    // Act
144
    // TODO: Apply constraint and solve
145
    System()->addConstraintArcRules(arc);
146
    System()->addConstraintPointOnArc(point, arc, 0, true);
147
    System()->addConstraintPointOnBSpline(point, bspline, &bsplineParam, 0, true);
148
    System()->addConstraintAngleViaPointAndParam(bspline,
149
                                                 arc,
150
                                                 point,
151
                                                 &bsplineParam,
152
                                                 &desiredAngle,
153
                                                 0,
154
                                                 true);
155
    int solveResult = System()->solve(params);
156
    if (solveResult == GCS::Success) {
157
        System()->applySolution();
158
    }
159

160
    // Assert
161
    EXPECT_EQ(solveResult, GCS::Success);
162
    // is point on arc?
163
    EXPECT_DOUBLE_EQ((arcRadius) * (arcRadius),
164
                     (pointX - arcCenterX) * (pointX - arcCenterX)
165
                         + (pointY - arcCenterY) * (pointY - arcCenterY));
166
    // is point on B-spline?
167
    GCS::DeriVector2 pointAtBSplineParam = bspline.Value(bsplineParam, 1.0);
168
    EXPECT_DOUBLE_EQ(pointAtBSplineParam.x, pointX);
169
    EXPECT_DOUBLE_EQ(pointAtBSplineParam.y, pointY);
170
    // TODO: are tangents at relevant parameter equal?
171
    GCS::DeriVector2 centerToPoint((pointX - arcCenterX), (pointY - arcCenterY));
172
    GCS::DeriVector2 tangentBSplineAtPoint(pointAtBSplineParam.dx, pointAtBSplineParam.dy);
173
    double dprd;
174
    // FIXME: This error is probably too high. Fixing this may require improving the solver,
175
    // however.
176
    EXPECT_NEAR(std::fabs(centerToPoint.crossProdNorm(tangentBSplineAtPoint, dprd))
177
                    / (centerToPoint.length() * tangentBSplineAtPoint.length()),
178
                1.0,
179
                0.005);
180
}
181