70 const Eigen::MatrixXd &polygonVertices,
71 const double &polygonDiameter,
72 const Eigen::Vector3d &polygonCentroid,
73 const std::vector<bool> &edgeDirections,
74 const Eigen::MatrixXd &edgeTangents,
75 std::vector<Eigen::MatrixXd> &Cmatrixkp1)
const
77 std::vector<Eigen::VectorXd> BinomialCoefficients(polynomialDegree + 2);
79 for (
unsigned int n = 0; n <= (polynomialDegree + 1); n++)
81 BinomialCoefficients[n].resize(n + 2);
82 for (
unsigned int k = 0; k <= n; k++)
88 const unsigned int numVertices = polygonVertices.cols();
89 const unsigned int numEdges = numVertices;
91 const unsigned int order_1D = (polynomialDegree + 2);
92 const unsigned int nkp1 = (polynomialDegree + 2) * (polynomialDegree + 3) / 2;
94 Cmatrixkp1.resize(numEdges);
96 for (
unsigned int e = 0; e < numEdges; ++e)
98 Cmatrixkp1[e] = Eigen::MatrixXd::Zero(nkp1, order_1D);
100 const Eigen::Vector3d &edgeStart = edgeDirections[e] ? polygonVertices.col(e)
101 : polygonVertices.col((e + 1) % numVertices);
102 const Eigen::Vector3d &edgeTangent = edgeTangents.col(e);
103 const double direction = edgeDirections[e] ? 1.0 : -1.0;
105 double invDiameter = 1.0 / polygonDiameter;
106 double XAminusXC = edgeStart(0) - polygonCentroid(0);
107 double YAminusYC = edgeStart(1) - polygonCentroid(1);
108 double X = XAminusXC * invDiameter;
109 double Y = YAminusYC * invDiameter;
110 double diffX = direction * edgeTangent(0) / XAminusXC;
111 double diffY = direction * edgeTangent(1) / YAminusYC;
113 if (abs(XAminusXC) > 1.0e-12 && abs(YAminusYC) > 1.0e-12)
115 Eigen::VectorXd vettX = Eigen::VectorXd::Ones(polynomialDegree + 2);
116 Eigen::VectorXd vettY = Eigen::VectorXd::Ones(polynomialDegree + 2);
117 Eigen::VectorXd vettDiffX = Eigen::VectorXd::Ones(polynomialDegree + 2);
118 Eigen::VectorXd vettDiffY = Eigen::VectorXd::Ones(polynomialDegree + 2);
120 for (
unsigned int i = 0; i < (polynomialDegree + 1); i++)
122 vettX[i + 1] = vettX[i] * X;
123 vettY[i + 1] = vettY[i] * Y;
124 vettDiffX[i + 1] = vettDiffX[i] * diffX;
125 vettDiffY[i + 1] = vettDiffY[i] * diffY;
128 Cmatrixkp1[e](0, 0) = 1.0;
129 unsigned int offsetRow = 1;
131 for (
unsigned int N = 1; N <= (polynomialDegree + 1); N++)
133 for (
unsigned int alphay = 0; alphay <= N; alphay++)
135 for (
unsigned int j = 0; j <= alphay; j++)
137 for (
unsigned int i = 0; i <= (N - alphay); i++)
139 Cmatrixkp1[e](offsetRow, (j + i)) = Cmatrixkp1[e](offsetRow, (j + i)) +
140 BinomialCoefficients[N - alphay][i] * vettDiffX[i] *
141 BinomialCoefficients[alphay][j] * vettDiffY[j];
144 Cmatrixkp1[e].block(offsetRow, 0, 1, order_1D) =
145 Cmatrixkp1[e].block(offsetRow, 0, 1, order_1D) * vettX[N - alphay] * vettY[alphay];
150 else if (abs(XAminusXC) > 1.0e-12 && abs(YAminusYC) <= 1.0e-12)
152 Eigen::VectorXd vettX = Eigen::VectorXd::Ones(polynomialDegree + 2);
153 Eigen::VectorXd vettY = Eigen::VectorXd::Ones(polynomialDegree + 2);
154 Eigen::VectorXd vettDiffX = Eigen::VectorXd::Ones(polynomialDegree + 2);
156 double YBminusYAForInvDiameter = direction * edgeTangent(1) * invDiameter;
158 for (
unsigned int i = 0; i < (polynomialDegree + 1); i++)
160 vettX[i + 1] = vettX[i] * X;
161 vettY[i + 1] = vettY[i] * YBminusYAForInvDiameter;
162 vettDiffX[i + 1] = vettDiffX[i] * diffX;
165 Cmatrixkp1[e](0, 0) = 1.0;
166 unsigned int offsetRow = 1;
168 for (
unsigned int N = 1; N <= (polynomialDegree + 1); N++)
170 for (
unsigned int alphay = 0; alphay <= N; alphay++)
172 unsigned int j = alphay;
173 for (
unsigned int i = 0; i <= (N - alphay); i++)
175 Cmatrixkp1[e](offsetRow, (j + i)) =
176 Cmatrixkp1[e](offsetRow, (j + i)) + BinomialCoefficients[N - alphay][i] * vettDiffX[i];
179 Cmatrixkp1[e].block(offsetRow, 0, 1, order_1D) =
180 Cmatrixkp1[e].block(offsetRow, 0, 1, order_1D) * vettX[N - alphay] * vettY[alphay];
185 else if (abs(XAminusXC) <= 1.0e-12 && abs(YAminusYC) > 1.0e-12)
187 Eigen::VectorXd vettX = Eigen::VectorXd::Ones(polynomialDegree + 2);
188 Eigen::VectorXd vettY = Eigen::VectorXd::Ones(polynomialDegree + 2);
189 Eigen::VectorXd vettDiffY = Eigen::VectorXd::Ones(polynomialDegree + 2);
191 double XBminusXAForInvDiameter = direction * edgeTangent(0) * invDiameter;
193 for (
unsigned int i = 0; i < (polynomialDegree + 1); i++)
195 vettX[i + 1] = vettX[i] * XBminusXAForInvDiameter;
196 vettY[i + 1] = vettY[i] * Y;
197 vettDiffY[i + 1] = vettDiffY[i] * diffY;
200 Cmatrixkp1[e](0, 0) = 1.0;
201 unsigned int offsetRow = 1;
203 for (
unsigned int N = 1; N <= (polynomialDegree + 1); N++)
205 for (
unsigned int alphay = 0; alphay <= N; alphay++)
207 for (
unsigned int j = 0; j <= alphay; j++)
209 unsigned int i = N - alphay;
210 Cmatrixkp1[e](offsetRow, (j + i)) =
211 Cmatrixkp1[e](offsetRow, (j + i)) + BinomialCoefficients[alphay][j] * vettDiffY[j];
213 Cmatrixkp1[e].block(offsetRow, 0, 1, order_1D) =
214 Cmatrixkp1[e].block(offsetRow, 0, 1, order_1D) * vettX[N - alphay] * vettY[alphay];
221 throw std::runtime_error(
"Cmatrix is wrong");