857{
859
863
865
874
878 const Eigen::ParametrizedLine<double, 3> line(s, (e-s).normalized());
879 const double dist_inf = line.distance(f1.
get_point());
881 const double len_sq = (e-s).squaredNorm();
882 const double dist_start_sq = (proj-s).squaredNorm();
883 const double dist_end_sq = (proj-e).squaredNorm();
884 if (dist_start_sq < len_sq && dist_end_sq < len_sq) {
885
887 } else {
888 const bool s_is_closer = dist_start_sq < dist_end_sq;
890 }
894
896 const Eigen::Hyperplane<double, 3> circle_plane(n, c);
898 if (proj.isApprox(c)) {
901 }
902 else {
903 if (deal_circle_result == false) {
904 const Eigen::Hyperplane<double, 3> circle_plane(n, c);
906 const double dist = std::sqrt(std::pow((proj - c).norm() - radius, 2.) + (f1.
get_point() - proj).squaredNorm());
907
908 const Vec3d p_on_circle = c + radius * (proj - c).normalized();
910 }
911 else {
914 }
915 }
919 Eigen::Hyperplane<double, 3> plane(
normal, pt);
921
922 }
926 }
929 std::vector<DistAndPoints> distances;
930
931 auto add_point_edge_distance = [&distances](
const Vec3d& v,
const std::pair<Vec3d, Vec3d>& e) {
933 double distance = res.distance_strict->dist;
934 Vec3d v2 = res.distance_strict->to;
935
936 const Vec3d e1e2 = e.second - e.first;
937 const Vec3d e1v2 = v2 - e.first;
938 if (e1v2.dot(e1e2) >= 0.0 && e1v2.norm() < e1e2.norm())
939 distances.emplace_back(distance, v, v2);
940 };
941
942 std::pair<Vec3d, Vec3d> e1 = f1.
get_edge();
943 std::pair<Vec3d, Vec3d> e2 = f2.
get_edge();
944
945 distances.emplace_back((e2.first - e1.first).norm(), e1.first, e2.first);
946 distances.emplace_back((e2.second - e1.first).norm(), e1.first, e2.second);
947 distances.emplace_back((e2.first - e1.second).norm(), e1.second, e2.first);
948 distances.emplace_back((e2.second - e1.second).norm(), e1.second, e2.second);
949 add_point_edge_distance(e1.first, e2);
950 add_point_edge_distance(e1.second, e2);
951 add_point_edge_distance(e2.first, e1);
952 add_point_edge_distance(e2.second, e1);
953 auto it = std::min_element(distances.begin(), distances.end(),
955 return item1.dist < item2.dist;
956 });
958
962 const std::pair<Vec3d, Vec3d> e = f1.
get_edge();
964 const Vec3d e1e2 = (e.second - e.first);
965 const Vec3d e1e2_unit = e1e2.normalized();
966
967 std::vector<DistAndPoints> distances;
970
971 const Eigen::Hyperplane<double, 3> plane(e1e2_unit,
center);
972 const Eigen::ParametrizedLine<double, 3> line = Eigen::ParametrizedLine<double, 3>::Through(e.first, e.second);
973 const Vec3d inter = line.intersectionPoint(plane);
974 const Vec3d e1inter = inter - e.first;
975 if (e1inter.dot(e1e2) >= 0.0 && e1inter.norm() < e1e2.norm()) distances.emplace_back(*
get_measurement(
SurfaceFeature(inter), f2).distance_strict);
976
977 auto it = std::min_element(distances.begin(), distances.end(), [](
const DistAndPoints &item1,
const DistAndPoints &item2) { return item1.dist < item2.dist; });
978 if (deal_circle_result == false) {
980 }
981 else{
982 const double dist = (it->from -
center).norm();
984 }
987 const auto [from, to] = f1.
get_edge();
989
990 const Vec3d edge_unit = (to - from).normalized();
992 std::vector<DistAndPoints> distances;
993 const Eigen::Hyperplane<double, 3> plane(
normal, origin);
994 distances.push_back(
DistAndPoints{ plane.absDistance(from), from, plane.projection(from) });
995 distances.push_back(
DistAndPoints{ plane.absDistance(to), to, plane.projection(to) });
996 auto it = std::min_element(distances.begin(), distances.end(),
998 return item1.dist < item2.dist;
999 });
1001 }
1002 else {
1004 std::vector<DistAndPoints> distances;
1008 if (!m.distance_infinite.has_value()) {
1009 distances.clear();
1010 break;
1011 }
1012 else
1013 distances.push_back(*m.distance_infinite);
1014 }
1015 }
1016 if (!distances.empty()) {
1017 auto it = std::min_element(distances.begin(), distances.end(),
1019 return item1.dist < item2.dist;
1020 });
1022 }
1023 }
1025 }
1033
1034
1035
1036
1037
1038
1039 struct ClosestInfo
1040 {
1041 double sqrDistance{ 0.0 };
1042 Vec3d circle0Closest{ Vec3d::Zero() };
1043 Vec3d circle1Closest{ Vec3d::Zero() };
1044
1045 inline bool operator < (
const ClosestInfo& other)
const {
return sqrDistance < other.sqrDistance; }
1046 };
1047 std::array<ClosestInfo, 16> candidates{};
1048
1049 const double zero = 0.0;
1050
1051 const Vec3d D = c1 - c0;
1052
1054
1055 const double one = 1.0;
1056 const double two = 2.0;
1057 const double r0sqr =
sqr(r0);
1058 const double r1sqr =
sqr(r1);
1059
1060
1062 const Vec3d U1 = basis[0];
1063 const Vec3d V1 = basis[1];
1064
1065
1066 const Vec3d N0xD = n0.cross(D);
1067 const Vec3d N0xU1 = n0.cross(U1);
1068 const Vec3d N0xV1 = n0.cross(V1);
1069 const double a0 = r1 * D.dot(U1);
1070 const double a1 = r1 * D.dot(V1);
1071 const double a2 = N0xD.dot(N0xD);
1072 const double a3 = r1 * N0xD.dot(N0xU1);
1073 const double a4 = r1 * N0xD.dot(N0xV1);
1074 const double a5 = r1sqr * N0xU1.dot(N0xU1);
1075 const double a6 = r1sqr * N0xU1.dot(N0xV1);
1076 const double a7 = r1sqr * N0xV1.dot(N0xV1);
1088 Polynomial1 p6 = p0 * tmp1 + tmp0 * p1 * tmp2 - r0sqr * tmp3;
1089 Polynomial1 p7 = p0 * tmp2 + p1 * tmp1 - r0sqr * tmp4;
1090
1091
1092
1093
1094
1095
1096
1097 const uint32_t maxIterations = 128;
1098 int32_t degree = 0;
1099 size_t numRoots = 0;
1100 std::array<double, 8> roots{};
1101 std::set<double> uniqueRoots{};
1102 size_t numPairs = 0;
1103 std::array<std::pair<double, double>, 16> pairs{};
1104 double temp = zero;
1105 double sn = zero;
1106
1107 if (p7.GetDegree() > 0 || p7[0] != zero) {
1108
1110 degree = static_cast<int32_t>(phi.GetDegree());
1111 assert(degree > 0);
1113 for (size_t i = 0; i < numRoots; ++i) {
1114 uniqueRoots.insert(roots[i]);
1115 }
1116
1117 for (auto const& cs : uniqueRoots) {
1118 if (std::fabs(cs) <= one) {
1119 temp = p7(cs);
1120 if (temp != zero) {
1121 sn = -p6(cs) / temp;
1122 pairs[numPairs++] = std::make_pair(cs, sn);
1123 }
1124 else {
1125 temp = std::max(one -
sqr(cs), zero);
1126 sn = std::sqrt(temp);
1127 pairs[numPairs++] = std::make_pair(cs, sn);
1128 if (sn != zero)
1129 pairs[numPairs++] = std::make_pair(cs, -sn);
1130 }
1131 }
1132 }
1133 }
1134 else {
1135
1136 degree = static_cast<int32_t>(p6.GetDegree());
1137 assert(degree > 0);
1139 for (size_t i = 0; i < numRoots; ++i) {
1140 uniqueRoots.insert(roots[i]);
1141 }
1142
1143 for (auto const& cs : uniqueRoots) {
1144 if (std::fabs(cs) <= one) {
1145 temp = std::max(one -
sqr(cs), zero);
1146 sn = std::sqrt(temp);
1147 pairs[numPairs++] = std::make_pair(cs, sn);
1148 if (sn != zero)
1149 pairs[numPairs++] = std::make_pair(cs, -sn);
1150 }
1151 }
1152 }
1153
1154 for (size_t i = 0; i < numPairs; ++i) {
1155 ClosestInfo& info = candidates[i];
1156 Vec3d delta = D + r1 * (pairs[i].first * U1 + pairs[i].second * V1);
1157 info.circle1Closest = c0 + delta;
1158 const double N0dDelta = n0.dot(delta);
1159 const double lenN0xDelta = n0.cross(delta).norm();
1160 if (lenN0xDelta > 0.0) {
1161 const double diff = lenN0xDelta - r0;
1162 info.sqrDistance =
sqr(N0dDelta) +
sqr(
diff);
1163 delta -= N0dDelta * n0;
1164 delta.normalize();
1165 info.circle0Closest = c0 + r0 * delta;
1166 }
1167 else {
1171 info.circle0Closest = c0 + r0U0;
1172 }
1173 }
1174
1175 std::sort(candidates.begin(), candidates.begin() + numPairs);
1176 }
1177 else {
1178 ClosestInfo& info = candidates[0];
1179
1180 const double N0dD = n0.dot(D);
1181 const Vec3d normProj = N0dD * n0;
1182 const Vec3d compProj = D - normProj;
1184 const double d = U.norm();
1185 U.normalize();
1186
1187
1188
1189
1190
1191 const double dmr1 = d - r1;
1193 if (dmr1 >= r0) {
1194
1195
1196
1198 info.circle0Closest = c0 + r0 * U;
1199 info.circle1Closest = c1 - r1 * U;
1200 }
1201 else {
1202
1203
1204 const double dpr1 = d + r1;
1205 if (dpr1 <= r0) {
1206
1208 if (d > 0.0) {
1209 info.circle0Closest = c0 + r0 * U;
1210 info.circle1Closest = c1 + r1 * U;
1211 }
1212 else {
1213
1214
1215
1217 info.circle0Closest = c0 + r0 * U;
1218 info.circle1Closest = c1 + r1 * U;
1219 }
1220 }
1221 else if (dmr1 <= -r0) {
1222
1224 if (d > 0.0) {
1225 info.circle0Closest = c0 - r0 * U;
1226 info.circle1Closest = c1 - r1 * U;
1227 }
1228 else {
1229
1230
1231
1233 info.circle0Closest = c0 + r0 * U;
1234 info.circle1Closest = c1 + r1 * U;
1235 }
1236 }
1237 else {
1239 info.circle0Closest = c0;
1240 info.circle1Closest = c1;
1241 }
1242 }
1243
1245 }
1246 if (deal_circle_result == false) {
1248 DistAndPoints{std::sqrt(candidates[0].sqrDistance), candidates[0].circle0Closest, candidates[0].circle1Closest});
1249 } else {
1250 const double dist = (c0 - c1).norm();
1252 }
1253
1257 const auto [idx2, normal2, origin2] = f2.
get_plane();
1258
1259 const bool coplanar =
are_parallel(normal1, normal2) && Eigen::Hyperplane<double, 3>(normal1,
center).absDistance(origin2) <
EPSILON;
1260 if (!coplanar) {
1262 std::vector<DistAndPoints> distances;
1266 if (!m.distance_infinite.has_value()) {
1267 distances.clear();
1268 break;
1269 }
1270 else
1271 distances.push_back(*m.distance_infinite);
1272 }
1273 }
1274 if (!distances.empty()) {
1275 auto it = std::min_element(distances.begin(), distances.end(),
1277 return item1.dist < item2.dist;
1278 });
1280 }
1281 else {
1282 const Eigen::Hyperplane<double, 3> plane(normal2, origin2);
1284 }
1285 }
1286 else {
1288 }
1289 }
1294 const auto [idx1, normal1, pt1] = f1.
get_plane();
1295 const auto [idx2, normal2, pt2] = f2.
get_plane();
1296
1298
1299 const Eigen::Hyperplane<double, 3> plane(normal2, pt2);
1301 }
1302 else
1304 }
1305
1306 if (swap) {
1308 auto back = dp.to;
1309 dp.to = dp.from;
1310 dp.from = back;
1311 };
1314 }
1317 }
1318 }
1319 return result;
1320}
static bool operator<(wxColour const &l, wxColour const &r)
Definition StateColor.cpp:11
Definition MeasureUtils.hpp:21
static int32_t Find(int32_t degree, const double *c, uint32_t maxIterations, double *roots)
Definition MeasureUtils.hpp:195
std::tuple< Vec3d, double, Vec3d > get_circle() const
Definition Measure.hpp:62
std::shared_ptr< std::vector< SurfaceFeature > > world_plane_features
Definition Measure.hpp:96
Vec3d get_point() const
Definition Measure.hpp:57
T dist(const boost::polygon::point_data< T > &p1, const boost::polygon::point_data< T > &p2)
Definition Geometry.cpp:298
static AngleAndEdges angle_plane_plane(const std::tuple< int, Vec3d, Vec3d > &p1, const std::tuple< int, Vec3d, Vec3d > &p2)
Definition Measure.cpp:818
static AngleAndEdges angle_edge_plane(const std::pair< Vec3d, Vec3d > &e, const std::tuple< int, Vec3d, Vec3d > &p)
Definition Measure.cpp:767
MeasurementResult get_measurement(const SurfaceFeature &a, const SurfaceFeature &b, bool deal_circle_result)
Definition Measure.cpp:856
Vec3d get_orthogonal(const Vec3d &v, bool unitLength)
Definition MeasureUtils.hpp:362
@ Undef
Definition Measure.hpp:24
float distance(SpiralVase::SpiralPoint a, SpiralVase::SpiralPoint b)
Definition SpiralVase.cpp:19
constexpr T sqr(T x)
Definition libslic3r.h:301
Slic3r::Polygons diff(const Slic3r::Polygon &subject, const Slic3r::Polygon &clip, ApplySafetyOffset do_safety_offset)
Definition ClipperUtils.cpp:731
Definition Measure.hpp:145
Definition Measure.hpp:165
std::optional< DistAndPoints > distance_infinite
Definition Measure.hpp:167
std::optional< AngleAndEdges > angle
Definition Measure.hpp:166
std::optional< Vec3d > distance_xyz
Definition Measure.hpp:169
std::optional< DistAndPoints > distance_strict
Definition Measure.hpp:168