750 lines
26 KiB
C
750 lines
26 KiB
C
#include <stdint.h>
|
|
#include <math.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#ifdef _WIN32
|
|
#define EXPORT __declspec(dllexport)
|
|
#else
|
|
#define EXPORT __attribute__((visibility("default")))
|
|
#endif
|
|
|
|
EXPORT int find_sharpest_seam_c(const double* pts, int M, int stride, double scale) {
|
|
if (M < 3) return 0;
|
|
|
|
// Find Y bounding range
|
|
double min_y = pts[1];
|
|
double max_y = pts[1];
|
|
for (int i = 1; i < M; ++i) {
|
|
double y = pts[i * stride + 1];
|
|
if (y < min_y) min_y = y;
|
|
if (y > max_y) max_y = y;
|
|
}
|
|
double y_range = max_y - min_y;
|
|
double corner_bonus = (y_range > 0.0) ? (0.1 * y_range < 2.0 ? 0.1 * y_range : 2.0) : 2.0;
|
|
|
|
int best_idx = 0;
|
|
double best_score = -1e30;
|
|
double best_x = 1e30;
|
|
|
|
for (int i = 0; i < M; ++i) {
|
|
int prev = (i == 0) ? M - 1 : i - 1;
|
|
int next = (i == M - 1) ? 0 : i + 1;
|
|
|
|
double xi = pts[i * stride + 0];
|
|
double yi = pts[i * stride + 1];
|
|
|
|
double vin_x = xi - pts[prev * stride + 0];
|
|
double vin_y = yi - pts[prev * stride + 1];
|
|
double vout_x = pts[next * stride + 0] - xi;
|
|
double vout_y = pts[next * stride + 1] - yi;
|
|
|
|
double l_in = sqrt(vin_x * vin_x + vin_y * vin_y);
|
|
double l_out = sqrt(vout_x * vout_x + vout_y * vout_y);
|
|
|
|
double sharpness = 0.0;
|
|
if (l_in > 1e-7 && l_out > 1e-7) {
|
|
double dot = vin_x * vout_x + vin_y * vout_y;
|
|
double cos_ang = dot / (l_in * l_out);
|
|
if (cos_ang > 1.0) cos_ang = 1.0;
|
|
if (cos_ang < -1.0) cos_ang = -1.0;
|
|
sharpness = 1.0 - cos_ang; // 0 for straight line, 1 for 90 deg corner, 2 for 180 deg turnaround
|
|
}
|
|
|
|
double score = yi + sharpness * corner_bonus;
|
|
|
|
// Highest score wins (back of the object). Tie-break: smallest X for deterministic consistency across layers
|
|
if (score > best_score + 1e-6) {
|
|
best_score = score;
|
|
best_x = xi;
|
|
best_idx = i;
|
|
} else if (fabs(score - best_score) <= 1e-6) {
|
|
if (xi < best_x) {
|
|
best_x = xi;
|
|
best_idx = i;
|
|
}
|
|
}
|
|
}
|
|
|
|
return best_idx;
|
|
}
|
|
|
|
EXPORT void align_path_seam_sharpest_c(const double* in_pts, int M, int stride, double* out_pts, double scale) {
|
|
int best_idx = find_sharpest_seam_c(in_pts, M, stride, scale);
|
|
if (best_idx == 0) {
|
|
memcpy(out_pts, in_pts, (M + 1) * stride * sizeof(double));
|
|
return;
|
|
}
|
|
int split = M - best_idx;
|
|
memcpy(out_pts, in_pts + (best_idx * stride), split * stride * sizeof(double));
|
|
memcpy(out_pts + (split * stride), in_pts, (best_idx + 1) * stride * sizeof(double));
|
|
}
|
|
|
|
// Fast 64-bit hash for 3 float32 values (12 bytes)
|
|
static inline uint64_t hash_v3(const float* v) {
|
|
const uint32_t* u = (const uint32_t*)v;
|
|
uint64_t h = (uint64_t)u[0] * 0x9e3779b97f4a7c15ULL;
|
|
h ^= (uint64_t)u[1] * 0xbf58476d1ce4e5b9ULL;
|
|
h = (h ^ (h >> 30)) * 0x94d049bb133111ebULL;
|
|
h ^= (uint64_t)u[2] * 0x4bfb2f35a09a447fULL;
|
|
h = (h ^ (h >> 27)) * 0x94d049bb133111ebULL;
|
|
return h ^ (h >> 31);
|
|
}
|
|
|
|
static inline int v3_eq(const float* a, const float* b) {
|
|
const uint32_t* ua = (const uint32_t*)a;
|
|
const uint32_t* ub = (const uint32_t*)b;
|
|
return (ua[0] == ub[0]) && (ua[1] == ub[1]) && (ua[2] == ub[2]);
|
|
}
|
|
|
|
EXPORT int weld_vertices_c(
|
|
const float* in_verts,
|
|
int num_in_verts,
|
|
float* out_unique_verts,
|
|
uint32_t* out_inv_indices
|
|
) {
|
|
if (num_in_verts <= 0) return 0;
|
|
|
|
uint32_t table_size = 1024;
|
|
while (table_size < (uint32_t)num_in_verts * 2) {
|
|
table_size <<= 1;
|
|
}
|
|
uint32_t mask = table_size - 1;
|
|
|
|
uint32_t* table = (uint32_t*)calloc(table_size, sizeof(uint32_t));
|
|
if (!table) return -1;
|
|
|
|
int unique_count = 0;
|
|
|
|
for (int i = 0; i < num_in_verts; ++i) {
|
|
const float* v = in_verts + (i * 3);
|
|
uint64_t h = hash_v3(v);
|
|
uint32_t slot = (uint32_t)(h & mask);
|
|
|
|
while (1) {
|
|
uint32_t entry = table[slot];
|
|
if (entry == 0) {
|
|
int uid = unique_count++;
|
|
out_unique_verts[uid * 3 + 0] = v[0];
|
|
out_unique_verts[uid * 3 + 1] = v[1];
|
|
out_unique_verts[uid * 3 + 2] = v[2];
|
|
table[slot] = (uint32_t)(uid + 1);
|
|
out_inv_indices[i] = (uint32_t)uid;
|
|
break;
|
|
}
|
|
uint32_t uid = entry - 1;
|
|
const float* existing_v = out_unique_verts + (uid * 3);
|
|
if (v3_eq(v, existing_v)) {
|
|
out_inv_indices[i] = (uint32_t)uid;
|
|
break;
|
|
}
|
|
slot = (slot + 1) & mask;
|
|
}
|
|
}
|
|
|
|
free(table);
|
|
return unique_count;
|
|
}
|
|
|
|
EXPORT double polygon_signed_area_2d_c(const double* pts, int N, int stride) {
|
|
if (N < 3) return 0.0;
|
|
double area = pts[(N - 1) * stride + 0] * pts[1] - pts[0] * pts[(N - 1) * stride + 1];
|
|
for (int i = 0; i < N - 1; ++i) {
|
|
area += pts[i * stride + 0] * pts[(i + 1) * stride + 1] - pts[(i + 1) * stride + 0] * pts[i * stride + 1];
|
|
}
|
|
return area * 0.5;
|
|
}
|
|
|
|
static inline int intersect_segments_2d(
|
|
double ax, double ay, double bx, double by,
|
|
double cx, double cy, double dx, double dy,
|
|
double* out_t, double* out_u
|
|
) {
|
|
double denom = (bx - ax) * (dy - cy) - (by - ay) * (dx - cx);
|
|
if (fabs(denom) < 1e-12) return 0;
|
|
double t = ((cx - ax) * (dy - cy) - (cy - ay) * (dx - cx)) / denom;
|
|
double u = ((cx - ax) * (by - ay) - (cy - ay) * (bx - ax)) / denom;
|
|
if (t >= -1e-6 && t <= 1.0 + 1e-6 && u >= -1e-6 && u <= 1.0 + 1e-6) {
|
|
if (t < 0.0) t = 0.0;
|
|
if (t > 1.0) t = 1.0;
|
|
if (u < 0.0) u = 0.0;
|
|
if (u > 1.0) u = 1.0;
|
|
*out_t = t;
|
|
*out_u = u;
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static inline int intersect_segment_triangle(
|
|
const double* p0, const double* p1,
|
|
const double* v0, const double* v1, const double* v2,
|
|
double* out_t, double* out_pt
|
|
) {
|
|
double e1x = v1[0] - v0[0], e1y = v1[1] - v0[1], e1z = v1[2] - v0[2];
|
|
double e2x = v2[0] - v0[0], e2y = v2[1] - v0[1], e2z = v2[2] - v0[2];
|
|
double nx = e1y * e2z - e1z * e2y;
|
|
double ny = e1z * e2x - e1x * e2z;
|
|
double nz = e1x * e2y - e1y * e2x;
|
|
double n_len_sq = nx * nx + ny * ny + nz * nz;
|
|
if (n_len_sq < 1e-14) return 0;
|
|
|
|
double d0 = (p0[0] - v0[0]) * nx + (p0[1] - v0[1]) * ny + (p0[2] - v0[2]) * nz;
|
|
double d1 = (p1[0] - v0[0]) * nx + (p1[1] - v0[1]) * ny + (p1[2] - v0[2]) * nz;
|
|
|
|
if ((d0 > 1e-6 && d1 > 1e-6) || (d0 < -1e-6 && d1 < -1e-6)) return 0;
|
|
|
|
double denom = d0 - d1;
|
|
if (fabs(denom) > 1e-12) {
|
|
double t = d0 / denom;
|
|
if (t < -1e-6 || t > 1.0 + 1e-6) return 0;
|
|
if (t < 0.0) t = 0.0;
|
|
if (t > 1.0) t = 1.0;
|
|
|
|
double ix = p0[0] + t * (p1[0] - p0[0]);
|
|
double iy = p0[1] + t * (p1[1] - p0[1]);
|
|
double iz = p0[2] + t * (p1[2] - p0[2]);
|
|
|
|
double c0x = (v1[1] - v0[1]) * (iz - v0[2]) - (v1[2] - v0[2]) * (iy - v0[1]);
|
|
double c0y = (v1[2] - v0[2]) * (ix - v0[0]) - (v1[0] - v0[0]) * (iz - v0[2]);
|
|
double c0z = (v1[0] - v0[0]) * (iy - v0[1]) - (v1[1] - v0[1]) * (ix - v0[0]);
|
|
double dot0 = c0x * nx + c0y * ny + c0z * nz;
|
|
|
|
double c1x = (v2[1] - v1[1]) * (iz - v1[2]) - (v2[2] - v1[2]) * (iy - v1[1]);
|
|
double c1y = (v2[2] - v1[2]) * (ix - v1[0]) - (v2[0] - v1[0]) * (iz - v1[2]);
|
|
double c1z = (v2[0] - v1[0]) * (iy - v1[1]) - (v2[1] - v1[1]) * (ix - v1[0]);
|
|
double dot1 = c1x * nx + c1y * ny + c1z * nz;
|
|
|
|
double c2x = (v0[1] - v2[1]) * (iz - v2[2]) - (v0[2] - v2[2]) * (iy - v2[1]);
|
|
double c2y = (v0[2] - v2[2]) * (ix - v2[0]) - (v0[0] - v2[0]) * (iz - v2[2]);
|
|
double c2z = (v0[0] - v2[0]) * (iy - v2[1]) - (v0[1] - v2[1]) * (ix - v2[0]);
|
|
double dot2 = c2x * nx + c2y * ny + c2z * nz;
|
|
|
|
double tol = -1e-6 * n_len_sq;
|
|
if (dot0 >= tol && dot1 >= tol && dot2 >= tol) {
|
|
*out_t = t;
|
|
out_pt[0] = ix;
|
|
out_pt[1] = iy;
|
|
out_pt[2] = iz;
|
|
return 1;
|
|
}
|
|
} else if (fabs(d0) < 1e-6) {
|
|
// Coplanar segment - test 3 edges in 2D projection along dominant normal axis
|
|
int ax0 = 0, ax1 = 1;
|
|
if (fabs(nx) >= fabs(ny) && fabs(nx) >= fabs(nz)) {
|
|
ax0 = 1; ax1 = 2;
|
|
} else if (fabs(ny) >= fabs(nx) && fabs(ny) >= fabs(nz)) {
|
|
ax0 = 0; ax1 = 2;
|
|
}
|
|
|
|
double best_t = 1e30;
|
|
int found = 0;
|
|
double t_cand, u_cand;
|
|
|
|
const double* verts[3] = {v0, v1, v2};
|
|
for (int e = 0; e < 3; ++e) {
|
|
const double* va = verts[e];
|
|
const double* vb = verts[(e + 1) % 3];
|
|
if (intersect_segments_2d(p0[ax0], p0[ax1], p1[ax0], p1[ax1],
|
|
va[ax0], va[ax1], vb[ax0], vb[ax1],
|
|
&t_cand, &u_cand)) {
|
|
if (t_cand < best_t) {
|
|
best_t = t_cand;
|
|
found = 1;
|
|
}
|
|
}
|
|
}
|
|
if (found) {
|
|
*out_t = best_t;
|
|
out_pt[0] = p0[0] + best_t * (p1[0] - p0[0]);
|
|
out_pt[1] = p0[1] + best_t * (p1[1] - p0[1]);
|
|
out_pt[2] = p0[2] + best_t * (p1[2] - p0[2]);
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
EXPORT int slice_triangles_at_z_c(
|
|
const double* tri_verts,
|
|
int num_tris,
|
|
double pz,
|
|
double* out_segs
|
|
) {
|
|
int num_segs = 0;
|
|
for (int j = 0; j < num_tris; ++j) {
|
|
const double* v[3] = {
|
|
tri_verts + (j * 9 + 0),
|
|
tri_verts + (j * 9 + 3),
|
|
tri_verts + (j * 9 + 6)
|
|
};
|
|
|
|
double tz_min = v[0][2] < v[1][2] ? (v[0][2] < v[2][2] ? v[0][2] : v[2][2]) : (v[1][2] < v[2][2] ? v[1][2] : v[2][2]);
|
|
double tz_max = v[0][2] > v[1][2] ? (v[0][2] > v[2][2] ? v[0][2] : v[2][2]) : (v[1][2] > v[2][2] ? v[1][2] : v[2][2]);
|
|
if (tz_max < pz - 1e-4 || tz_min > pz + 1e-4) continue;
|
|
|
|
double pts2d[3][2];
|
|
int n_pts2d = 0;
|
|
|
|
for (int e = 0; e < 3; ++e) {
|
|
const double* va = v[e];
|
|
const double* vb = v[(e + 1) % 3];
|
|
double dza = va[2] - pz;
|
|
double dzb = vb[2] - pz;
|
|
|
|
if ((dza <= 0.0 && dzb >= 0.0) || (dza >= 0.0 && dzb <= 0.0)) {
|
|
double denom = vb[2] - va[2];
|
|
double u = (fabs(denom) > 1e-12) ? (pz - va[2]) / denom : 0.0;
|
|
if (u < 0.0) u = 0.0;
|
|
if (u > 1.0) u = 1.0;
|
|
double qx = va[0] + u * (vb[0] - va[0]);
|
|
double qy = va[1] + u * (vb[1] - va[1]);
|
|
|
|
if (n_pts2d == 0 || (fabs(qx - pts2d[0][0]) > 1e-6 || fabs(qy - pts2d[0][1]) > 1e-6)) {
|
|
if (n_pts2d < 2) {
|
|
pts2d[n_pts2d][0] = qx;
|
|
pts2d[n_pts2d][1] = qy;
|
|
n_pts2d++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (n_pts2d == 2) {
|
|
out_segs[num_segs * 4 + 0] = pts2d[0][0];
|
|
out_segs[num_segs * 4 + 1] = pts2d[0][1];
|
|
out_segs[num_segs * 4 + 2] = pts2d[1][0];
|
|
out_segs[num_segs * 4 + 3] = pts2d[1][1];
|
|
num_segs++;
|
|
}
|
|
}
|
|
return num_segs;
|
|
}
|
|
|
|
EXPORT int intersect_path_with_2d_segments_c(
|
|
const double* path_pts,
|
|
int num_pts,
|
|
int stride,
|
|
const double* mesh_segs,
|
|
int num_segs,
|
|
double pz,
|
|
int mode,
|
|
const double* ref_point,
|
|
int* out_seg_idx,
|
|
double* out_t,
|
|
double* out_pt
|
|
) {
|
|
if (num_pts < 4 || num_segs <= 0) return 0;
|
|
int M = num_pts - 1;
|
|
|
|
double p_min_x = path_pts[0], p_max_x = path_pts[0];
|
|
double p_min_y = path_pts[1], p_max_y = path_pts[1];
|
|
for (int i = 1; i < num_pts; ++i) {
|
|
double px = path_pts[i * stride + 0];
|
|
double py = path_pts[i * stride + 1];
|
|
if (px < p_min_x) p_min_x = px;
|
|
if (px > p_max_x) p_max_x = px;
|
|
if (py < p_min_y) p_min_y = py;
|
|
if (py > p_max_y) p_max_y = py;
|
|
}
|
|
p_min_x -= 1e-4; p_max_x += 1e-4;
|
|
p_min_y -= 1e-4; p_max_y += 1e-4;
|
|
|
|
int found = 0;
|
|
int best_seg = -1;
|
|
double best_t = 0.0;
|
|
double best_pt[3] = {0.0, 0.0, 0.0};
|
|
double best_dist_sq = 1e30;
|
|
|
|
double rx = ref_point ? ref_point[0] : path_pts[0];
|
|
double ry = ref_point ? ref_point[1] : path_pts[1];
|
|
double rz = ref_point ? ref_point[2] : pz;
|
|
|
|
for (int i = 0; i < M; ++i) {
|
|
const double* p0 = path_pts + (i * stride);
|
|
const double* p1 = path_pts + ((i + 1) * stride);
|
|
|
|
double s_min_x = (p0[0] < p1[0] ? p0[0] : p1[0]) - 1e-4;
|
|
double s_max_x = (p0[0] > p1[0] ? p0[0] : p1[0]) + 1e-4;
|
|
double s_min_y = (p0[1] < p1[1] ? p0[1] : p1[1]) - 1e-4;
|
|
double s_max_y = (p0[1] > p1[1] ? p0[1] : p1[1]) + 1e-4;
|
|
|
|
for (int s = 0; s < num_segs; ++s) {
|
|
double q0x = mesh_segs[s * 4 + 0];
|
|
double q0y = mesh_segs[s * 4 + 1];
|
|
double q1x = mesh_segs[s * 4 + 2];
|
|
double q1y = mesh_segs[s * 4 + 3];
|
|
|
|
double q_min_x = (q0x < q1x ? q0x : q1x) - 1e-4;
|
|
double q_max_x = (q0x > q1x ? q0x : q1x) + 1e-4;
|
|
if (q_max_x < s_min_x || q_min_x > s_max_x) continue;
|
|
|
|
double q_min_y = (q0y < q1y ? q0y : q1y) - 1e-4;
|
|
double q_max_y = (q0y > q1y ? q0y : q1y) + 1e-4;
|
|
if (q_max_y < s_min_y || q_min_y > s_max_y) continue;
|
|
|
|
double t_cand, u_cand;
|
|
if (intersect_segments_2d(p0[0], p0[1], p1[0], p1[1], q0x, q0y, q1x, q1y, &t_cand, &u_cand)) {
|
|
double ix = p0[0] + t_cand * (p1[0] - p0[0]);
|
|
double iy = p0[1] + t_cand * (p1[1] - p0[1]);
|
|
double iz = pz;
|
|
|
|
if (mode == 1) { // FIRST
|
|
*out_seg_idx = i;
|
|
*out_t = t_cand;
|
|
out_pt[0] = ix;
|
|
out_pt[1] = iy;
|
|
out_pt[2] = iz;
|
|
return 1;
|
|
}
|
|
|
|
double dx = ix - rx;
|
|
double dy = iy - ry;
|
|
double dz = iz - rz;
|
|
double d_sq = dx * dx + dy * dy + dz * dz;
|
|
|
|
if (!found || d_sq < best_dist_sq) {
|
|
found = 1;
|
|
best_dist_sq = d_sq;
|
|
best_seg = i;
|
|
best_t = t_cand;
|
|
best_pt[0] = ix;
|
|
best_pt[1] = iy;
|
|
best_pt[2] = iz;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (found) {
|
|
*out_seg_idx = best_seg;
|
|
*out_t = best_t;
|
|
out_pt[0] = best_pt[0];
|
|
out_pt[1] = best_pt[1];
|
|
out_pt[2] = best_pt[2];
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
EXPORT int find_path_mesh_intersection_c(
|
|
const double* path_pts,
|
|
int num_pts,
|
|
int stride,
|
|
const double* tri_verts,
|
|
int num_tris,
|
|
int mode,
|
|
const double* ref_point,
|
|
int* out_seg_idx,
|
|
double* out_t,
|
|
double* out_pt
|
|
) {
|
|
if (num_pts < 4 || num_tris <= 0) return 0;
|
|
int M = num_pts - 1;
|
|
|
|
// 1. Path bounding box
|
|
double p_min_x = path_pts[0], p_max_x = path_pts[0];
|
|
double p_min_y = path_pts[1], p_max_y = path_pts[1];
|
|
double p_min_z = path_pts[2], p_max_z = path_pts[2];
|
|
for (int i = 1; i < num_pts; ++i) {
|
|
double px = path_pts[i * stride + 0];
|
|
double py = path_pts[i * stride + 1];
|
|
double pz = path_pts[i * stride + 2];
|
|
if (px < p_min_x) p_min_x = px;
|
|
if (px > p_max_x) p_max_x = px;
|
|
if (py < p_min_y) p_min_y = py;
|
|
if (py > p_max_y) p_max_y = py;
|
|
if (pz < p_min_z) p_min_z = pz;
|
|
if (pz > p_max_z) p_max_z = pz;
|
|
}
|
|
double p_eps = 1e-4;
|
|
p_min_x -= p_eps; p_max_x += p_eps;
|
|
p_min_y -= p_eps; p_max_y += p_eps;
|
|
p_min_z -= p_eps; p_max_z += p_eps;
|
|
|
|
// 2. Pre-filter triangles that overlap the path bounding box
|
|
int* active_tris = (int*)malloc(num_tris * sizeof(int));
|
|
if (!active_tris) return 0;
|
|
int num_active = 0;
|
|
|
|
for (int j = 0; j < num_tris; ++j) {
|
|
const double* v0 = tri_verts + (j * 9 + 0);
|
|
const double* v1 = tri_verts + (j * 9 + 3);
|
|
const double* v2 = tri_verts + (j * 9 + 6);
|
|
|
|
double tz_min = v0[2] < v1[2] ? (v0[2] < v2[2] ? v0[2] : v2[2]) : (v1[2] < v2[2] ? v1[2] : v2[2]);
|
|
double tz_max = v0[2] > v1[2] ? (v0[2] > v2[2] ? v0[2] : v2[2]) : (v1[2] > v2[2] ? v1[2] : v2[2]);
|
|
if (tz_max < p_min_z || tz_min > p_max_z) continue;
|
|
|
|
double tx_min = v0[0] < v1[0] ? (v0[0] < v2[0] ? v0[0] : v2[0]) : (v1[0] < v2[0] ? v1[0] : v2[0]);
|
|
double tx_max = v0[0] > v1[0] ? (v0[0] > v2[0] ? v0[0] : v2[0]) : (v1[0] > v2[0] ? v1[0] : v2[0]);
|
|
if (tx_max < p_min_x || tx_min > p_max_x) continue;
|
|
|
|
double ty_min = v0[1] < v1[1] ? (v0[1] < v2[1] ? v0[1] : v2[1]) : (v1[1] < v2[1] ? v1[1] : v2[1]);
|
|
double ty_max = v0[1] > v1[1] ? (v0[1] > v2[1] ? v0[1] : v2[1]) : (v1[1] > v2[1] ? v1[1] : v2[1]);
|
|
if (ty_max < p_min_y || ty_min > p_max_y) continue;
|
|
|
|
active_tris[num_active++] = j;
|
|
}
|
|
|
|
if (num_active == 0) {
|
|
free(active_tris);
|
|
return 0;
|
|
}
|
|
|
|
int found = 0;
|
|
int best_seg = -1;
|
|
double best_t = 0.0;
|
|
double best_pt[3] = {0.0, 0.0, 0.0};
|
|
double best_dist_sq = 1e30;
|
|
|
|
double rx = ref_point ? ref_point[0] : path_pts[0];
|
|
double ry = ref_point ? ref_point[1] : path_pts[1];
|
|
double rz = ref_point ? ref_point[2] : path_pts[2];
|
|
|
|
int is_planar = (p_max_z - p_min_z) < 1e-3;
|
|
|
|
if (is_planar) {
|
|
double pz = path_pts[2];
|
|
double* mesh_segs = (double*)malloc(num_active * 4 * sizeof(double));
|
|
int num_segs = 0;
|
|
|
|
if (mesh_segs) {
|
|
for (int a = 0; a < num_active; ++a) {
|
|
int j = active_tris[a];
|
|
const double* v[3] = {
|
|
tri_verts + (j * 9 + 0),
|
|
tri_verts + (j * 9 + 3),
|
|
tri_verts + (j * 9 + 6)
|
|
};
|
|
|
|
double pts2d[3][2];
|
|
int n_pts2d = 0;
|
|
|
|
for (int e = 0; e < 3; ++e) {
|
|
const double* va = v[e];
|
|
const double* vb = v[(e + 1) % 3];
|
|
double dza = va[2] - pz;
|
|
double dzb = vb[2] - pz;
|
|
|
|
if ((dza <= 0.0 && dzb >= 0.0) || (dza >= 0.0 && dzb <= 0.0)) {
|
|
double denom = vb[2] - va[2];
|
|
double u = (fabs(denom) > 1e-12) ? (pz - va[2]) / denom : 0.0;
|
|
if (u < 0.0) u = 0.0;
|
|
if (u > 1.0) u = 1.0;
|
|
double qx = va[0] + u * (vb[0] - va[0]);
|
|
double qy = va[1] + u * (vb[1] - va[1]);
|
|
|
|
if (n_pts2d == 0 || (fabs(qx - pts2d[0][0]) > 1e-6 || fabs(qy - pts2d[0][1]) > 1e-6)) {
|
|
if (n_pts2d < 2) {
|
|
pts2d[n_pts2d][0] = qx;
|
|
pts2d[n_pts2d][1] = qy;
|
|
n_pts2d++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (n_pts2d == 2) {
|
|
mesh_segs[num_segs * 4 + 0] = pts2d[0][0];
|
|
mesh_segs[num_segs * 4 + 1] = pts2d[0][1];
|
|
mesh_segs[num_segs * 4 + 2] = pts2d[1][0];
|
|
mesh_segs[num_segs * 4 + 3] = pts2d[1][1];
|
|
num_segs++;
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < M; ++i) {
|
|
const double* p0 = path_pts + (i * stride);
|
|
const double* p1 = path_pts + ((i + 1) * stride);
|
|
|
|
double s_min_x = (p0[0] < p1[0] ? p0[0] : p1[0]) - 1e-4;
|
|
double s_max_x = (p0[0] > p1[0] ? p0[0] : p1[0]) + 1e-4;
|
|
double s_min_y = (p0[1] < p1[1] ? p0[1] : p1[1]) - 1e-4;
|
|
double s_max_y = (p0[1] > p1[1] ? p0[1] : p1[1]) + 1e-4;
|
|
|
|
for (int s = 0; s < num_segs; ++s) {
|
|
double q0x = mesh_segs[s * 4 + 0];
|
|
double q0y = mesh_segs[s * 4 + 1];
|
|
double q1x = mesh_segs[s * 4 + 2];
|
|
double q1y = mesh_segs[s * 4 + 3];
|
|
|
|
double q_min_x = (q0x < q1x ? q0x : q1x) - 1e-4;
|
|
double q_max_x = (q0x > q1x ? q0x : q1x) + 1e-4;
|
|
if (q_max_x < s_min_x || q_min_x > s_max_x) continue;
|
|
|
|
double q_min_y = (q0y < q1y ? q0y : q1y) - 1e-4;
|
|
double q_max_y = (q0y > q1y ? q0y : q1y) + 1e-4;
|
|
if (q_max_y < s_min_y || q_min_y > s_max_y) continue;
|
|
|
|
double t_cand, u_cand;
|
|
if (intersect_segments_2d(p0[0], p0[1], p1[0], p1[1], q0x, q0y, q1x, q1y, &t_cand, &u_cand)) {
|
|
double ix = p0[0] + t_cand * (p1[0] - p0[0]);
|
|
double iy = p0[1] + t_cand * (p1[1] - p0[1]);
|
|
double iz = pz;
|
|
|
|
if (mode == 1) { // FIRST
|
|
*out_seg_idx = i;
|
|
*out_t = t_cand;
|
|
out_pt[0] = ix;
|
|
out_pt[1] = iy;
|
|
out_pt[2] = iz;
|
|
free(mesh_segs);
|
|
free(active_tris);
|
|
return 1;
|
|
}
|
|
|
|
double dx = ix - rx;
|
|
double dy = iy - ry;
|
|
double dz = iz - rz;
|
|
double d_sq = dx * dx + dy * dy + dz * dz;
|
|
|
|
if (!found || d_sq < best_dist_sq) {
|
|
found = 1;
|
|
best_dist_sq = d_sq;
|
|
best_seg = i;
|
|
best_t = t_cand;
|
|
best_pt[0] = ix;
|
|
best_pt[1] = iy;
|
|
best_pt[2] = iz;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
free(mesh_segs);
|
|
}
|
|
}
|
|
|
|
if (!found) {
|
|
for (int i = 0; i < M; ++i) {
|
|
const double* p0 = path_pts + (i * stride);
|
|
const double* p1 = path_pts + ((i + 1) * stride);
|
|
|
|
double s_min_x = (p0[0] < p1[0] ? p0[0] : p1[0]) - 1e-4;
|
|
double s_max_x = (p0[0] > p1[0] ? p0[0] : p1[0]) + 1e-4;
|
|
double s_min_y = (p0[1] < p1[1] ? p0[1] : p1[1]) - 1e-4;
|
|
double s_max_y = (p0[1] > p1[1] ? p0[1] : p1[1]) + 1e-4;
|
|
double s_min_z = (p0[2] < p1[2] ? p0[2] : p1[2]) - 1e-4;
|
|
double s_max_z = (p0[2] > p1[2] ? p0[2] : p1[2]) + 1e-4;
|
|
|
|
for (int a = 0; a < num_active; ++a) {
|
|
int j = active_tris[a];
|
|
const double* v0 = tri_verts + (j * 9 + 0);
|
|
const double* v1 = tri_verts + (j * 9 + 3);
|
|
const double* v2 = tri_verts + (j * 9 + 6);
|
|
|
|
double cand_t = 0.0;
|
|
double cand_pt[3] = {0.0, 0.0, 0.0};
|
|
if (intersect_segment_triangle(p0, p1, v0, v1, v2, &cand_t, cand_pt)) {
|
|
if (mode == 1) { // FIRST
|
|
*out_seg_idx = i;
|
|
*out_t = cand_t;
|
|
out_pt[0] = cand_pt[0];
|
|
out_pt[1] = cand_pt[1];
|
|
out_pt[2] = cand_pt[2];
|
|
free(active_tris);
|
|
return 1;
|
|
}
|
|
|
|
double dx = cand_pt[0] - rx;
|
|
double dy = cand_pt[1] - ry;
|
|
double dz = cand_pt[2] - rz;
|
|
double d_sq = dx * dx + dy * dy + dz * dz;
|
|
|
|
if (!found || d_sq < best_dist_sq) {
|
|
found = 1;
|
|
best_dist_sq = d_sq;
|
|
best_seg = i;
|
|
best_t = cand_t;
|
|
best_pt[0] = cand_pt[0];
|
|
best_pt[1] = cand_pt[1];
|
|
best_pt[2] = cand_pt[2];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
free(active_tris);
|
|
|
|
if (found) {
|
|
*out_seg_idx = best_seg;
|
|
*out_t = best_t;
|
|
out_pt[0] = best_pt[0];
|
|
out_pt[1] = best_pt[1];
|
|
out_pt[2] = best_pt[2];
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
EXPORT int reposition_path_seam_c(
|
|
const double* in_pts,
|
|
int num_pts,
|
|
int stride,
|
|
int seg_idx,
|
|
double t,
|
|
const double* intersect_pt,
|
|
double* out_pts
|
|
) {
|
|
if (num_pts < 4) {
|
|
memcpy(out_pts, in_pts, num_pts * stride * sizeof(double));
|
|
return num_pts;
|
|
}
|
|
int M = num_pts - 1;
|
|
|
|
// Check snapping to existing vertices
|
|
if (t <= 1e-6) {
|
|
int k = seg_idx;
|
|
if (k == 0) {
|
|
memcpy(out_pts, in_pts, num_pts * stride * sizeof(double));
|
|
return num_pts;
|
|
}
|
|
int split = M - k;
|
|
memcpy(out_pts, in_pts + (k * stride), split * stride * sizeof(double));
|
|
memcpy(out_pts + (split * stride), in_pts, (k + 1) * stride * sizeof(double));
|
|
return num_pts;
|
|
}
|
|
if (1.0 - t <= 1e-6) {
|
|
int next_k = (seg_idx + 1) % M;
|
|
if (next_k == 0) {
|
|
memcpy(out_pts, in_pts, num_pts * stride * sizeof(double));
|
|
return num_pts;
|
|
}
|
|
int split = M - next_k;
|
|
memcpy(out_pts, in_pts + (next_k * stride), split * stride * sizeof(double));
|
|
memcpy(out_pts + (split * stride), in_pts, (next_k + 1) * stride * sizeof(double));
|
|
return num_pts;
|
|
}
|
|
|
|
// Insert new vertex at intersect_pt
|
|
// Output: [intersect_pt, in_pts[k+1..M-1], in_pts[0..k], intersect_pt]
|
|
int k = seg_idx;
|
|
int out_idx = 0;
|
|
|
|
// 1. First point: intersect_pt
|
|
for (int s = 0; s < stride; ++s) {
|
|
out_pts[out_idx * stride + s] = (s < 3) ? intersect_pt[s] : in_pts[k * stride + s];
|
|
}
|
|
out_idx++;
|
|
|
|
// 2. Points from k+1 to M-1
|
|
int count_rest = M - (k + 1);
|
|
if (count_rest > 0) {
|
|
memcpy(out_pts + (out_idx * stride), in_pts + ((k + 1) * stride), count_rest * stride * sizeof(double));
|
|
out_idx += count_rest;
|
|
}
|
|
|
|
// 3. Points from 0 to k
|
|
int count_prefix = k + 1;
|
|
memcpy(out_pts + (out_idx * stride), in_pts, count_prefix * stride * sizeof(double));
|
|
out_idx += count_prefix;
|
|
|
|
// 4. Last point: intersect_pt
|
|
for (int s = 0; s < stride; ++s) {
|
|
out_pts[out_idx * stride + s] = (s < 3) ? intersect_pt[s] : in_pts[k * stride + s];
|
|
}
|
|
out_idx++;
|
|
|
|
return out_idx;
|
|
}
|