Geometry & Recognition

std::vector<double> uniform_knot(const double h, const int resolution) {
    std::vector<double> knot(resolution);
    for (int i = knot.size(); i-->0;) 
        knot[i] = i * h;
    return knot;
}
// nCpt: number of control points;
// knot: knot vector(uniform);
std::vector<double> design_matrix(const std::vector<double>& knot, const int nCpt) {
    // D: design matrix;
    std::vector<double> D(knot.size() * nCpt, 0);	
    for (int k = knot.size(); k-->0;) {
        int curr  = int(knot[k]);
        int prev  = max(0,        curr - 1);	
        int next  = min(nCpt - 1, curr + 1);
        int nnext = min(nCpt - 1, curr + 2);

        double s = knot[k] - curr;

        D[k * nCpt + prev]  += basis0(s);
        D[k * nCpt + curr]  += basis1(s);
        D[k * nCpt + next]  += basis2(s);
        D[k * nCpt + nnext] += basis3(s);
    }
    return D;
};
// Scatter matrix
// S = transpose(D)*D; 
std::vector<double> scatter_matrix(const std::vector<double>& D, int rows, int cols) {
    std::vector<double> S(cols * cols);
    for (int i = 0; i < cols; i++)
        for (int j = i; j < cols; j++) {
            double sum = 0;
            for (int k = 0; k < rows; k++) 
                sum += D[k * cols + i] * D[k * cols + j];
            S[i * cols + j] = sum;
        }
    
    // lower triangle of symmetric scatter matrix S;
    for (int i = 0; i < cols; i++) 
        for (int j = 0; j < i; j++)
            S[i * cols + j] = S[j * cols + i];
    return S;
}
std::vector<CfPt> BSplineFit_LS(const std::vector<CfPt>& P, const int nCpt) {
    if (P.size() < 2) return std::vector<CfPt> ();
    const double delta = double(nCpt - 1) / (P.size() - 1);
    std::vector<double> knot = uniform_knot(delta, P.size());
    // D: design matrix;
    std::vector<double> D = design_matrix(knot, nCpt);
    // S: scatter matrix(symmetric): S = transpose(D)*D;
    std::vector<double> S = scatter_matrix(D, knot.size(), nCpt);
 
    // X = transpose(D)*P;
    std::vector<CfPt> X(nCpt, 0);
    for (int i = 0; i < nCpt; ++i)
        for (int k = P.size(); k-->0;) 
            X[i] += P[k] * D[k * nCpt + i]; // scalar 곱은 뒤에서;

    // inverse(S) -> S;
    if (psinv(&S[0], nCpt) == -1) 
        return std::vector<CfPt> ();
    
    // Q = inverse(S)*X = inverse(transpose(D)*D)*transpose(D)*P;
    std::vector<CfPt> Q(nCpt, 0); // estimated control points;
    for (int i = 0; i < nCpt; ++i) {
        for (int k = 0; k < nCpt; ++k) 
            Q[i] += X[k] * S[i * nCpt + k];  //scalar 곱은 뒤에서;
    }
    return Q;
};