|
5 | 5 |
|
6 | 6 | import numpy as np |
7 | 7 | import open3d as o3d |
8 | | - |
9 | 8 | from diffCheck import diffcheck_bindings |
10 | 9 |
|
11 | 10 | def cloud_2_cloud_distance(source, target, signed=False): |
12 | 11 | """ |
13 | 12 | Compute the Euclidean distance for every point of a source pcd to its closest point on a target pointcloud |
14 | 13 | """ |
15 | | - distances = np.asarray(source.compute_P2PDistance(target)) |
16 | | - |
17 | 14 | if signed: |
18 | | - |
19 | | - # Build a KD-tree for the target points |
20 | | - pcd = o3d.geometry.PointCloud() |
21 | | - pcd.points = o3d.utility.Vector3dVector(target.points) |
22 | | - kdtree = o3d.geometry.KDTreeFlann(pcd) |
23 | | - print("KD-tree built successfully.") |
24 | | - |
25 | | - for i in range(len(source.points)): |
26 | | - |
27 | | - query = np.asarray(source.points[i], dtype=np.float64).reshape(3) |
28 | | - # Query the KD-tree to find the nearest neighbor |
29 | | - try: |
30 | | - _, idx, _ = kdtree.search_knn_vector_3d(query, 1) |
31 | | - except Exception as e: |
32 | | - print(f"Error querying KD-tree for point {i}: {e}") |
33 | | - continue |
34 | | - |
35 | | - closest_idx = idx[0] |
36 | | - # Calculate the direction from target to source |
37 | | - direction = source.points[i] - target.points[closest_idx] |
38 | | - |
39 | | - # Calculate the signed distance |
40 | | - dot_product = np.dot(direction, target.normals[closest_idx]) |
41 | | - if dot_product < 0: |
42 | | - distances[i] = -distances[i] |
| 15 | + distances = np.asarray(source.compute_distance(target, is_abs=False)) |
| 16 | + else: |
| 17 | + distances = np.asarray(source.compute_distance(target, is_abs=True)) |
43 | 18 |
|
44 | 19 | return distances |
45 | 20 |
|
46 | 21 |
|
47 | | -def cloud_2_mesh_distance(source, target): |
| 22 | +def cloud_2_mesh_distance(source, target, signed=False): |
48 | 23 | """ |
49 | | - Calculate the distance between every point of a source pcd to its closest point on a target mesh |
| 24 | + Calculate the distance between every point of a source pcd to its closest point on a target beam |
50 | 25 | """ |
51 | 26 |
|
52 | 27 | # for every point on the PCD compute the point_2_mesh_distance |
53 | | - |
54 | | - distances = np.ones(len(source.points), dtype=float) |
55 | | - |
56 | | - for i in range(len(source.points)): |
57 | | - distances[i] = point_2_mesh_distance(target, source.points[i]) |
58 | | - |
59 | | - return distances |
60 | | - |
61 | | - |
62 | | -def point_2_mesh_distance(geo, query_point): |
63 | | - """ |
64 | | - Calculate the closest distance between a point and a target geometry |
65 | | - """ |
66 | | - # make a kdtree of the vertices to get the relevant vertices indexes |
67 | | - pcd = diffcheck_bindings.dfb_geometry.DFPointCloud() |
68 | | - pcd.points = geo.vertices |
69 | | - kd_tree = o3d.geometry.KDTreeFlann(pcd) |
70 | | - |
71 | | - # assume smallest distance is the distance to the closest vertex |
72 | | - _, idx, _ = kd_tree.search_knn_vector_3d(query_point, 1) |
73 | | - if idx: |
74 | | - nearest_vertex_idx = idx[0] |
75 | | - else: |
76 | | - raise ValueError("The mesh or brep has no vertices. Please provide a valid geometry.") |
77 | | - nearest_vertex = np.asarray(geo.vertices)[nearest_vertex_idx] |
78 | | - dist = np.linalg.norm(query_point - nearest_vertex) |
79 | | - |
80 | | - # Find its neighbors with distance less than _dist_ multiplied by two. |
81 | | - search_distance = dist * 2 |
82 | | - _, v_indices, _ = kd_tree.search_radius_vector_3d(query_point, search_distance) |
83 | | - |
84 | | - # Find the faces that belong to these filtered vertices. |
85 | | - geo_triangles = np.asarray(geo.triangles) |
86 | | - candidate_mask = np.isin(geo_triangles, v_indices) |
87 | | - candidate_faces = geo_triangles[np.any(candidate_mask, axis=1)] |
88 | | - |
89 | | - # Step 4: Loop over candidate faces |
90 | | - shortest_distance = float('inf') |
91 | | - |
92 | | - for face in candidate_faces: |
93 | | - #v0, v1, v2 = np.asarray(geo.vertices)[face] |
94 | | - pt_face_dist = point_2_face_distance(face, query_point) |
95 | | - if pt_face_dist < shortest_distance: |
96 | | - shortest_distance = pt_face_dist |
97 | | - |
98 | | - return shortest_distance |
99 | | - |
100 | | - |
101 | | -def point_2_face_distance(face, point): |
102 | | - """ |
103 | | - Calculate the closest distance between a point and a face |
104 | | - """ |
105 | | - |
106 | | - if len(face.vertices) == 3: |
107 | | - return point_2_triangle_distance(point, face) |
108 | | - elif len(face.vertices) == 4: |
109 | | - return point_2_quad_distance(point, face) |
110 | | - else: |
111 | | - raise ValueError("Face must be a triangle or quadrilateral") |
112 | | - |
113 | | - |
114 | | -def point_2_triangle_distance(point, triangle): |
115 | | - """ |
116 | | - Calculate the shortest distance from a point to a triangle. |
117 | | - """ |
118 | | - a, b, c = triangle |
119 | | - |
120 | | - bary_coords = barycentric_coordinates(point, a, b, c) |
121 | | - |
122 | | - # If the point is inside or on the triangle, use the barycentric coordinates to find the closest point |
123 | | - if np.all(bary_coords >= 0): |
124 | | - closest_point = bary_coords[0] * a + bary_coords[1] * b + bary_coords[2] * c |
125 | | - |
126 | | - # If the point is outside the triangle, project it onto the triangle edges and find the closest point |
| 28 | + if signed: |
| 29 | + distances = np.asarray(target.compute_distance(source, is_abs=False)) |
127 | 30 | else: |
128 | | - proj = np.array([np.dot(point - a, b - a) / np.dot(b - a, b - a), |
129 | | - np.dot(point - b, c - b) / np.dot(c - b, c - b), |
130 | | - np.dot(point - c, a - c) / np.dot(a - c, a - c)]) |
131 | | - proj = np.clip(proj, 0, 1) |
132 | | - closest_point = np.array([a + proj[0] * (b - a), b + proj[1] * (c - b), c + proj[2] * (a - c)]).min(axis=0) |
133 | | - |
134 | | - return np.linalg.norm(closest_point - point) |
135 | | - |
| 31 | + distances = np.asarray(target.compute_distance(source, is_abs=True)) |
136 | 32 |
|
137 | | -def barycentric_coordinates(p, a, b, c): |
138 | | - """ |
139 | | - Calculate the barycentric coordinates of point p with respect to the triangle defined by points a, b, and c. |
140 | | - """ |
141 | | - v0 = b - a |
142 | | - v1 = c - a |
143 | | - v2 = p - a |
144 | | - |
145 | | - d00 = np.dot(v0, v0) |
146 | | - d01 = np.dot(v0, v1) |
147 | | - d11 = np.dot(v1, v1) |
148 | | - d20 = np.dot(v2, v0) |
149 | | - d21 = np.dot(v2, v1) |
150 | | - |
151 | | - denom = d00 * d11 - d01 * d01 |
152 | | - v = (d11 * d20 - d01 * d21) / denom |
153 | | - w = (d00 * d21 - d01 * d20) / denom |
154 | | - u = 1.0 - v - w |
155 | | - |
156 | | - return np.array([u, v, w]) |
157 | | - |
158 | | - |
159 | | -def point_2_quad_distance(point, quad): |
160 | | - """ |
161 | | - Calculate the shortest distance from a point to a quadrilateral. |
162 | | - """ |
163 | | - a, b, c, d = quad.vertices |
164 | | - |
165 | | - # Calculate the distance to the two triangles that form the quadrilateral |
166 | | - return min(point_2_triangle_distance(point, [a, b, c]), |
167 | | - point_2_triangle_distance(point, [c, d, a])) |
| 33 | + return distances |
168 | 34 |
|
169 | 35 |
|
170 | 36 | def compute_mse(distances): |
|
0 commit comments