fix: voxel size != 1.0 error

This commit is contained in:
gdamms 2023-01-25 21:24:29 +01:00
parent 150210fa26
commit 1d8f97b2fd

View file

@ -1,7 +1,7 @@
import numpy as np import numpy as np
def fast_voxel_intersect(start, end, origin, step, shape) -> tuple[list, list, list]: def fast_voxel_intersect(start_, end_, origin_, step_, shape_) -> tuple[list, list, list]:
"""Compute the voxels intersected by a line segment. """Compute the voxels intersected by a line segment.
Args: Args:
@ -17,14 +17,14 @@ def fast_voxel_intersect(start, end, origin, step, shape) -> tuple[list, list, l
""" """
# Convert to numpy arrays # Convert to numpy arrays
start = np.asarray(start) start_ = np.asarray(start_)
end = np.asarray(end) end_ = np.asarray(end_)
origin = np.asarray(origin) origin_ = np.asarray(origin_)
step = np.asarray(step) step_ = np.asarray(step_)
# Translate line segment to voxel grid # Translate line segment to voxel grid
start = start - origin start = (start_ - origin_) / step_
end = end - origin end = (end_ - origin_) / step_
# Initialize list of intersected voxels # Initialize list of intersected voxels
intersections = [] intersections = []
@ -45,16 +45,18 @@ def fast_voxel_intersect(start, end, origin, step, shape) -> tuple[list, list, l
# Initialize current position to start # Initialize current position to start
position = start.copy() position = start.copy()
# print("position: ", position)
# Initialize early exit
is_in = False
# Main loop # Main loop
while True: while True:
# Compute the distance to the next boundaries # Compute the distance to the next boundaries
next_boundaries = np.divide(position + step * direction_signs, step) next_boundaries = position + direction_signs
errored = np.abs(np.round(next_boundaries) - next_boundaries) < 1e-12 errored = np.abs(np.round(next_boundaries) - next_boundaries) < 1e-12
next_boundaries[errored] = np.round(next_boundaries[errored]) next_boundaries[errored] = np.round(next_boundaries[errored])
distances = ((1 - is_negative) * np.floor(next_boundaries) + distances = ((1 - is_negative) * np.floor(next_boundaries) +
is_negative * np.ceil(next_boundaries)) * step - position is_negative * np.ceil(next_boundaries)) - position
# Determine the nearest boundary to be reached # Determine the nearest boundary to be reached
boundary_distances = np.abs(distances / direction) boundary_distances = np.abs(distances / direction)
@ -68,28 +70,24 @@ def fast_voxel_intersect(start, end, origin, step, shape) -> tuple[list, list, l
# Update position # Update position
position = position + clothest_boundary_distance * direction position = position + clothest_boundary_distance * direction
# print("position_update: ", position)
# Correct position to be on boundary # Correct position to be on boundary
position[clothest_boundary] = round( position[clothest_boundary] = round(position[clothest_boundary])
position[clothest_boundary] / step[clothest_boundary]) * step[clothest_boundary]
# Get corresponding voxel # Get corresponding voxel
on_boundary = np.mod(position, step) == 0 on_boundary = np.mod(position, np.ones_like(position)) == 0
voxel = np.floor_divide(position, step) * step - is_negative * on_boundary * step voxel = np.floor(position) - is_negative * on_boundary
# Add voxel to list # Add voxel to list
idx = np.floor_divide(voxel, step).astype(int) idx = np.floor(voxel).astype(int)
if np.any(idx < 0) or np.any(idx >= shape): if np.any(idx < 0) or np.any(idx >= shape_):
if is_in:
break
continue continue
# print(f"voxel: {voxel}, step: {step}, idx: {idx}") intersections.append(position * step_ + origin_)
intersections.append(position + origin) voxels.append(voxel * step_ + origin_)
voxels.append(voxel + origin)
voxels_idx.append(idx) voxels_idx.append(idx)
is_in = True
# print(f"intersections: {intersections}")
# print(f"voxels: {voxels}")
# print(f"voxels_idx: {voxels_idx}")
return intersections, voxels, voxels_idx return intersections, voxels, voxels_idx
@ -119,13 +117,15 @@ if __name__ == '__main__':
# Plot line segment # Plot line segment
plt.plot([start[0], end[0]], [start[1], end[1]], 'k-') plt.plot([start[0], end[0]], [start[1], end[1]], 'k-')
plt.plot(start[0], start[1], 'go')
plt.plot(end[0], end[1], 'ro')
# Plot intersection points # Plot intersection points
for pos in positions: for pos in positions:
plt.plot(pos[0], pos[1], 'bo') plt.plot(pos[0], pos[1], 'bo')
# Plot start and end points
plt.plot(start[0], start[1], 'go')
plt.plot(end[0], end[1], 'ro')
# Plot voxel grid # Plot voxel grid
plt.axis('equal') plt.axis('equal')
plt.xlim((-10, 10)) plt.xlim((-10, 10))
@ -144,7 +144,7 @@ if __name__ == '__main__':
# Define voxel grid # Define voxel grid
origin = np.array([-5., -5.]) origin = np.array([-5., -5.])
step = np.array([0.7, 0.7]) step = np.array([1.4, 1.4])
shape = (10, 10) shape = (10, 10)
# Define segment # Define segment