import numpy as np
import matplotlib.pyplot as plt
from itertools import product


def check_line_voxel(
    px, py, pz,
    dx, dy, dz,
    vx, vy, vz,
    c
):
    """Check if a line intersects a voxel."""

    # Compute the intersection bounds
    kx1 = (px - dx) / vx
    ky1 = (py - dy) / vy
    kz1 = (pz - dz) / vz
    kx2 = (px - dx + c) / vx
    ky2 = (py - dy + c) / vy
    kz2 = (pz - dz + c) / vz

    # Order the bounds
    kxmin = np.min(np.concatenate([
        kx1[:, np.newaxis],
        kx2[:, np.newaxis]
    ], axis=1), axis=1)
    kymin = np.min(np.concatenate([
        ky1[:, np.newaxis],
        ky2[:, np.newaxis]
    ], axis=1), axis=1)
    kzmin = np.min(np.concatenate([
        kz1[:, np.newaxis],
        kz2[:, np.newaxis]
    ], axis=1), axis=1)
    kxmax = np.max(np.concatenate([
        kx1[:, np.newaxis],
        kx2[:, np.newaxis]
    ], axis=1), axis=1)
    kymax = np.max(np.concatenate([
        ky1[:, np.newaxis],
        ky2[:, np.newaxis]
    ], axis=1), axis=1)
    kzmax = np.max(np.concatenate([
        kz1[:, np.newaxis],
        kz2[:, np.newaxis]
    ], axis=1), axis=1)

    # Check if the bounds overlap
    kmax = np.min(np.concatenate([
        kxmax[:, np.newaxis],
        kymax[:, np.newaxis],
        kzmax[:, np.newaxis]
    ], axis=1), axis=1)
    kmin = np.max(np.concatenate([
        kxmin[:, np.newaxis],
        kymin[:, np.newaxis],
        kzmin[:, np.newaxis]
    ], axis=1), axis=1)
    return kmin <= kmax

c = 1.0
points = np.array([[x, y, z] for x, y, z in product(
    np.arange(-5.0, 4.0, c),
    np.arange(-5.0, 4.0, c),
    np.arange(-5.0, 4.0, c))
])
while True:

    fig = plt.figure()
    ax = plt.axes(projection='3d')

    d = np.random.rand(3) * 1 - 0.5
    v = np.random.rand(3) * 1 - 0.5

    px, py, pz = points[:, 0], points[:, 1], points[:, 2]
    dx, dy, dz = d
    vx, vy, vz = v

    bool_vect = check_line_voxel(px, py, pz, dx, dy, dz, vx, vy, vz, c)

    # plot cube
    for i, (px, py, pz) in enumerate(points):
        if not bool_vect[i]:
            continue

        ax.plot([px, px+c], [py, py], [pz, pz], 'b')
        ax.plot([px, px+c], [py, py], [pz+c, pz+c], 'b')
        ax.plot([px, px+c], [py+c, py+c], [pz, pz], 'b')
        ax.plot([px, px+c], [py+c, py+c], [pz+c, pz+c], 'b')
        ax.plot([px, px], [py, py+c], [pz, pz], 'b')
        ax.plot([px, px], [py, py+c], [pz+c, pz+c], 'b')
        ax.plot([px+c, px+c], [py, py+c], [pz, pz], 'b')
        ax.plot([px+c, px+c], [py, py+c], [pz+c, pz+c], 'b')
        ax.plot([px, px], [py, py], [pz, pz+c], 'b')
        ax.plot([px, px], [py+c, py+c], [pz, pz+c], 'b')
        ax.plot([px+c, px+c], [py, py], [pz, pz+c], 'b')
        ax.plot([px+c, px+c], [py+c, py+c], [pz, pz+c], 'b')

    # plot line
    ax.plot([dx, dx+vx], [dy, dy+vy], [dz, dz+vz], 'g')

    plt.show()