Cost distance is one of the fundamental functions in geographical information systems (GISs). 3D cost distance function makes the analysis of movement in 3D frictions possible. In this paper, we propose an algorithm and efficient data structures to accurately calculate the cost distance in discrete 3D space. Specifically, Dijkstra’s algorithm is used to calculate the least cost between initial voxels and all the other voxels in 3D space. During the calculation, unnecessary bends along the travel path are constantly corrected to retain the accurate least cost. Our results show that the proposed algorithm can generate true Euclidean distance in homogeneous frictions and can provide more accurate least cost in heterogeneous frictions than that provided by several existing methods. Furthermore, the proposed data structures, i.e., a heap combined with a hash table, significantly improve the algorithm’s efficiency. The algorithm and data structures have been verified via several applications including planning the shortest drone delivery path in an urban environment, generating volumetric viewshed, and calculating the minimum hydraulic resistance.