kernel.cpp

template <typename T>
bool operator()(T const* const* parameters, T* residuals) const {
    const T* inputs = parameters[0];
    size_t i = 0;
    for (size_t n = _constraints.size(); i < n; ++i) {
        T residual(0.0);
        const Constraint& constraint = _constraints[i];
        switch (constraint.type) {
            case ConstraintType::ArcLineTangent: {
                const auto& info = constraint.arc_line_tangent;
                EntityId lsource = _entity_map.at(info.line.id)->line.source_point;
                EntityId ltarget = _entity_map.at(info.line.id)->line.target_point;
                JetVec2<T> lsourcePt = get_coord_for_point(inputs, lsource);
                JetVec2<T> ltargetPt = get_coord_for_point(inputs, ltarget);
                EntityId acenter = _entity_map.at(info.arc.id)->arc.center_point;
                EntityId asource = _entity_map.at(info.arc.id)->arc.source_point;
                EntityId atarget = _entity_map.at(info.arc.id)->arc.target_point;
                EntityId aend = info.is_target ? atarget : asource;
                JetVec2<T> acenterPt = get_coord_for_point(inputs, acenter);
                JetVec2<T> aendPt = get_coord_for_point(inputs, aend);
                residual = dot(lsourcePt - ltargetPt, acenterPt - aendPt);
                break;
            }
            case ConstraintType::PointPointDistance: {
                const auto& info = constraint.point_point_distance;
                JetVec2<T> a = get_coord_for_point(inputs, info.source);
                JetVec2<T> b = get_coord_for_point(inputs, info.target);
                residual = distance(a, b) - info.distance;
                break;
            }
        }
        residuals[i] = residual * residual;
    }

    // Next, process the hidden "equal line length" constraint caused by the existence of arcs.
    for (size_t j = 0, n = _arc_entities.size(); j < n; ++j, ++i) {
        const auto& info = _arc_entities[j]->arc;
        JetVec2<T> s = get_coord_for_point(inputs, info.source_point);
        JetVec2<T> t = get_coord_for_point(inputs, info.target_point);
        JetVec2<T> c = get_coord_for_point(inputs, info.center_point);
        T residual = distance(s, c) - distance(t, c);
        residuals[i] = residual * residual;
    }
    return true;
}