Cast ray in voxel space (i.e. 3d-grid) #Unity

BoundsInt.cs

namespace UnityEngine
{
    using System;
    using System.Runtime.InteropServices;
    using UnityEngine.Assertions.Must;

    [StructLayout(LayoutKind.Auto)]
    internal struct BoundsInt
    {
        private Vector3i _min;
        private Vector3i _max;

        public Vector3 Center
        {
            get
            {
                return (this.Max - this.Min) / 2f;
            }
        }

        public Vector3 Extents
        {
            get
            {
                return this.Max - this.Center;
            }
        }

        public Vector3i Max
        {
            get
            {
                return this._max;
            }
            set
            {
                this._max = value;
                this.Size.Any(c => c < 0).MustBeFalse();
            }
        }

        public Vector3i Min
        {
            get
            {
                return this._min;
            }
            set
            {
                this._min = value;
                this.Size.Any(c => c < 0).MustBeFalse();
            }
        }

        public Vector3i Size
        {
            get
            {
                return this.Max - this.Min;
            }
            set
            {
                this.Max = this.Min + value;
                this.Size.Any(c => c < 0).MustBeFalse();
            }
        }

        public BoundsInt(Vector3i min, Vector3i max)
        {
            this._min = min;
            this._max = max;
        }

        public Vector3i ClosestPoint(Vector3i point)
        {
            throw new NotImplementedException();
        }

        public bool Contains(Vector3i point)
        {
            throw new NotImplementedException();
        }

        public void Encapsulate(Vector3i point)
        {
            throw new NotImplementedException();
        }

        public void Encapsulate(BoundsInt bounds)
        {
            throw new NotImplementedException();
        }

        public override bool Equals(object other)
        {
            throw new NotImplementedException();
        }

        public void Expand(float amount)
        {
            throw new NotImplementedException();
        }

        public void Expand(Vector3i amount)
        {
            throw new NotImplementedException();
        }

        public override int GetHashCode()
        {
            throw new NotImplementedException();
        }

        public bool IntersectRay(Ray ray)
        {
            throw new NotImplementedException();
        }

        public bool IntersectRay(Ray ray, out float distance)
        {
            throw new NotImplementedException();
        }

        public bool Intersects(BoundsInt bounds)
        {
            return (this.Min.x > bounds.Max.x || this.Max.x < bounds.Min.x || this.Min.y > bounds.Max.y || this.Max.y < bounds.Min.y || this.Min.z > bounds.Max.z ? false : this.Max.z >= bounds.Min.z);
        }

        public static bool operator ==(BoundsInt lhs, BoundsInt rhs)
        {
            throw new NotImplementedException();
        }

        public static bool operator !=(BoundsInt lhs, BoundsInt rhs)
        {
            return !(lhs == rhs);
        }

        public float SqrDistance(Vector3i point)
        {
            throw new NotImplementedException();
        }
    }
}

Vector3i.cs

using System;
using System.Diagnostics;
using UnityEngine;

[Serializable]
public struct Vector3i : IEquatable<Vector3i>
{
    public static readonly Vector3i zero = new Vector3i(0, 0, 0);
    public static readonly Vector3i one = new Vector3i(1, 1, 1);

    public static readonly Vector3i forward = new Vector3i(0, 0, 1);
    public static readonly Vector3i back = new Vector3i(0, 0, -1);
    public static readonly Vector3i up = new Vector3i(0, 1, 0);
    public static readonly Vector3i down = new Vector3i(0, -1, 0);
    public static readonly Vector3i left = new Vector3i(-1, 0, 0);
    public static readonly Vector3i right = new Vector3i(1, 0, 0);

    public static readonly Vector3i[] directions = new Vector3i[] {
        left, right,
        back, forward,
        down, up,
    };

    public int x, y, z; // do not rename, would cause deserialization problems

    [DebuggerStepThrough]
    public Vector3i(int x, int y, int z)
    {
        this.x = x;
        this.y = y;
        this.z = z;
    }

    public Vector3i(int x, int y)
    {
        this.x = x;
        this.y = y;
        this.z = 0;
    }

    public Vector3i(Vector3 original, System.Func<float, int> convert)
    {
        this.x = convert(original.x);
        this.y = convert(original.y);
        this.z = convert(original.z);
    }

    public Vector3i(int value) : this()
    {
        this.x = this.y = this.z = value;
    }

    public int Volume
    {
        get
        {
            return this.x * this.y * this.z;
        }
    }

    public int this[int index]
    {
        get
        {
            switch (index)
            {
                case 0:
                    return this.x;
                case 1:
                    return this.y;
                case 2:
                    return this.z;
                default:
                    throw new ArgumentOutOfRangeException("index");
            }
        }
        set
        {
            switch (index)
            {
                case 0:
                    this.x = value;
                    break;
                case 1:
                    this.y = value;
                    break;
                case 2:
                    this.z = value;
                    break;
                default:
                    throw new ArgumentOutOfRangeException("index");
            }
        }
    }

    public static Vector3i operator -(Vector3i a)
    {
        return new Vector3i(-a.x, -a.y, -a.z);
    }

    [DebuggerStepThrough]
    public static Vector3i operator -(Vector3i a, Vector3i b)
    {
        return new Vector3i(a.x - b.x, a.y - b.y, a.z - b.z);
    }

    public static Vector3i operator +(Vector3i a, Vector3i b)
    {
        return new Vector3i(a.x + b.x, a.y + b.y, a.z + b.z);
    }

    public static Vector3i operator *(Vector3i v, int factor)
    {
        return new Vector3i(v.x * factor, v.y * factor, v.z * factor);
    }

    public static Vector3 operator *(Vector3i v, float factor)
    {
        return new Vector3(v.x * factor, v.y * factor, v.z * factor);
    }

    public static Vector3i operator /(Vector3i v, int factor)
    {
        return new Vector3i(v.x / factor, v.y / factor, v.z / factor);
    }

    public static Vector3 operator /(Vector3i v, float factor)
    {
        return new Vector3(v.x / factor, v.y / factor, v.z / factor);
    }

    //public static Vector3i operator %(Vector3i v, int factor)
    //{
    //    return new Vector3i(v.x % factor, v.y % factor, v.z % factor);
    //}

    public static implicit operator Vector3(Vector3i v)
    {
        return new Vector3(v.x, v.y, v.z);
    }

	public static explicit operator Vector3i(Vector3 v)
	{
		return Floor(v);
	}

	public static bool operator ==(Vector3i a, Vector3i b)
    {
        return a.x == b.x &&
                a.y == b.y &&
                a.z == b.z;
    }

    public static bool operator !=(Vector3i a, Vector3i b)
    {
        return a.x != b.x ||
                a.y != b.y ||
                a.z != b.z;
    }

    public static Vector3i Min(Vector3i a, Vector3i b)
    {
        return new Vector3i(Mathf.Min(a.x, b.x), Mathf.Min(a.y, b.y), Mathf.Min(a.z, b.z));
    }

    public static Vector3i Max(Vector3i a, Vector3i b)
    {
        return new Vector3i(Mathf.Max(a.x, b.x), Mathf.Max(a.y, b.y), Mathf.Max(a.z, b.z));
    }

    public static Vector3i Floor(Vector3 v)
    {
        return new Vector3i(
            Mathf.FloorToInt(v.x),
            Mathf.FloorToInt(v.y),
            Mathf.FloorToInt(v.z)
            );
    }

    public static Vector3i Ceil(Vector3 v)
    {
        return new Vector3i(
            Mathf.CeilToInt(v.x),
            Mathf.CeilToInt(v.y),
            Mathf.CeilToInt(v.z)
            );
    }

    public static Vector3i Round(Vector3 v)
    {
        return new Vector3i(
            Mathf.RoundToInt(v.x),
            Mathf.RoundToInt(v.y),
            Mathf.RoundToInt(v.z)
            );
    }

    public Vector3i Mul(Vector3i factor)
    {
        return new Vector3i(x * factor.x, y * factor.y, z * factor.z);
    }

    public Vector3i Div(Vector3i factor)
    {
        return new Vector3i(x / factor.x, y / factor.y, z / factor.z);
    }

	public Vector3i Sign()
	{
		return new Vector3i(
			System.Math.Sign(this.x),
			System.Math.Sign(this.y),
			System.Math.Sign(this.z));
    }

    public bool Any(Predicate<int> p)
    {
        if (p(x))
            return true;
        if (p(y))
            return true;
        if (p(z))
            return true;

        return false;
    }

    public override bool Equals(object other)
    {
        if (!(other is Vector3i))
			return false;

        return this.Equals((Vector3i)other);
    }

    public override int GetHashCode()
    {
        //return x.GetHashCode() ^ y.GetHashCode() << 2 ^ z.GetHashCode() >> 2;
        unchecked
        {
            var hash = (int)23988961768543;
            hash = hash * (int)8187723713453 + x.GetHashCode();
            hash = hash * (int)8187723713453 + y.GetHashCode();
            hash = hash * (int)8187723713453 + z.GetHashCode();
            return hash;
        }
    }

    public override string ToString()
    {
        return string.Format("Vector3i({0} {1} {2})", x, y, z);
    }

	public bool Equals(Vector3i other)
	{
		return x == other.x &&
			   y == other.y &&
			   z == other.z;
	}
}

VoxelRaycast.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using ModelSystem;
using UnityEngine;

// Heavily based on:
// http://gamedev.stackexchange.com/a/49423/8806
internal class VoxelRaycast
{
    private readonly BoundsInt? worldBounds;

    public VoxelRaycast(BoundsInt? worldBounds)
    {
        this.worldBounds = worldBounds;
    }

    /**
     * Call the callback with (position, face) of all blocks along the line
     * segment from point 'origin' in vector direction 'direction' of length
     * 'radius'. 'radius' may be infinite, but beware infinite loop in this case.
     *
     * 'face' is the normal vector of the face of that block that was entered.
     *
     * If the callback returns a true value, the traversal will be stopped.
     */
    public void Raycast(
        Vector3 origin,
        Vector3 direction,
        float radius,
        Func<Vector3i, Vector3i, bool> callback)
    {
        // From "A Fast Voxel Traversal Algorithm for Ray Tracing"
        // by John Amanatides and Andrew Woo, 1987
        // <http://www.cse.yorku.ca/~amana/research/grid.pdf>
        // <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
        // Extensions to the described algorithm:
        //   • Imposed a distance limit.
        //   • The face passed through to reach the current cube is provided to
        //     the callback.

        // The foundation of this algorithm is a parameterized representation of
        // the provided ray,
        //                    origin + t * direction,
        // except that t is not actually stored; rather, at any given point in the
        // traversal, we keep track of the *greater* t values which we would have
        // if we took a step sufficient to cross a cube boundary along that axis
        // (i.e. change the integer part of the coordinate) in the variables
        // tMaxX, tMaxY, and tMaxZ.

        // Cube containing origin point.
        var x = Mathf.FloorToInt(origin[0]);
        var y = Mathf.FloorToInt(origin[1]);
        var z = Mathf.FloorToInt(origin[2]);

        // Break out direction vector.
        var dx = direction[0];
        var dy = direction[1];
        var dz = direction[2];

        // Direction to increment x,y,z when stepping.
        var stepX = signum(dx);
        var stepY = signum(dy);
        var stepZ = signum(dz);

        // See description above. The initial values depend on the fractional
        // part of the origin.
        var tMaxX = intbound(origin[0], dx);
        var tMaxY = intbound(origin[1], dy);
        var tMaxZ = intbound(origin[2], dz);

        // The change in t when taking a step (always positive).
        var tDeltaX = stepX / dx;
        var tDeltaY = stepY / dy;
        var tDeltaZ = stepZ / dz;

        // Buffer for reporting faces to the callback.
        var face = new Vector3i();

        // Avoids an infinite loop.
        if (dx == 0 && dy == 0 && dz == 0)
            throw new Exception("Ray-cast in zero direction!");

        // Rescale from units of 1 cube-edge to units of 'direction' so we can
        // compare with 't'.
        radius /= Mathf.Sqrt(dx * dx + dy * dy + dz * dz);

        // Deal with world bounds or their absence.
        Vector3i min, max;
        min = max = default(Vector3i);
        var worldIsUnlimited = !this.worldBounds.HasValue;
        if (!worldIsUnlimited)
        {
            min = this.worldBounds.Value.Min;
            max = this.worldBounds.Value.Max;
        }

        while (worldIsUnlimited || (
            /* ray has not gone past bounds of world */
            (stepX > 0 ? x < max.x : x >= min.x) &&
            (stepY > 0 ? y < max.y : y >= min.y) &&
            (stepZ > 0 ? z < max.z : z >= min.z)))
        {
            // Invoke the callback, unless we are not *yet* within the bounds of the
            // world.
            if (worldIsUnlimited ||
                !(x < min.x || y < min.y || z < min.z || x >= max.x || y >= max.y || z >= max.z))
                if (callback(new Vector3i(x, y, z), face))
                    break;

            // tMaxX stores the t-value at which we cross a cube boundary along the
            // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
            // chooses the closest cube boundary. Only the first case of the four
            // has been commented in detail.
            if (tMaxX < tMaxY)
            {
                if (tMaxX < tMaxZ)
                {
                    if (tMaxX > radius)
                        break;
                    // Update which cube we are now in.
                    x += stepX;
                    // Adjust tMaxX to the next X-oriented boundary crossing.
                    tMaxX += tDeltaX;
                    // Record the normal vector of the cube face we entered.
                    face[0] = -stepX;
                    face[1] = 0;
                    face[2] = 0;
                }
                else
                {
                    if (tMaxZ > radius)
                        break;
                    z += stepZ;
                    tMaxZ += tDeltaZ;
                    face[0] = 0;
                    face[1] = 0;
                    face[2] = -stepZ;
                }
            }
            else
            {
                if (tMaxY < tMaxZ)
                {
                    if (tMaxY > radius)
                        break;
                    y += stepY;
                    tMaxY += tDeltaY;
                    face[0] = 0;
                    face[1] = -stepY;
                    face[2] = 0;
                }
                else
                {
                    // Identical to the second case, repeated for simplicity in
                    // the conditionals.
                    if (tMaxZ > radius)
                        break;
                    z += stepZ;
                    tMaxZ += tDeltaZ;
                    face[0] = 0;
                    face[1] = 0;
                    face[2] = -stepZ;
                }
            }
        }
    }

    private float intbound(float s, float ds)
    {
        // Some kind of edge case, see:
        // http://gamedev.stackexchange.com/questions/47362/cast-ray-to-select-block-in-voxel-game#comment160436_49423
        var sIsInteger = Mathf.Round(s) == s;
        if (ds < 0 && sIsInteger)
            return 0;

        return (ds > 0 ? Mathf.Ceil(s) - s : s - Mathf.Floor(s)) / Mathf.Abs(ds);
    }

    private int signum(float x)
    {
        return x > 0 ? 1 : x < 0 ? -1 : 0;
    }
}

Another edge case: intbound does not work if s is zero. To work properly, I had to define a custom ceil function:

private static float ceil(float s) { if (s == 0f) return 1f; else return Mathf.Ceil(s); }

To replace the call to Mathf.Ceil used in intbound. Then it worked properly even if the ray started centered on precisely origin.