|
|
@@ -0,0 +1,929 @@
|
|
|
+(in-package :cl-user)
|
|
|
+(defpackage #:ocm.s2
|
|
|
+ (:use :cl))
|
|
|
+(in-package :ocm.s2)
|
|
|
+
|
|
|
+;; In the process of converting a latitude-longitude pair to a 64-bit cell
|
|
|
+;; id, the following coordinate systems are used:
|
|
|
+;;
|
|
|
+;; (id)
|
|
|
+;; An S2CellId is a 64-bit encoding of a face and a Hilbert curve position
|
|
|
+;; on that face. The Hilbert curve position implicitly encodes both the
|
|
|
+;; position of a cell and its subdivision level (see s2cell_id.h).
|
|
|
+;;
|
|
|
+;; (face, i, j)
|
|
|
+;; Leaf-cell coordinates. "i" and "j" are integers in the range
|
|
|
+;; [0,(2**30)-1] that identify a particular leaf cell on the given face.
|
|
|
+;; The (i, j) coordinate system is right-handed on each face, and the
|
|
|
+;; faces are oriented such that Hilbert curves connect continuously from
|
|
|
+;; one face to the next.
|
|
|
+;;
|
|
|
+;; (face, s, t)
|
|
|
+;; Cell-space coordinates. "s" and "t" are real numbers in the range
|
|
|
+;; [0,1] that identify a point on the given face. For example, the point
|
|
|
+;; (s, t) = (0.5, 0.5) corresponds to the center of the top-level face
|
|
|
+;; cell. This point is also a vertex of exactly four cells at each
|
|
|
+;; subdivision level greater than zero.
|
|
|
+;;
|
|
|
+;; (face, si, ti)
|
|
|
+;; Discrete cell-space coordinates. These are obtained by multiplying
|
|
|
+;; "s" and "t" by 2**31 and rounding to the nearest unsigned integer.
|
|
|
+;; Discrete coordinates lie in the range [0,2**31]. This coordinate
|
|
|
+;; system can represent the edge and center positions of all cells with
|
|
|
+;; no loss of precision (including non-leaf cells). In binary, each
|
|
|
+;; coordinate of a level-k cell center ends with a 1 followed by
|
|
|
+;; (30 - k) 0s. The coordinates of its edges end with (at least)
|
|
|
+;; (31 - k) 0s.
|
|
|
+;;
|
|
|
+;; (face, u, v)
|
|
|
+;; Cube-space coordinates in the range [-1,1]. To make the cells at each
|
|
|
+;; level more uniform in size after they are projected onto the sphere,
|
|
|
+;; we apply a nonlinear transformation of the form u=f(s), v=f(t).
|
|
|
+;; The (u, v) coordinates after this transformation give the actual
|
|
|
+;; coordinates on the cube face (modulo some 90 degree rotations) before
|
|
|
+;; it is projected onto the unit sphere.
|
|
|
+;;
|
|
|
+;; (face, u, v, w)
|
|
|
+;; Per-face coordinate frame. This is an extension of the (face, u, v)
|
|
|
+;; cube-space coordinates that adds a third axis "w" in the direction of
|
|
|
+;; the face normal. It is always a right-handed 3D coordinate system.
|
|
|
+;; Cube-space coordinates can be converted to this frame by setting w=1,
|
|
|
+;; while (u,v,w) coordinates can be projected onto the cube face by
|
|
|
+;; dividing by w, i.e. (face, u/w, v/w).
|
|
|
+;;
|
|
|
+;; (x, y, z)
|
|
|
+;; Direction vector (S2Point). Direction vectors are not necessarily unit
|
|
|
+;; length, and are often chosen to be points on the biunit cube
|
|
|
+;; [-1,+1]x[-1,+1]x[-1,+1]. They can be be normalized to obtain the
|
|
|
+;; corresponding point on the unit sphere.
|
|
|
+;;
|
|
|
+;; (lat, lng)
|
|
|
+;; Latitude and longitude (S2LatLng). Latitudes must be between -90 and
|
|
|
+;; 90 degrees inclusive, and longitudes must be between -180 and 180
|
|
|
+;; degrees inclusive.
|
|
|
+;;
|
|
|
+;; Note that the (i, j), (s, t), (si, ti), and (u, v) coordinate systems are
|
|
|
+;; right-handed on all six faces.
|
|
|
+
|
|
|
+(defparameter +swap-mask+ 1)
|
|
|
+(defparameter +invert-mask+ 2)
|
|
|
+(defparameter +max-cell-level+ 30 "This is the number of levels needed to specify a leaf cell.")
|
|
|
+(defparameter +limit-ij+ (ash 1 +max-cell-level+) "The maximum index of a valid leaf cell plus one. The range of valid leaf cell indices is [0..+limit-ij+-1].")
|
|
|
+(defparameter +max-si-ti+ (ash 1 (+ 1 +max-cell-level+)) "The maximum value of an si- or ti-coordinate. The range of valid (si,ti) values is [0..+max-si-ti+].")
|
|
|
+
|
|
|
+;; An S2CellId is a 64-bit unsigned integer that uniquely identifies a
|
|
|
+;; cell in the S2 cell decomposition. It has the following format:
|
|
|
+;;
|
|
|
+;; id = [face][face_pos]
|
|
|
+;;
|
|
|
+;; face: a 3-bit number (range 0..5) encoding the cube face.
|
|
|
+;;
|
|
|
+;; face_pos: a 61-bit number encoding the position of the center of this
|
|
|
+;; cell along the Hilbert curve over this face (see the Wiki
|
|
|
+;; pages for details).
|
|
|
+;;
|
|
|
+;; Sequentially increasing cell ids follow a continuous space-filling curve
|
|
|
+;; over the entire sphere. They have the following properties:
|
|
|
+;;
|
|
|
+;; - The id of a cell at level k consists of a 3-bit face number followed
|
|
|
+;; by k bit pairs that recursively select one of the four children of
|
|
|
+;; each cell. The next bit is always 1, and all other bits are 0.
|
|
|
+;; Therefore, the level of a cell is determined by the position of its
|
|
|
+;; lowest-numbered bit that is turned on (for a cell at level k, this
|
|
|
+;; position is 2 * (kMaxLevel - k).)
|
|
|
+;;
|
|
|
+;; - The id of a parent cell is at the midpoint of the range of ids spanned
|
|
|
+;; by its children (or by its descendants at any level).
|
|
|
+(defparameter +face-bits+ 3)
|
|
|
+(defparameter +num-faces+ 6)
|
|
|
+(defparameter +max-level+ +max-cell-level+)
|
|
|
+(defparameter +pos-bits+ (1+ (* 2 +max-level+)))
|
|
|
+(defparameter +max-size+ (ash 1 +max-level+))
|
|
|
+
|
|
|
+(defun st-to-ij (s)
|
|
|
+ (max 0 (min (- +limit-ij+ 1) (round (- (* s +limit-ij+) 0.5d0)))))
|
|
|
+
|
|
|
+;; Use quadratic (s,t) -> (u,v) projection. See S2 sources for explanation.
|
|
|
+(defun st-to-uv (s)
|
|
|
+ (* (/ 1 3)
|
|
|
+ (if (>= s 0.5d0)
|
|
|
+ (- (* 4 s s) 1)
|
|
|
+ (- 1 (* 4 (- s 1) (- s 1))))))
|
|
|
+(defun uv-to-st (u)
|
|
|
+ (if (>= u 0)
|
|
|
+ (* 0.5d0 (sqrt (+ 1 (* 3 u))))
|
|
|
+ (- 1 (* 0.5d0 (sqrt (- 1 (* 3 u)))))))
|
|
|
+
|
|
|
+(defstruct point
|
|
|
+ (x 0.0d0 :type double-float)
|
|
|
+ (y 0.0d0 :type double-float)
|
|
|
+ (z 0.0d0 :type double-float))
|
|
|
+
|
|
|
+(defstruct latlng
|
|
|
+ (lat 0d0 :type double-float)
|
|
|
+ (lng 0d0 :type double-float))
|
|
|
+
|
|
|
+(defun deg-to-rad (deg)
|
|
|
+ (* (/ pi 180d0) deg))
|
|
|
+(defun rad-to-deg (rad)
|
|
|
+ (* (/ 180d0 pi) rad))
|
|
|
+
|
|
|
+(defun from-radians (lat-rad lng-rad)
|
|
|
+ (make-latlng :lat lat-rad :lng lng-rad))
|
|
|
+(defun from-degrees (lat-deg lng-deg)
|
|
|
+ (make-latlng :lat (deg-to-rad lat-deg) :lng (deg-to-rad lng-deg)))
|
|
|
+(defun to-degrees (latlng)
|
|
|
+ (declare (type latlng latlng))
|
|
|
+ (with-slots (lat lng) latlng
|
|
|
+ (cons (rad-to-deg lat) (rad-to-deg lng))))
|
|
|
+(defun latlng-to-point (latlng)
|
|
|
+ (declare (type latlng latlng))
|
|
|
+ (with-slots (lat lng) latlng
|
|
|
+ (let* ((phi lat)
|
|
|
+ (theta lng)
|
|
|
+ (cos-phi (cos phi)))
|
|
|
+ (make-point :x (* (cos theta) cos-phi)
|
|
|
+ :y (* (sin theta) cos-phi)
|
|
|
+ :z (sin phi)))))
|
|
|
+(defun point-lat (p)
|
|
|
+ (declare (type point p))
|
|
|
+ (with-slots (x y z) p
|
|
|
+ (atan z (sqrt (+ (* x x) (* y y))))))
|
|
|
+(defun point-lng (p)
|
|
|
+ (declare (type point p))
|
|
|
+ (with-slots (x y z) p
|
|
|
+ (atan y x)))
|
|
|
+(defun point-to-lat-lng (p)
|
|
|
+ (declare (type point p))
|
|
|
+ (from-radians (point-lat p) (point-lng p)))
|
|
|
+
|
|
|
+(defun dot (p1 p2)
|
|
|
+ (declare (type point p1 p2))
|
|
|
+ (+ (* (point-x p1) (point-x p2))
|
|
|
+ (* (point-y p1) (point-y p2))
|
|
|
+ (* (point-z p1) (point-z p2))))
|
|
|
+
|
|
|
+(defparameter +face-uvw-axes+
|
|
|
+ (make-array '(6 3) :element-type 'point :initial-contents
|
|
|
+ (list
|
|
|
+ (list (make-point :y 1d0) (make-point :z 1d0) (make-point :x 1d0))
|
|
|
+ (list (make-point :x -1d0) (make-point :z 1d0) (make-point :y 1d0))
|
|
|
+ (list (make-point :x -1d0) (make-point :y -1d0) (make-point :z 1d0))
|
|
|
+ (list (make-point :z -1d0) (make-point :y -1d0) (make-point :x -1d0))
|
|
|
+ (list (make-point :z -1d0) (make-point :x 1d0) (make-point :y -1d0))
|
|
|
+ (list (make-point :y 1d0) (make-point :x 1d0) (make-point :z -1d0))))
|
|
|
+ "The U,V,W axes for each face.")
|
|
|
+
|
|
|
+(defun get-xyz-face (p)
|
|
|
+ (declare (type point p))
|
|
|
+ (with-slots (x y z) p
|
|
|
+ (cond
|
|
|
+ ((and (> (abs x) (abs y))
|
|
|
+ (> (abs x) (abs z)))
|
|
|
+ (if (>= x 0) 0 3))
|
|
|
+ ((and (> (abs y) (abs x))
|
|
|
+ (> (abs y) (abs z)))
|
|
|
+ (if (>= y 0) 1 4))
|
|
|
+ (t
|
|
|
+ (if (>= z 0) 2 5)))))
|
|
|
+
|
|
|
+(defun get-uvw-axis (face axis)
|
|
|
+ (aref +face-uvw-axes+ face axis))
|
|
|
+
|
|
|
+(defun get-norm (face)
|
|
|
+ (get-uvw-axis face 2))
|
|
|
+
|
|
|
+(defun valid-face-xyz-to-uv (face p)
|
|
|
+ (declare (type point p))
|
|
|
+ (assert (> (dot (get-norm face) p) 0))
|
|
|
+ (with-slots (x y z) p
|
|
|
+ (ecase face
|
|
|
+ (0 (values (/ y x) (/ z x)))
|
|
|
+ (1 (values (/ (- x) y) (/ z y)))
|
|
|
+ (2 (values (/ (- x) z) (/ (- y) z)))
|
|
|
+ (3 (values (/ z x) (/ y x)))
|
|
|
+ (4 (values (/ z y) (/ (- x) y)))
|
|
|
+ (5 (values (/ (- y) z) (/ (- x) z))))))
|
|
|
+
|
|
|
+(defun xyz-to-face-uv (p)
|
|
|
+ (declare (type point p))
|
|
|
+ (let ((face (get-xyz-face p)))
|
|
|
+ (multiple-value-bind (u v)
|
|
|
+ (valid-face-xyz-to-uv face p)
|
|
|
+ (values face u v))))
|
|
|
+
|
|
|
+(defparameter +lookup-bits+ 4)
|
|
|
+(defvar *lookup-pos* (make-array (ash 1 (+ 2 (* +lookup-bits+ 2))) :element-type '(unsigned-byte 16)))
|
|
|
+(defvar *lookup-ij* (make-array (ash 1 (+ 2 (* +lookup-bits+ 2))) :element-type '(unsigned-byte 16)))
|
|
|
+(defparameter +ij-to-pos+
|
|
|
+ #(#(0 1 3 2) ; canonical order
|
|
|
+ #(0 3 1 2) ; axes swapped
|
|
|
+ #(2 3 1 0) ; bits inverted
|
|
|
+ #(2 1 3 0)); swapped & inverted
|
|
|
+ "IJtoPos[orientation][ij] -> pos")
|
|
|
+(defparameter +pos-to-ij+
|
|
|
+ #(#(0 1 3 2) ; canonical order: (0,0), (0,1), (1,1), (1,0)
|
|
|
+ #(0 2 3 1) ; axes swapped: (0,0), (1,0), (1,1), (0,1)
|
|
|
+ #(3 2 0 1) ; bits inverted: (1,1), (1,0), (0,0), (0,1)
|
|
|
+ #(3 1 0 2)); swapped & inverted: (1,1), (0,1), (0,0), (1,0)
|
|
|
+ "PosToIJ[orientation][pos] -> ij")
|
|
|
+(defparameter +pos-to-orient+
|
|
|
+ (make-array 4 :element-type '(unsigned-byte 8)
|
|
|
+ :initial-contents (list +swap-mask+ 0 0 (logior +swap-mask+ +invert-mask+)))
|
|
|
+ "PosToOrientation[pos] -> orientation_modifier")
|
|
|
+
|
|
|
+(defun init-lookup ()
|
|
|
+ (labels ((init-lookup-cell (level i j orig-orient pos orient)
|
|
|
+ (if (= level +lookup-bits+)
|
|
|
+ (let ((ij (+ (ash i +lookup-bits+) j)))
|
|
|
+ (setf (aref *lookup-pos* (+ (ash ij 2) orig-orient)) (+ (ash pos 2) orient))
|
|
|
+ (setf (aref *lookup-ij* (+ (ash pos 2) orig-orient)) (+ (ash ij 2) orient)))
|
|
|
+ (let ((level (1+ level))
|
|
|
+ (i (ash i 1))
|
|
|
+ (j (ash j 1))
|
|
|
+ (pos (ash pos 2))
|
|
|
+ (r (aref +pos-to-ij+ orient)))
|
|
|
+ (init-lookup-cell level (+ i (ash (aref r 0) -1)) (+ j (logand (aref r 0) 1))
|
|
|
+ orig-orient pos (logxor orient (aref +pos-to-orient+ 0)))
|
|
|
+ (init-lookup-cell level (+ i (ash (aref r 1) -1)) (+ j (logand (aref r 1) 1))
|
|
|
+ orig-orient (+ pos 1) (logxor orient (aref +pos-to-orient+ 1)))
|
|
|
+ (init-lookup-cell level (+ i (ash (aref r 2) -1)) (+ j (logand (aref r 2) 1))
|
|
|
+ orig-orient (+ pos 2) (logxor orient (aref +pos-to-orient+ 2)))
|
|
|
+ (init-lookup-cell level (+ i (ash (aref r 3) -1)) (+ j (logand (aref r 3) 1))
|
|
|
+ orig-orient (+ pos 3) (logxor orient (aref +pos-to-orient+ 3)))))))
|
|
|
+ (init-lookup-cell 0 0 0 0 0 0)
|
|
|
+ (init-lookup-cell 0 0 0 +swap-mask+ 0 +swap-mask+)
|
|
|
+ (init-lookup-cell 0 0 0 +invert-mask+ 0 +invert-mask+)
|
|
|
+ (init-lookup-cell 0 0 0 (logior +swap-mask+ +invert-mask+) 0 (logior +swap-mask+ +invert-mask+))))
|
|
|
+
|
|
|
+(eval-when (:execute :compile-toplevel :load-toplevel)
|
|
|
+ (init-lookup))
|
|
|
+
|
|
|
+(defun from-face-ij (face i j)
|
|
|
+ (let ((n (ash face (1- +pos-bits+)))
|
|
|
+ (bits (logand face +swap-mask+)))
|
|
|
+ (macrolet ((get-bits (k)
|
|
|
+ `(let ((mask (1- (ash 1 +lookup-bits+))))
|
|
|
+ (setf bits
|
|
|
+ (aref *lookup-pos*
|
|
|
+ (+ bits
|
|
|
+ (ash (logand mask (ash i (- (* ,k +lookup-bits+)))) (+ +lookup-bits+ 2))
|
|
|
+ (ash (logand mask (ash j (- (* ,k +lookup-bits+)))) 2))))
|
|
|
+ (setf n (logior n (ash (ash bits -2)
|
|
|
+ (* ,k 2 +lookup-bits+)))
|
|
|
+ bits (logand bits (logior +swap-mask+ +invert-mask+))))))
|
|
|
+ (get-bits 7)
|
|
|
+ (get-bits 6)
|
|
|
+ (get-bits 5)
|
|
|
+ (get-bits 4)
|
|
|
+ (get-bits 3)
|
|
|
+ (get-bits 2)
|
|
|
+ (get-bits 1)
|
|
|
+ (get-bits 0))
|
|
|
+ (1+ (* 2 n))))
|
|
|
+
|
|
|
+(defun point-to-cell (p)
|
|
|
+ (multiple-value-bind (face u v)
|
|
|
+ (xyz-to-face-uv p)
|
|
|
+ (from-face-ij face
|
|
|
+ (st-to-ij (uv-to-st u))
|
|
|
+ (st-to-ij (uv-to-st v)))))
|
|
|
+
|
|
|
+(defun cell-to-face-ij-orientation (id)
|
|
|
+ (let* ((i 0)
|
|
|
+ (j 0)
|
|
|
+ (face (cell-face id))
|
|
|
+ (bits (logand face +swap-mask+)))
|
|
|
+ (macrolet ((get-bits (k)
|
|
|
+ `(let ((nbits (if (= ,k 7) (- +max-level+ (* 7 +lookup-bits+)) +lookup-bits+)))
|
|
|
+ (setf bits (aref *lookup-ij*
|
|
|
+ (+ bits
|
|
|
+ (ash (logand (ash id (- (1+ (* ,k 2 +lookup-bits+))))
|
|
|
+ (1- (ash 1 (* 2 nbits)))) 2))))
|
|
|
+ (setf i (+ i (ash (ash bits (- (+ +lookup-bits+ 2)))
|
|
|
+ (* ,k +lookup-bits+)))
|
|
|
+ j (+ j (ash (logand (ash bits -2)
|
|
|
+ (1- (ash 1 +lookup-bits+)))
|
|
|
+ (* ,k +lookup-bits+)))
|
|
|
+ bits (logand bits (logior +swap-mask+ +invert-mask+))))))
|
|
|
+ (get-bits 7)
|
|
|
+ (get-bits 6)
|
|
|
+ (get-bits 5)
|
|
|
+ (get-bits 4)
|
|
|
+ (get-bits 3)
|
|
|
+ (get-bits 2)
|
|
|
+ (get-bits 1)
|
|
|
+ (get-bits 0)
|
|
|
+ (values face i j (logxor bits (if (zerop (logand (cell-lsb id) #x1111111111111110)) 0 +swap-mask+))))))
|
|
|
+
|
|
|
+(defun cell-get-center-face-si-ti (id)
|
|
|
+ (multiple-value-bind (face i j orient) (cell-to-face-ij-orientation id)
|
|
|
+ (declare (ignore orient))
|
|
|
+ (let ((delta (if (cell-is-leaf id) 1 (if (zerop (logand 1 (logxor i (ash id -2)))) 0 2))))
|
|
|
+ (values face (+ delta (* 2 i)) (+ delta (* 2 j))))))
|
|
|
+
|
|
|
+(defun face-si-ti-to-xyz (face si ti)
|
|
|
+ (face-uv-ti-xyz face
|
|
|
+ (st-to-uv (si-ti-to-st si))
|
|
|
+ (st-to-uv (si-ti-to-st ti))))
|
|
|
+
|
|
|
+(defun face-uv-ti-xyz (face u v)
|
|
|
+ (declare (type double-float u v))
|
|
|
+ (ecase face
|
|
|
+ (0 (make-point :x 1d0 :y u :z v))
|
|
|
+ (1 (make-point :x (- u) :y 1d0 :z v))
|
|
|
+ (2 (make-point :x (- u) :y (- v) :z 1d0))
|
|
|
+ (3 (make-point :x -1d0 :y (- v) :z (- u)))
|
|
|
+ (4 (make-point :x v :y -1d0 :z (- u)))
|
|
|
+ (5 (make-point :x v :y u :z -1d0))))
|
|
|
+
|
|
|
+(defun si-ti-to-st (si)
|
|
|
+ (assert (<= si +max-si-ti+))
|
|
|
+ (* si (/ 1d0 +max-si-ti+)))
|
|
|
+
|
|
|
+(defun cell-to-point-raw (id)
|
|
|
+ (apply #'face-si-ti-to-xyz (multiple-value-list (cell-get-center-face-si-ti id))))
|
|
|
+
|
|
|
+(defun find-lsb-set (num)
|
|
|
+ (loop for i upto 63 when (logbitp i num) return i))
|
|
|
+
|
|
|
+(defun find-msb-set (num)
|
|
|
+ (loop for i from 63 downto 0 when (logbitp i num) return i))
|
|
|
+
|
|
|
+(defun lsb-for-level (level)
|
|
|
+ (ash 1 (* 2 (- +max-level+ level))))
|
|
|
+
|
|
|
+(defun cell-face (id)
|
|
|
+ (ash id (- +pos-bits+)))
|
|
|
+
|
|
|
+(defun cell-lsb (id)
|
|
|
+ (logand id (1+ (lognot id))))
|
|
|
+
|
|
|
+(defun cell-level (id)
|
|
|
+ (assert (not (zerop id)))
|
|
|
+ (- +max-level+ (ash (find-lsb-set id) -1)))
|
|
|
+
|
|
|
+(defun cell-pos (id)
|
|
|
+ (logand id (lognot 0)))
|
|
|
+
|
|
|
+(defun cell-is-valid (id)
|
|
|
+ (and (< (cell-face id) +num-faces+)
|
|
|
+ (not (zerop (logand (cell-lsb id) #x1555555555555555)))))
|
|
|
+
|
|
|
+(defun cell-child-position (id &optional level)
|
|
|
+ (unless level (setf level (cell-level id)))
|
|
|
+ (assert (cell-is-valid id))
|
|
|
+ (assert (>= level 1))
|
|
|
+ (assert (<= level (cell-level id)))
|
|
|
+ (logand 3 (ash id (- (1+ (* 2 (- +max-level+ level)))))))
|
|
|
+
|
|
|
+(defun cell-range-min (id)
|
|
|
+ (- id (1- (cell-lsb id)) ))
|
|
|
+(defun cell-range-max (id)
|
|
|
+ (+ id (1- (cell-lsb id))))
|
|
|
+(defun cell-is-leaf (id)
|
|
|
+ (not (zerop (logand id 1))))
|
|
|
+(defun cell-is-face (id)
|
|
|
+ (zerop (logand id (1- (lsb-for-level 0)))))
|
|
|
+
|
|
|
+(defun cell-parent (id &optional level)
|
|
|
+ (assert (cell-is-valid id))
|
|
|
+ (if level (progn
|
|
|
+ (assert (>= level 0))
|
|
|
+ (assert (<= level (cell-level id))))
|
|
|
+ (progn (assert (not (cell-is-face id)))))
|
|
|
+ (let ((new-lsb (if level (lsb-for-level level) (ash (cell-lsb id) 2))))
|
|
|
+ (logior new-lsb (logand id (1+ (lognot new-lsb))))))
|
|
|
+
|
|
|
+(defun cell-child (id pos)
|
|
|
+ (assert (cell-is-valid id))
|
|
|
+ (assert (not (cell-is-leaf id)))
|
|
|
+ (+ id (* (+ (* 2 pos) 1 -4) (ash (cell-lsb id) -2))))
|
|
|
+
|
|
|
+(defun cell-to-string (id)
|
|
|
+ (format nil "~a/~{~a~}" (cell-face id)
|
|
|
+ (loop for level from 1 to (cell-level id)
|
|
|
+ collect (cell-child-position id level))))
|
|
|
+
|
|
|
+;;;;;;;;;
|
|
|
+;;;
|
|
|
+(defun make-span (vector offset &optional size)
|
|
|
+ (declare (type vector vector))
|
|
|
+ (let ((length (length vector)))
|
|
|
+ (assert (< offset length))
|
|
|
+ (when size (assert (< (+ offset size) length)))
|
|
|
+ (make-array (if size size (- length offset))
|
|
|
+ :element-type (or (cadr (type-of vector)) t)
|
|
|
+ :displaced-to vector
|
|
|
+ :displaced-index-offset offset)))
|
|
|
+
|
|
|
+(deftype byte-vector () `(vector (unsigned-byte 8)))
|
|
|
+
|
|
|
+(defun make-encoder (size)
|
|
|
+ (make-array size :element-type '(unsigned-byte 8) :fill-pointer 0 :adjustable t))
|
|
|
+
|
|
|
+(defun encoder-avail (encoder)
|
|
|
+ (declare (type byte-vector encoder))
|
|
|
+ (- (array-total-size encoder)
|
|
|
+ (fill-pointer encoder)))
|
|
|
+
|
|
|
+(defun encoder-ensure (encoder size)
|
|
|
+ (declare (type byte-vector encoder))
|
|
|
+ (when (< (encoder-avail encoder) size)
|
|
|
+ (adjust-array encoder (+ (fill-pointer encoder) size))))
|
|
|
+
|
|
|
+(defun encoder-put8 (encoder value)
|
|
|
+ (declare (type byte-vector encoder)
|
|
|
+ (type (unsigned-byte 8) value))
|
|
|
+ (assert (> (encoder-avail encoder) 1))
|
|
|
+ (vector-push value encoder)
|
|
|
+ encoder)
|
|
|
+
|
|
|
+(defun encoder-putn (encoder vector)
|
|
|
+ (declare (type byte-vector encoder)
|
|
|
+ (type byte-vector vector))
|
|
|
+ (loop for value across vector do (vector-push value encoder))
|
|
|
+ encoder)
|
|
|
+
|
|
|
+(defun encoder-length (encoder)
|
|
|
+ (declare (type byte-vector encoder))
|
|
|
+ (fill-pointer encoder))
|
|
|
+
|
|
|
+(defparameter +varint-max32+ 5 "Maximum varint encoding length for uint32")
|
|
|
+(defun encoder-put-varint32 (encoder v)
|
|
|
+ (declare (type byte-vector encoder)
|
|
|
+ (type (unsigned-byte 32) v))
|
|
|
+ (let ((b 128))
|
|
|
+ (cond
|
|
|
+ ((< v (ash 1 7)) (encoder-put8 encoder v))
|
|
|
+ ((< v (ash 1 14))
|
|
|
+ (encoder-put8 encoder (logand 255 (logior v b)))
|
|
|
+ (encoder-put8 encoder (logand 255 (ash v -7))))
|
|
|
+ ((< v (ash 1 21))
|
|
|
+ (encoder-put8 encoder (logand 255 (logior v b)))
|
|
|
+ (encoder-put8 encoder (logand 255 (logior (ash v -7) b)))
|
|
|
+ (encoder-put8 encoder (logand 255 (ash v -14))))
|
|
|
+ ((< v (ash 1 28))
|
|
|
+ (encoder-put8 encoder (logand 255 (logior v b)))
|
|
|
+ (encoder-put8 encoder (logand 255 (logior (ash v -7) b)))
|
|
|
+ (encoder-put8 encoder (logand 255 (logior (ash v -14) b)))
|
|
|
+ (encoder-put8 encoder (logand 255 (ash v -21))))
|
|
|
+ (t
|
|
|
+ (encoder-put8 encoder (logand 255 (logior v b)))
|
|
|
+ (encoder-put8 encoder (logand 255 (logior (ash v -7) b)))
|
|
|
+ (encoder-put8 encoder (logand 255 (logior (ash v -14) b)))
|
|
|
+ (encoder-put8 encoder (logand 255 (logior (ash v -21) b)))
|
|
|
+ (encoder-put8 encoder (logand 255 (ash v -28))))))
|
|
|
+ encoder)
|
|
|
+
|
|
|
+(defparameter +varint-max64+ 10 "Maximum varint encoding length for uint64")
|
|
|
+(defun encoder-put-varint64 (encoder v)
|
|
|
+ (declare (type byte-vector encoder)
|
|
|
+ (type (unsigned-byte 64) v))
|
|
|
+ (if (< v (ash 1 28))
|
|
|
+ (encoder-put-varint32 encoder v)
|
|
|
+ (let ((x32 (logand (1- (ash 1 32)) (logior v (ash 1 7) (ash 1 21))))
|
|
|
+ (y32 (logand (1- (ash 1 32)) (logior v (ash 1 14) (ash 1 28)))))
|
|
|
+ (encoder-put8 encoder (logand 255 x32))
|
|
|
+ (encoder-put8 encoder (logand 255 (ash y32 -7)))
|
|
|
+ (encoder-put8 encoder (logand 255 (ash x32 -14)))
|
|
|
+ (encoder-put8 encoder (logand 255 (ash y32 -21)))
|
|
|
+ (if (< v (ash 1 35))
|
|
|
+ (encoder-put8 encoder (logand 255 (ash v -28)))
|
|
|
+ (progn
|
|
|
+ (encoder-put8 encoder (logand 255 (logior (ash v -28) (ash 1 7))))
|
|
|
+ (encoder-put-varint32 encoder (ash v -35)))))))
|
|
|
+
|
|
|
+;;; decoder ;;;
|
|
|
+(defun make-decoder (size &key (fill-pointer 0) initial-contents initial-element displaced-to)
|
|
|
+ (apply #'make-array size
|
|
|
+ :element-type '(unsigned-byte 8)
|
|
|
+ :fill-pointer fill-pointer
|
|
|
+ (append
|
|
|
+ (when initial-contents `(:initial-contents ,initial-contents))
|
|
|
+ (when initial-element `(:initial-element ,initial-element))
|
|
|
+ (when displaced-to `(:displaced-to ,displaced-to :displaced-index-offset ,(fill-pointer displaced-to))))))
|
|
|
+
|
|
|
+(defun decoder-avail (decoder)
|
|
|
+ (declare (type byte-vector decoder))
|
|
|
+ (- (array-total-size decoder)
|
|
|
+ (fill-pointer decoder)))
|
|
|
+
|
|
|
+(defun decoder-skip (decoder &optional (count 1))
|
|
|
+ (declare (type byte-vector decoder)
|
|
|
+ (type (unsigned-byte 32) count))
|
|
|
+ (incf (fill-pointer decoder) count))
|
|
|
+
|
|
|
+(defun decoder-reset (decoder)
|
|
|
+ (declare (type byte-vector decoder))
|
|
|
+ (setf (fill-pointer decoder) 0)
|
|
|
+ decoder)
|
|
|
+
|
|
|
+(defun make-sub-decoder (decoder size &optional will-decode)
|
|
|
+ (when (>= (decoder-avail decoder) size)
|
|
|
+ (prog1 (make-decoder size :fill-pointer (if will-decode 0 size) :displaced-to decoder)
|
|
|
+ (decoder-skip decoder size))))
|
|
|
+
|
|
|
+(defmacro with-decoder-reset ((decoder &optional (offset 0)) &body body)
|
|
|
+ (let ((fp (gensym "FP"))
|
|
|
+ (dc (gensym "DC")))
|
|
|
+ `(let* ((,dc ,decoder)
|
|
|
+ (,fp (fill-pointer ,dc)))
|
|
|
+ (unwind-protect
|
|
|
+ (progn
|
|
|
+ (setf (fill-pointer ,dc) ,offset)
|
|
|
+ ,@body)
|
|
|
+ (setf (fill-pointer ,dc) ,fp)))))
|
|
|
+
|
|
|
+(defun decoder-get8 (decoder)
|
|
|
+ (declare (type byte-vector decoder))
|
|
|
+ (prog1 (aref decoder (fill-pointer decoder))
|
|
|
+ (decoder-skip decoder)))
|
|
|
+
|
|
|
+(defun decoder-peek (decoder &optional (offset 0))
|
|
|
+ (declare (type byte-vector decoder)
|
|
|
+ (type (unsigned-byte 32) offset))
|
|
|
+ (aref decoder (+ (fill-pointer decoder) offset)))
|
|
|
+
|
|
|
+(defun decoder-get-varint64 (decoder)
|
|
|
+ (declare (type byte-vector decoder))
|
|
|
+ (loop
|
|
|
+ for b = (decoder-get8 decoder) then (decoder-get8 decoder)
|
|
|
+ for i from 0
|
|
|
+ with result = 0
|
|
|
+ do (incf result (ash (if (zerop i) b (1- b)) (* i 7)))
|
|
|
+ until (< b 128)
|
|
|
+ finally (return result)))
|
|
|
+
|
|
|
+;;;
|
|
|
+;;; encoded-uint-vector ;;;
|
|
|
+;;;
|
|
|
+(defun encode-uint-with-length (value length encoder)
|
|
|
+ (declare (type integer value)
|
|
|
+ (type (integer 0 8) length)
|
|
|
+ (type byte-vector encoder))
|
|
|
+ (assert (>= (encoder-avail encoder) length))
|
|
|
+ (loop repeat length
|
|
|
+ do (encoder-put8 encoder (logand value 255))
|
|
|
+ do (setf value (ash value -8)))
|
|
|
+ (assert (zerop value))
|
|
|
+ (values))
|
|
|
+
|
|
|
+(defun get-uint-with-length (decoder length)
|
|
|
+ (declare (type byte-vector decoder)
|
|
|
+ (type (integer 0 8) length))
|
|
|
+ (let ((value 0))
|
|
|
+ (loop for i from (1- length) downto 0
|
|
|
+ do (setf value (+ (ash value 8)
|
|
|
+ (decoder-peek decoder i))))
|
|
|
+ (decoder-skip decoder length)
|
|
|
+ value))
|
|
|
+
|
|
|
+(defun get-type-byte-size (v-type)
|
|
|
+ (values
|
|
|
+ (typecase v-type
|
|
|
+ (list (case (car v-type)
|
|
|
+ ((array simple-array vector) (get-type-byte-size (cadr v-type)))
|
|
|
+ ((unsigned-byte signed-byte) (ceiling (cadr v-type) 8))
|
|
|
+ (integer (when (cadr v-type) (ceiling (log (cadr v-type) 2) 8)))
|
|
|
+ (t nil)))
|
|
|
+ (symbol (when (eql v-type 'fixnum) 8))
|
|
|
+ (t nil))))
|
|
|
+
|
|
|
+(defun encode-uint-vector (v encoder)
|
|
|
+ (declare (type vector v)
|
|
|
+ (type byte-vector encoder))
|
|
|
+ (let ((length (1+ (ash (find-msb-set
|
|
|
+ (loop
|
|
|
+ for value across v
|
|
|
+ with bits = 1
|
|
|
+ do (setf bits (logior bits value))
|
|
|
+ finally (return bits)))
|
|
|
+ -3))))
|
|
|
+ (assert (<= 1 length 8))
|
|
|
+ (encoder-ensure encoder (+ (* (length v) length)
|
|
|
+ +varint-max64+))
|
|
|
+ (encoder-put-varint64 encoder
|
|
|
+ (logior (* (length v) (get-type-byte-size (type-of v)))
|
|
|
+ (1- length)))
|
|
|
+ (loop for value across v
|
|
|
+ do (encode-uint-with-length value length encoder))
|
|
|
+ encoder))
|
|
|
+
|
|
|
+(defstruct encoded-uint-vector
|
|
|
+ (size 0 :type (unsigned-byte 32))
|
|
|
+ (length 1 :type (integer 1 8))
|
|
|
+ (bytes 8 :type (member 1 2 4 8))
|
|
|
+ (data nil :type byte-vector))
|
|
|
+
|
|
|
+(defun encoded-uint-vector-init (decoder &optional (bytes 8))
|
|
|
+ (declare (type byte-vector decoder)
|
|
|
+ (type (member 1 2 4 8) bytes))
|
|
|
+ (let ((size-len (decoder-get-varint64 decoder)))
|
|
|
+ (when size-len
|
|
|
+ (let* ((size (floor size-len bytes))
|
|
|
+ (length (1+ (logand size-len (1- bytes))))
|
|
|
+ (bytes-length (* size length))
|
|
|
+ (data (make-sub-decoder decoder bytes-length)))
|
|
|
+ (when data
|
|
|
+ (make-encoded-uint-vector :size size :length length :bytes bytes
|
|
|
+ :data data))))))
|
|
|
+
|
|
|
+(defun encoded-uint-vector-get (euv pos)
|
|
|
+ (declare (type encoded-uint-vector euv)
|
|
|
+ (type (unsigned-byte 32) pos))
|
|
|
+ (with-slots (size data length) euv
|
|
|
+ (assert (< pos size))
|
|
|
+ (with-decoder-reset (data (* pos length))
|
|
|
+ (get-uint-with-length data length))))
|
|
|
+
|
|
|
+(defun encoded-uint-vector-decode (euv)
|
|
|
+ (declare (type encoded-uint-vector euv))
|
|
|
+ (let* ((size (encoded-uint-vector-size euv))
|
|
|
+ (result (make-array size :element-type '(unsigned-byte 64))))
|
|
|
+ (dotimes (i size result)
|
|
|
+ (setf (aref result i) (encoded-uint-vector-get euv i)))))
|
|
|
+
|
|
|
+;; encoded-string-vector
|
|
|
+(defstruct string-vector-encoder
|
|
|
+ (offsets (make-array 0 :element-type '(unsigned-byte 64) :adjustable t :fill-pointer 0)
|
|
|
+ :type (vector (unsigned-byte 64)))
|
|
|
+ (data (make-encoder 0) :type byte-vector))
|
|
|
+
|
|
|
+(defun string-vector-encoder-add-via-encoder (sve)
|
|
|
+ (declare (type string-vector-encoder sve))
|
|
|
+ (with-slots (offsets data) sve
|
|
|
+ (vector-push-extend (length data) offsets)
|
|
|
+ data))
|
|
|
+
|
|
|
+(defun string-vector-encoder-size (sve)
|
|
|
+ (declare (type string-vector-encoder sve))
|
|
|
+ (length (string-vector-encoder-offsets sve)))
|
|
|
+
|
|
|
+(defun string-vector-encoder-encode (sve encoder)
|
|
|
+ (declare (type string-vector-encoder sve)
|
|
|
+ (type byte-vector encoder))
|
|
|
+ (with-slots (offsets data) sve
|
|
|
+ (vector-push-extend (length data) offsets)
|
|
|
+ (encode-uint-vector (make-span offsets 1) encoder)
|
|
|
+ (encoder-ensure encoder (encoder-length data))
|
|
|
+ (encoder-putn encoder data)
|
|
|
+ encoder))
|
|
|
+
|
|
|
+(defstruct encoded-string-vector
|
|
|
+ (offsets nil :type encoded-uint-vector)
|
|
|
+ (data nil :type byte-vector))
|
|
|
+
|
|
|
+(defun encoded-string-vector-init (decoder)
|
|
|
+ (declare (type byte-vector decoder))
|
|
|
+ (let ((offsets (encoded-uint-vector-init decoder 8)))
|
|
|
+ (when offsets
|
|
|
+ (let* ((size (encoded-uint-vector-size offsets))
|
|
|
+ (length (if (zerop size) 0 (encoded-uint-vector-get offsets (1- size))))
|
|
|
+ (data (make-sub-decoder decoder length)))
|
|
|
+ (when data
|
|
|
+ (make-encoded-string-vector :offsets offsets :data data))))))
|
|
|
+
|
|
|
+(defun encoded-string-vector-size (esv)
|
|
|
+ (declare (type encoded-string-vector esv))
|
|
|
+ (encoded-uint-vector-size (encoded-string-vector-offsets esv)))
|
|
|
+
|
|
|
+(defun encoded-string-vector-get (esv index &optional fill-pointer)
|
|
|
+ (declare (type encoded-string-vector esv)
|
|
|
+ (type (unsigned-byte 32) index))
|
|
|
+ (with-slots (offsets data) esv
|
|
|
+ (let* ((start (if (zerop index) 0 (encoded-uint-vector-get offsets (1- index))))
|
|
|
+ (length (- (encoded-uint-vector-get offsets index) start)))
|
|
|
+ (with-decoder-reset (data start)
|
|
|
+ (make-decoder length :displaced-to data :fill-pointer fill-pointer)))))
|
|
|
+
|
|
|
+(defun encoded-string-vector-decode (esv)
|
|
|
+ (declare (type encoded-string-vector esv))
|
|
|
+ (let* ((size (encoded-string-vector-size esv))
|
|
|
+ (result (make-array size :element-type '(vector (unsigned-byte 8)))))
|
|
|
+ (dotimes (i size result)
|
|
|
+ (setf (aref result i) (encoded-string-vector-get esv i)))))
|
|
|
+
|
|
|
+;; encoded-cell-id-vector
|
|
|
+(defstruct encoded-cell-id-vector
|
|
|
+ (deltas nil :type encoded-uint-vector)
|
|
|
+ (base 0 :type (unsigned-byte 64))
|
|
|
+ (shift 0 :type (unsigned-byte 8)))
|
|
|
+
|
|
|
+(defun encoded-cell-id-vector-size (ecv)
|
|
|
+ (declare (type encoded-cell-id-vector ecv))
|
|
|
+ (with-slots (deltas) ecv
|
|
|
+ (when deltas
|
|
|
+ (encoded-uint-vector-size deltas))))
|
|
|
+
|
|
|
+(defun encoded-cell-id-vector-get (ecv i)
|
|
|
+ (declare (type encoded-cell-id-vector ecv)
|
|
|
+ (type (unsigned-byte 32) i))
|
|
|
+ (with-slots (deltas base shift) ecv
|
|
|
+ (+ base
|
|
|
+ (ash (encoded-uint-vector-get deltas i) shift))))
|
|
|
+
|
|
|
+(defun encoded-cell-id-vector-init (decoder)
|
|
|
+ (declare (type byte-vector decoder))
|
|
|
+ (when (>= (decoder-avail decoder) 2)
|
|
|
+ (let* ((code+len (decoder-get8 decoder))
|
|
|
+ (shift-code (ash code+len -3)))
|
|
|
+ (when (= shift-code 31)
|
|
|
+ (setf shift-code (+ 29 (decoder-get8 decoder))))
|
|
|
+ (let* ((base-len (logand code+len 7))
|
|
|
+ (base (get-uint-with-length decoder base-len))
|
|
|
+ shift)
|
|
|
+ (when base
|
|
|
+ (setf base (ash base (- 64 (* 8 (max 1 base-len)))))
|
|
|
+ (if (>= shift-code 29)
|
|
|
+ (setf shift (1+ (* 2 (- shift-code 29)))
|
|
|
+ base (logior base (ash 1 (1- shift))))
|
|
|
+ (setf shift (* 2 shift-code)))
|
|
|
+ (make-encoded-cell-id-vector
|
|
|
+ :deltas (encoded-uint-vector-init decoder 8)
|
|
|
+ :base base
|
|
|
+ :shift shift))))))
|
|
|
+
|
|
|
+(defun encoded-cell-id-vector-decode (eciv)
|
|
|
+ (declare (type encoded-cell-id-vector eciv))
|
|
|
+ (let* ((size (encoded-cell-id-vector-size eciv))
|
|
|
+ (result (make-array size :element-type '(unsigned-byte 64))))
|
|
|
+ (dotimes (i size result)
|
|
|
+ (setf (aref result i) (encoded-cell-id-vector-get eciv i)))))
|
|
|
+
|
|
|
+(defun encode-cell-id-vector (v encoder)
|
|
|
+ (declare (type (vector (unsigned-byte 64)) v)
|
|
|
+ (type byte-vector encoder))
|
|
|
+ (destructuring-bind (v-or v-and v-min v-max)
|
|
|
+ (loop for cell-id across v
|
|
|
+ with v-or = 0
|
|
|
+ with v-and = -1
|
|
|
+ do (setf v-or (logior v-or cell-id)
|
|
|
+ v-and (logand v-and cell-id))
|
|
|
+ maximizing cell-id into v-max
|
|
|
+ minimizing cell-id into v-min
|
|
|
+ finally (return (list v-or v-and v-min v-max)))
|
|
|
+ (let ((e-base 0)
|
|
|
+ (e-base-len 0)
|
|
|
+ (e-shift 0)
|
|
|
+ (e-max-delta-msb 0))
|
|
|
+ (when (> v-or 0)
|
|
|
+ (setf e-shift (min 56 (dpb 0 (byte 1 0) (find-lsb-set v-or))))
|
|
|
+ (unless (zerop (logand v-and (ash 1 e-shift)))
|
|
|
+ (incf e-shift))
|
|
|
+ (let ((e-bytes (1- (ash 1 65))))
|
|
|
+ (dotimes (len 8)
|
|
|
+ (let* ((t-base (mask-field (byte (* 8 len) (- 64 (* 8 len))) v-min))
|
|
|
+ (t-max-delta-msb (max 0 (1- (integer-length (ash (- v-max t-base)
|
|
|
+ (- e-shift))))))
|
|
|
+ (t-bytes (+ len (* (length v) (1+ (ash t-max-delta-msb -3))))))
|
|
|
+ (when (< t-bytes e-bytes)
|
|
|
+ (setf e-base t-base
|
|
|
+ e-base-len len
|
|
|
+ e-max-delta-msb t-max-delta-msb
|
|
|
+ e-bytes t-bytes)))))
|
|
|
+ (when (and (logbitp 0 e-shift)
|
|
|
+ (not (= (logand e-max-delta-msb 7) 7)))
|
|
|
+ (decf e-shift)))
|
|
|
+ (assert (<= e-base-len 7))
|
|
|
+ (assert (<= e-shift 56))
|
|
|
+ (encoder-ensure encoder (+ 2 e-base-len))
|
|
|
+
|
|
|
+ (let ((shift-code (ash e-shift -1)))
|
|
|
+ (when (logbitp 0 e-shift)
|
|
|
+ (setf shift-code (min 31 (+ shift-code 29))))
|
|
|
+ (encoder-put8 encoder (logior (ash shift-code 3)
|
|
|
+ e-base-len))
|
|
|
+ (when (= shift-code 31)
|
|
|
+ (encoder-put8 encoder (ash e-shift -1))))
|
|
|
+
|
|
|
+ (encode-uint-with-length
|
|
|
+ (ash e-base (- (- 64 (* 8 (max 1 e-base-len))))) e-base-len encoder)
|
|
|
+
|
|
|
+ (let ((deltas (make-array (length v) :element-type '(unsigned-byte 64))))
|
|
|
+ (loop for cell-id across v
|
|
|
+ for i from 0
|
|
|
+ do (setf (aref deltas i)
|
|
|
+ (ash (- cell-id e-base) (- e-shift))))
|
|
|
+ (encode-uint-vector deltas encoder)))))
|
|
|
+
|
|
|
+;; encoded-point-vector
|
|
|
+(defstruct encoded-point-vector
|
|
|
+ (size 0 :type (unsigned-byte 32))
|
|
|
+ (blocks nil :type encoded-string-vector)
|
|
|
+ (base 0 :type (unsigned-byte 64))
|
|
|
+ (level 0 :type (unsigned-byte 8))
|
|
|
+ (have-exceptions nil :type boolean))
|
|
|
+
|
|
|
+(defstruct cell-point
|
|
|
+ (level 0 :type (unsigned-byte 8))
|
|
|
+ (face 0 :type (unsigned-byte 8))
|
|
|
+ (si 0 :type (unsigned-byte 32))
|
|
|
+ (ti 0 :type (unsigned-byte 32)))
|
|
|
+
|
|
|
+(defparameter +block-shift+ 4)
|
|
|
+(defparameter +block-size+ (ash 1 +block-shift+))
|
|
|
+
|
|
|
+(defparameter +exception+ -1)
|
|
|
+
|
|
|
+(defstruct block-code
|
|
|
+ (delta-bits 0 :type (signed-byte 32))
|
|
|
+ (offset-bits 0 :type (signed-byte 32))
|
|
|
+ (overlap-bits 0 :type (signed-byte 32)))
|
|
|
+
|
|
|
+(defun bit-mask (n)
|
|
|
+ (declare (type (integer 0 64) n))
|
|
|
+ (if (zerop n) 0
|
|
|
+ (dpb -1 (byte n 0) 0)))
|
|
|
+
|
|
|
+(defun max-bits-for-level (level)
|
|
|
+ (declare (type (integer 0 30) level))
|
|
|
+ (+ (* 2 level) 3))
|
|
|
+
|
|
|
+(defun base-shift (level base-bits)
|
|
|
+ (declare (type (integer 0 30) level)
|
|
|
+ (type (integer 0 64) base-bits))
|
|
|
+ (max 0 (- (max-bits-for-level level) base-bits)))
|
|
|
+
|
|
|
+(defun encode-point-vector-compact (points encoder)
|
|
|
+ (declare (type (vector point) points)
|
|
|
+ (type byte-vector encoder))
|
|
|
+ (multiple-value-bind (level cell-points)
|
|
|
+ (choose-best-level points)
|
|
|
+ (when (< level 0)
|
|
|
+ (return (encode-point-vector-fast points encoder)))
|
|
|
+ (multiple-value-bind (values have-exceptions) (convert-cells-to-values cell-points level)
|
|
|
+ (multiple-value-bind (base base-bits) (choose-base values level have-exceptions)
|
|
|
+ (let ((num-blocks (ash (+ (length values) +block-size+ -1) (- +block-shift+)))
|
|
|
+ (base-bytes (ash base-bits -3)))
|
|
|
+ (encoder-ensure encoder (+ 2 base-bytes))
|
|
|
+ (let ((last-block-count (- (length values) (* +block-size+ (1- num-blocks))))
|
|
|
+ (blocks (make-string-vector-encoder))
|
|
|
+ (exceptions (make-array 0 :element-type point :adjustable t)))
|
|
|
+ (assert (>= last-block-count 0))
|
|
|
+ (assert (<= last-block-count +block-size+))
|
|
|
+ (assert (<= base-bytes 7))
|
|
|
+ (assert (<= level 30))
|
|
|
+ (encoder-put8 (logior +encoded-point-vector-cell-ids+
|
|
|
+ (ash have-exceptions 3)
|
|
|
+ (ash (1- last-block-count) 4)))
|
|
|
+ (encoder-put8 (logior base-bytes
|
|
|
+ (ash level 3)))
|
|
|
+ (encode-uint-with-length (ash base (- (base-shift level base-bits))) base-bytes encoder)
|
|
|
+
|
|
|
+ (loop for i from 0 to (length values) step +block-size+
|
|
|
+ for block-size = (min +block-size+ (- (length values) i))
|
|
|
+ for code = (get-block-code values i block-size base have-exceptions)
|
|
|
+ do (let ((block (string-vector-encoder-add-via-encoder blocks))
|
|
|
+ (offset-bytes (ash (block-code-offset-bits code) -3))
|
|
|
+ (delta-nibbles (ash (block-code-delta-bits code) -2))
|
|
|
+ (overlap-nibbles (ash (block-code-overlap-bits code) -2)))
|
|
|
+ (encoder-ensure block (+ 1 offset-bytes (* delta-nibbles (ash +block-size+ -1))))
|
|
|
+ (assert (<= (- offset-bytes overlap-nibbles) 7))
|
|
|
+ (assert (<= overlap-nibbles 1))
|
|
|
+ (assert (<= delta-nibbles 16))
|
|
|
+ (encoder-put8 block (logior (- offset-bytes overlap-nibbles)
|
|
|
+ (ash overlap-nibbles 3)
|
|
|
+ (ash (1- delta-nibbles) 4)))
|
|
|
+
|
|
|
+ (let ((offset (1- (ash 1 65)))
|
|
|
+ (num-exceptions 0))
|
|
|
+ (dotimes (i block-size)
|
|
|
+ (let ((value (aref values (+ i j))))
|
|
|
+ (if (equalp value +exception+)
|
|
|
+ (incf num-exceptions)
|
|
|
+ (progn
|
|
|
+ (assert (>= value base))
|
|
|
+ (setf offset (min offset (- value base)))))))
|
|
|
+ (when (= num-exceptions block-size)
|
|
|
+ (setf offset))
|
|
|
+
|
|
|
+ (let ((offset-shift (- (block-code-delta-bits code)
|
|
|
+ (block-code-overlap-bits code))))
|
|
|
+ (setf offset (logand offset (lognot (bit-mask offset-shift))))
|
|
|
+ (assert (= (zerop offset) (zerop offset-bytes)))
|
|
|
+ (when (> offset 0)
|
|
|
+ (encode-uint-with-length (ash offset (- offset-shift)) offset-bytes block))
|
|
|
+
|
|
|
+ (let ((delta-bytes (ash (1+ delta-nibbles) -1)))
|
|
|
+ (setf (fill-pointer exceptions) 0)
|
|
|
+ (dotimes (j block-size)
|
|
|
+ (let ((delta 0)
|
|
|
+ (value (arerf values (+ i j))))
|
|
|
+ (if (equalp value +exception+)
|
|
|
+ (progn
|
|
|
+ (setf delta (length exceptions))
|
|
|
+ (vector-push-extend (aref points (+ i j)) exceptions))
|
|
|
+ (progn
|
|
|
+ (assert (>= value (+ offset base)))
|
|
|
+ (setf delta (- value (+ offset base)))
|
|
|
+ (when have-exceptions
|
|
|
+ (assert (<= delta (- (1- (ash 1 65)) +block-size+)))
|
|
|
+ (incf delta +block-size+))))
|
|
|
+ (assert (<= delta (bit-mask (block-code-delta-bits delta))))
|
|
|
+ (when (and (logbitp delta-nibbles 0)
|
|
|
+ (logbitp j 0))
|
|
|
+ ())
|
|
|
+ )))
|
|
|
+ ))))
|
|
|
+))))))
|
