nvg.lisp

@@ -1,6 +1,6 @@
 (cl:defpackage :ocm.nvg
-  (:use :cl :cffi-c-ref :alexandria)
-  (:export run))
+     (:use :cl :cffi-c-ref :alexandria)
+     (:export run))
 (cl:in-package :ocm.nvg)
 
 (defun aget (alist key)
@@ -36,31 +36,99 @@
 
 (defvar *objects* nil "List of OSM referenced objects to render")
 
+
+
+(defun fix-multipoly (rel)
+  (labels ((id (obj) (ocm.osm::obj-id obj))
+           (try-attach (way ways)
+             (loop
+                with way-nodes = (ocm.osm::way-refs way)
+                with way-first = (car way-nodes)
+                with way-last = (car (last way-nodes))
+                for prev in ways
+                for prev-nodes = (ocm.osm::way-refs prev)
+                for prev-first = (car prev-nodes)
+                for prev-last = (car (last prev-nodes))
+                when (and way-first way-last prev-first prev-last)
+                do (when-let (nodes (cond
+                                      ((= (id way-first) (id prev-last))
+                                       (append prev-nodes (cdr way-nodes)))
+                                      ((= (id way-last) (id prev-first))
+                                       (append way-nodes (cdr prev-nodes)))
+                                      ((= (id way-first) (id prev-first))
+                                       (append (reverse way-nodes) (cdr prev-nodes)))
+                                      ((= (id way-last) (id prev-last))
+                                       (append prev-nodes (cdr (reverse way-nodes))))
+                                      ))
+                     (setf (ocm.osm::way-refs prev) nodes)
+                     (return prev)))))
+    (loop with inner-ways and outer-ways
+       for mem in (ocm.osm::rel-memids rel)
+       for mem-role in (ocm.osm::rel-roles rel)
+       for mem-type in (ocm.osm::rel-types rel)
+       for parts = (if (equal "inner" mem-role) inner-ways outer-ways)
+       when (ocm.osm::way-p mem)
+       unless (try-attach mem parts)
+       do (let ((new-way (ocm.osm::make-way :id (id mem)
+                                            :refs (copy-list (ocm.osm::way-refs mem)))))
+            (if (equal "inner" mem-role) (push new-way inner-ways) (push new-way outer-ways)))
+       finally (return (ocm.osm::make-rel
+                        :id (id rel) :tags (ocm.osm::obj-tags rel)
+                        :memids  (append outer-ways inner-ways)
+                        :roles (append (loop for w in outer-ways collect "outer")
+                                       (loop for w in inner-ways collect "inner"))
+                        :types (append (loop for w in outer-ways collect :way)
+                                       (loop for w in inner-ways collect :way)))))))
+
+(defun osm-fix (obj)
+  (cond
+    ((multipoly-p obj) (fix-multipoly obj))
+    (:otherwise obj)))
+
 (defun load-test-objects ()
   (setf *objects* nil)
   (ocm.osm::with-related (obj "spb.osm.pbf" '(
                                               (nil "natural" "water")
                                               (nil "natural" "wood")
-                                              (nil "natural" "scrub")))
-    (push obj *objects*)))
+                                              (nil "natural" "scrub")
+                                              (nil "highway" "primary")
+                                              (nil "highway" "secondary")))
+    (push (osm-fix obj) *objects*)))
 
-(defparameter +granularity+ (expt 10 8))
+(defparameter +granularity+ (expt 10 9))
+;; Web mercator
 (defun node-x (node)
-  (float (/ (ocm.osm::node-lon node) +granularity+)))
+  (float
+   (* (expt 2 *zoom*)
+      (/ (+ (/ (ocm.osm::node-lon node) +granularity+) 180) 360))))
 (defun node-y (node)
-  (float (-  (/ (ocm.osm::node-lat node) +granularity+))))
+  (let* ((lat (/ (ocm.osm::node-lat node) +granularity+))
+         (lat-rad (/ (* lat pi) 180)))
+    (float
+     (* (expt 2 (1- *zoom*))
+        (- 1 (/ (log (+ (tan lat-rad) (/ 1 (cos lat-rad))))
+                pi))))))
+
+(defun offset-x (node)
+  (coerce (- (node-x node) (node-x *corner*)) 'single-float))
+(defun offset-y (node)
+  (coerce (- (node-y node) (node-y *corner*)) 'single-float))
 
 (defun render-path (nodes)
   (%nvg:begin-path *context*)
   (let ((start (car nodes)))
-    (%nvg:move-to *context* (node-x start) (node-y start))
+    (%nvg:move-to *context* (offset-x start) (offset-y start))
     (loop for node in (cdr nodes)
-       do (%nvg:line-to *context* (node-x node) (node-y node)))))
+       do (%nvg:line-to *context* (offset-x node) (offset-y node)))))
 
-(defparameter +highway-color+ '(250 0 0 255))
+(defparameter +highway-color+ '(150 100 0 255))
+(defparameter +highway-primary-color+ '(150 30 0 255))
 (defparameter +strokes+
   `(
-    (((:way "highway")) :color ,+highway-color+ :width 10)))
+    (((:way "lanes" "5")) :color ,+highway-primary-color+ :width 0.005)
+    (((:way "lanes" "3")) :color ,+highway-primary-color+ :width 0.004)
+    (((:way "highway" "primary")) :color ,+highway-primary-color+ :width 0.003)
+    (((:way "highway")) :color ,+highway-color+ :width 0.001)))
 
 (defun apply-stroke (tags)
   (when-let (options (find-match +strokes+ tags :way))
@@ -102,26 +170,34 @@
 (defun render-poly (nodes tags &optional hole)
   (render-path nodes)
   (%nvg:close-path *context*)
-  (when hole
-    (%nvg:path-winding *context* (cffi:foreign-bitfield-value '%nvg:solidity :hole)))
-  (apply-fill tags)
+  (if hole
+      (%nvg:path-winding *context* (cffi:foreign-bitfield-value '%nvg:solidity :hole))
+      (apply-fill tags))
   (%nvg:fill *context*))
 
 (defparameter +closed-lines+
   '((:way "highway")))
 
-(defun is-area (nodes tags)
+(defun closed-p (nodes)
   (let ((first (first nodes))
         (last (car (last nodes))))
     (and first last
          (= (if (ocm.osm::obj-p first) (ocm.osm::obj-id first) first)
-            (if (ocm.osm::obj-p last) (ocm.osm::obj-id last) last))
-         (not (ocm.osm::query-match2 +closed-lines+ :way tags)))))
+            (if (ocm.osm::obj-p last) (ocm.osm::obj-id last) last)))))
+
+(defun area-p (nodes tags)
+  (and (closed-p nodes)
+       (not (ocm.osm::query-match2 +closed-lines+ :way tags))))
+
+(defun multipoly-p (rel)
+  (and (ocm.osm::rel-p rel)
+       (equal (aget (tags rel) "type")
+              "multipolygon")))
 
 (defun render-way (nodes tags)
-  (if (is-area nodes tags)
+  (if (area-p nodes tags)
       (render-poly nodes tags)
-      (render-path nodes)))
+      (render-line nodes tags)))
 
 (defun render-multipoly (rel)
   (loop for mem in (ocm.osm::rel-memids rel)
@@ -129,7 +205,7 @@
      for mem-type in (ocm.osm::rel-types rel)
      when (ocm.osm::way-p mem) ;; skip non-ways
      do (render-poly (ocm.osm::way-refs mem)
-                     (merge-tags (tags mem) (tags rel))
+                     (tags rel)
                      (equal "inner" mem-role))))
 
 (defun render-objects (objects)
@@ -138,8 +214,7 @@
           (ocm.osm::way
            (render-way (ocm.osm::way-refs obj) (tags obj)))
           (ocm.osm::rel
-           (when (equal (aget (tags obj) "type")
-                        "multipolygon")
+           (when (multipoly-p obj)
              (render-multipoly obj))))))
 
 (defun render ()
@@ -150,17 +225,31 @@
 
   (%nvg:end-frame *context*))
 
-(defun process-keys ()
-  (when (member %glfw:+key-left+ *pressed-keys*) (incf *pos-x* +translate-step+))
-  (when (member %glfw:+key-right+ *pressed-keys*) (decf *pos-x* +translate-step+))
-  (when (member %glfw:+key-up+ *pressed-keys*) (incf *pos-y* +translate-step+))
-  (when (member %glfw:+key-down+ *pressed-keys*) (decf *pos-y* +translate-step+))
-  (when (member %glfw:+key-minus+ *pressed-keys*) (decf *scale* +scale-step+))
-  (when (member %glfw:+key-equal+ *pressed-keys*) (incf *scale* +scale-step+)))
-
-(defparameter +translate-step+ 80f0)
+(defun render-test ()
+  (%nvg:begin-path *context*)
+  (%nvg:rect *context* 100f0 100f0 120f0 30f0)
+  (%nvg:circle *context* 120f0 120f0 5f0)
+  (%nvg:path-winding *context* (cffi:foreign-bitfield-value '%nvg:solidity :hole))
+
+  (c-with ((color %nvg:color))
+    (%nvg:fill-color *context* (%nvg:rgba (color &)
+                                          (round (* 127.5 (+ 1 (sin (%glfw:get-timer-value)))))
+                                          192 192 255)))
+  (%nvg:fill *context*)
+)
+
+(defparameter +translate-step+ 1)
 (defparameter +scale-step+ 0.3)
 (defvar *pressed-keys* nil)
+(defun process-keys ()
+  (let ((step (* +translate-step+ (expt 2 *zoom*))))
+    (when (member %glfw:+key-left+ *pressed-keys*) (decf (ocm.osm::node-lon *corner*) step))
+    (when (member %glfw:+key-right+ *pressed-keys*) (incf (ocm.osm::node-lon *corner*) step))
+    (when (member %glfw:+key-up+ *pressed-keys*) (decf (ocm.osm::node-lat *corner*) step))
+    (when (member %glfw:+key-down+ *pressed-keys*) (incf (ocm.osm::node-lat *corner*) step))
+    (when (member %glfw:+key-minus+ *pressed-keys*) (decf *zoom* +scale-step+))
+    (when (member %glfw:+key-equal+ *pressed-keys*) (incf *zoom* +scale-step+))))
+
 (glfw:define-key-callback on-keys (window key scancode action mod-keys)
   (declare (ignorable window key scancode action mod-keys))
   (format t "KEY: ~{~A ~}~%" (list key scancode action mod-keys))
@@ -170,13 +259,7 @@
       ((or
         (eql key %glfw:+key-escape+)
         (eql key %glfw:+key-q+))
-       (%glfw:set-window-should-close window %glfw:+true+))
-      ((eql key %glfw:+key-left+) (incf *pos-x* +translate-step+))
-      ((eql key %glfw:+key-right+) (decf *pos-x* +translate-step+))
-      ((eql key %glfw:+key-up+) (incf *pos-y* +translate-step+))
-      ((eql key %glfw:+key-down+) (decf *pos-y* +translate-step+))
-      ((eql key %glfw:+key-minus+) (decf *scale* +scale-step+))
-      ((eql key %glfw:+key-equal+) (incf *scale* +scale-step+))))
+       (%glfw:set-window-should-close window %glfw:+true+))))
   (when (eql action %glfw:+release+)
     (setf *pressed-keys* (delete key *pressed-keys*)))
   (print (list *scale* *pos-x* *pos-y*)))
@@ -240,9 +323,15 @@
       (gl:viewport 0 0 *frame-width* *frame-height*)
 
       ;; reset
-      (setf *scale* 300f0
-            *pos-x* -90270.0
-            *pos-y* 180380.0)
+      ;; (setf *scale* 540f0
+      ;;       *pos-x* -162910.0
+      ;;       *pos-y* 324540.0)
+      (setf *zoom* 20
+            *corner* (ocm.osm::make-node :LAT 59931756500 :LON 30293207600))
+
+      (setf *scale* 1f0
+            *pos-x* 0.0
+            *pos-y* 0.0)
 
       ;; Creating NanoVG context
       (let ((*context* (nvg:make-context)))

s2.lisp

@@ -3,6 +3,23 @@
   (:use :cl))
 (in-package :ocm.s2)
 
+;; This file contains documentation of the various coordinate systems used
+;; throughout the library.  Most importantly, S2 defines a framework for
+;; decomposing the unit sphere into a hierarchy of "cells".  Each cell is a
+;; quadrilateral bounded by four geodesics.  The top level of the hierarchy is
+;; obtained by projecting the six faces of a cube onto the unit sphere, and
+;; lower levels are obtained by subdividing each cell into four children
+;; recursively.  Cells are numbered such that sequentially increasing cells
+;; follow a continuous space-filling curve over the entire sphere.  The
+;; transformation is designed to make the cells at each level fairly uniform
+;; in size.
+;;
+;;
+;; ////////////////////////// S2Cell Decomposition /////////////////////////
+;;
+;; The following methods define the cube-to-sphere projection used by
+;; the S2Cell decomposition.
+;;
 ;; In the process of converting a latitude-longitude pair to a 64-bit cell
 ;; id, the following coordinate systems are used:
 ;;
@@ -65,11 +82,13 @@
 ;; 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+].")
+(defconstant +swap-mask+ 1)
+(defconstant +invert-mask+ 2)
+(defconstant +max-cell-level+ 28 "This is the number of levels needed to specify a leaf cell.")
+(defconstant +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].")
+(defconstant +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+].")
+(deftype ij () `(integer 0 ,+limit-ij+))
+(deftype si-ti () `(integer 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:
@@ -94,25 +113,47 @@
 ;;
 ;;  - 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+))
-
+(defconstant +face-bits+ 3)
+(defconstant +num-faces+ 6)
+(defconstant +max-level+ +max-cell-level+)
+(defconstant +pos-bits+ (1+ (* 2 +max-level+)))
+(defconstant +max-size+ (ash 1 +max-level+))
+(deftype level () `(integer 0 ,+max-level+))
+(deftype face () `(integer 0 ,(1- +num-faces+)))
+(deftype cell-id () `(integer 0 ,(1- (ash 1 (+ +face-bits+ +pos-bits+)))))
+
+(declaim (inline ij-to-stmin))
+(defun ij-to-stmin (i)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type ij i))
+  (* i (/ 1d0 +limit-ij+)))
+
+(declaim (inline st-to-ij))
 (defun st-to-ij (s)
-  (max 0 (min (- +limit-ij+ 1) (round (- (* s +limit-ij+) 0.5d0)))))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (double-float s))
+  (max 0 (min (1- +limit-ij+)
+              (the ij (round (- (* s +limit-ij+) 0.5d0))))))
 
 ;; Use quadratic (s,t) -> (u,v) projection. See S2 sources for explanation.
+(declaim (inline st-to-uv))
 (defun st-to-uv (s)
-  (* (/ 1 3)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type double-float s))
+  (* (/ 1 3d0)
      (if (>= s 0.5d0)
-         (- (* 4 s s) 1)
-         (- 1 (* 4 (- s 1) (- s 1))))))
+         (1- (* 4 s s))
+         (- 1 (* 4 (- 1 s) (- 1 s))))))
+
+(declaim (inline uv-to-st))
 (defun uv-to-st (u)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type double-float u))
   (if (>= u 0)
-      (* 0.5d0 (sqrt (+ 1 (* 3 u))))
-      (- 1 (* 0.5d0 (sqrt (- 1 (* 3 u)))))))
+      (* 0.5d0 (the double-float (sqrt (1+ (* 3 u)))))
+      (- 1 (* 0.5d0 (the double-float (sqrt (- 1 (* 3 u))))))))
+
+;;
 
 (defstruct point
   (x 0.0d0 :type double-float)
@@ -123,21 +164,32 @@
   (lat 0d0 :type double-float)
   (lng 0d0 :type double-float))
 
+(declaim (inline deg-to-rad))
 (defun deg-to-rad (deg)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type double-float deg))
   (* (/ pi 180d0) deg))
+
+(declaim (inline rad-to-deg))
 (defun rad-to-deg (rad)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type double-float 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))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (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))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type latlng latlng))
   (with-slots (lat lng) latlng
     (let* ((phi lat)
            (theta lng)
@@ -146,19 +198,25 @@
                   :y (* (sin theta) cos-phi)
                   :z (sin phi)))))
 (defun point-lat (p)
-  (declare (type point p))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (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
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type point p))
+  (with-slots (x y) p
     (atan y x)))
+
 (defun point-to-lat-lng (p)
   (declare (type point p))
   (from-radians (point-lat p) (point-lng p)))
 
+(declaim (inline dot))
 (defun dot (p1 p2)
-  (declare (type point p1 p2))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type point p1 p2))
   (+ (* (point-x p1) (point-x p2))
      (* (point-y p1) (point-y p2))
      (* (point-z p1) (point-z p2))))
@@ -175,7 +233,8 @@
   "The U,V,W axes for each face.")
 
 (defun get-xyz-face (p)
-  (declare (type point p))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type point p))
   (with-slots (x y z) p
     (cond
       ((and (> (abs x) (abs y))
@@ -187,14 +246,23 @@
       (t
        (if (>= z 0) 2 5)))))
 
+(declaim (inline get-uvw-axis))
 (defun get-uvw-axis (face axis)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type (simple-array point *) +face-uvw-axes+)
+           (type face face axis))
   (aref +face-uvw-axes+ face axis))
 
+(declaim (inline get-norm))
 (defun get-norm (face)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type face face))
   (get-uvw-axis face 2))
 
 (defun valid-face-xyz-to-uv (face p)
-  (declare (type point p))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type point p)
+           (type face face))
   (assert (> (dot (get-norm face) p) 0))
   (with-slots (x y z) p
     (ecase face
@@ -206,22 +274,40 @@
       (5 (values (/ (- y) z) (/ (- x) z))))))
 
 (defun xyz-to-face-uv (p)
-  (declare (type point p))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (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)
+;; The following lookup tables are used to convert efficiently between an
+;; (i,j) cell index and the corresponding position along the Hilbert curve.
+;; "lookup_pos" maps 4 bits of "i", 4 bits of "j", and 2 bits representing the
+;; orientation of the current cell into 8 bits representing the order in which
+;; that subcell is visited by the Hilbert curve, plus 2 bits indicating the
+;; new orientation of the Hilbert curve within that subcell.  (Cell
+;; orientations are represented as combination of kSwapMask and kInvertMask.)
+;;
+;; "lookup_ij" is an inverted table used for mapping in the opposite
+;; direction.
+;;
+;; We also experimented with looking up 16 bits at a time (14 bits of position
+;; plus 2 of orientation) but found that smaller lookup tables gave better
+;; performance.  (2KB fits easily in the primary cache.)
+
+(defconstant +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,0) (0,1) (1,0) (1,1)
   #(#(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  2  3
   #(#(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)
@@ -259,19 +345,120 @@
 (eval-when (:execute :compile-toplevel :load-toplevel)
   (init-lookup))
 
+(declaim (inline lsb32))
+(defun lsb32 (n)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type (integer 0 #xFFFFFFFF) n))
+  (loop with rc of-type (integer 0 31) = 31
+     for i from 4 downto 0
+     for shift of-type (integer 0 16) = (ash 1 4) then (ash shift -1)
+     for x = (logand #xFFFFFFFF (ash n shift))
+     when (not (zerop x))
+     do (setf n x rc (- rc shift))
+     finally (return rc)))
+
+(declaim (inline lsb62))
+(defun lsb62 (n)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type fixnum n))
+  (let ((bottombits (logand #xFFFFFFFF n)))
+    (if (zerop bottombits)
+        (+ 32 (the (integer 0 31) (lsb32 (ash n -32))))
+        (lsb32 bottombits))))
+
+(declaim (inline find-lsb-set))
+(defun find-lsb-set (num)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id num))
+  (lsb62 num)
+;  (or (loop for i upto +pos-bits+ when (logbitp i num) return i) 0)
+  )
+
+(declaim (inline log2floor32))
+(defun log2floor32 (n)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type (unsigned-byte 32) n))
+  (if (zerop n) -1
+      (loop with log of-type (integer 0 31) = 0
+         with value of-type (integer 0 #xFFFFFFFF) = n
+         for i from 4 downto 0
+         for shift of-type (integer 0 16) = (ash 1 i)
+         for x = (ash value (- shift))
+         when (not (zerop x))
+         do (setf value x log (+ log shift))
+         finally (return log))))
+
+(declaim (inline log2floor62))
+(defun log2floor62 (n)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type fixnum n))
+  (let ((topbits (ash n -32)))
+    (if (zerop topbits)
+        (log2floor32 (logand n #xFFFFFFFF))
+        (+ 32 (the (integer 0 31) (log2floor32 topbits))))))
+
+(declaim (inline find-msb-set))
+(defun find-msb-set (num)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id num))
+  (log2floor62 num))
+
+(declaim (inline cell-lsb))
+(defun cell-lsb (id)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id))
+  (logand id (1+ (lognot id))))
+
+(declaim (inline lsb-for-level))
+(defun lsb-for-level (level)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type level level))
+  (ash 1 (* 2 (- +max-level+ level))))
+
+(declaim (inline cell-face))
+(defun cell-face (id)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id))
+  (ash id (- +pos-bits+)))
+
+(declaim (inline cell-pos))
+(defun cell-pos (id)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (cell-id id))
+  (logand id (1- (ash 1 +pos-bits+))))
+
+(declaim (inline cell-is-valid))
+(defun cell-is-valid (id)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (cell-id id))
+  (and (< (cell-face id) +num-faces+)
+       (not (zerop (logand (cell-lsb id) #x5555555555555555 (1- (ash 1 +pos-bits+)))))))
+
+(defun cell-level (id)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id))
+  (assert (not (zerop id)))
+  (- +max-level+
+     (ash (find-lsb-set id) -1)))
+
 (defun from-face-ij (face i j)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type face face)
+           (type ij i j)
+           (type (simple-array (unsigned-byte 16) *) *lookup-pos*))
   (let ((n (ash face (1- +pos-bits+)))
-        (bits (logand face +swap-mask+)))
+        (bits (logand face +swap-mask+))
+        (mask (1- (ash 1 +lookup-bits+))))
+    (declare (type cell-id n))
     (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+))))))
+                 `(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)))
+                        n (logior n (the cell-id (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)
@@ -280,55 +467,27 @@
       (get-bits 2)
       (get-bits 1)
       (get-bits 0))
-    (1+ (* 2 n))))
+    (1+ (ash n 1))))
 
 (defun point-to-cell (p)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type point 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))))
+(declaim (inline si-ti-to-st))
+(defun si-ti-to-st (si)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type si-ti si))
+  (* si (/ 1d0 +max-si-ti+)))
 
 (defun face-uv-ti-xyz (face u v)
-  (declare (type double-float u v))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type double-float u v)
+           (type face face))
   (ecase face
     (0 (make-point :x 1d0 :y u :z v))
     (1 (make-point :x (- u) :y 1d0 :z v))
@@ -337,82 +496,138 @@
     (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 face-si-ti-to-xyz (face si ti)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type si-ti si ti)
+           (type face face))
+  (face-uv-ti-xyz face
+                  (st-to-uv (si-ti-to-st si))
+                  (st-to-uv (si-ti-to-st ti))))
 
+(declaim (inline cell-child-position))
 (defun cell-child-position (id &optional level)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id)
+           (type (or null level) level))
   (unless level (setf level (cell-level id)))
   (assert (cell-is-valid id))
   (assert (>= level 1))
-  (assert (<= level (cell-level id)))
+  (assert (<= level (the 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))))
+(declaim (inline cell-is-leaf))
 (defun cell-is-leaf (id)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id))
   (not (zerop (logand id 1))))
+
+(declaim (inline cell-is-face))
 (defun cell-is-face (id)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id))
   (zerop (logand id (1- (lsb-for-level 0)))))
 
+(declaim (inline cell-range-min))
+(defun cell-range-min (id)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id))
+  (- id (1- (cell-lsb id))))
+
+(declaim (inline cell-range-max))
+(defun cell-range-max (id)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id))
+  (+ id (1- (cell-lsb id))))
+
+(declaim (inline cell-parent))
 (defun cell-parent (id &optional level)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id)
+           (type (or null level) level))
   (assert (cell-is-valid id))
   (if level (progn
               (assert (>= level 0))
-              (assert (<= level (cell-level id))))
+              (assert (<= level (the 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))))))
 
+(declaim (inline cell-child))
 (defun cell-child (id pos)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id)
+           (type (integer 0 3) 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-face-ij-orientation (id)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id)
+           (type (simple-array (unsigned-byte 16) *) *lookup-ij*))
+  (let* ((i 0)
+         (j 0)
+         (face (cell-face id))
+         (bits (logand face +swap-mask+)))
+    (declare (type ij i j)
+             (type face face)
+             (type (integer 0 3) bits))
+    (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)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id))
+  (multiple-value-bind (face i j orient) (cell-to-face-ij-orientation id)
+    (declare (ignore orient)
+             (type ij i j))
+    (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 cell-to-point-raw (id)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type cell-id id))
+  (apply #'face-si-ti-to-xyz (multiple-value-list (cell-get-center-face-si-ti id))))
+
 (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))))
+    (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+             (type cell-id id))
+    (format nil "~a/~{~a~}" (cell-face id)
+            (loop for level from 1 to (the level (cell-level id))
+               collect (cell-child-position id level))))
 
 ;;;;;;;;;
 ;;;
+
+
 (defun make-span (vector offset &optional size)
-  (declare (type vector vector))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type vector vector)
+           (type fixnum offset)
+           (type (or null fixnum) size))
   (let ((length (length vector)))
     (assert (< offset length))
-    (when size (assert (< (+ offset size) length)))
-    (make-array (if size size (- length offset))
+    (when size (assert (< (the fixnum (+ offset size)) length)))
+    (make-array (or size (the fixnum (- length offset)))
                 :element-type (or (cadr (type-of vector)) t)
                 :displaced-to vector
                 :displaced-index-offset offset)))
@@ -420,38 +635,53 @@
 (deftype byte-vector () `(vector (unsigned-byte 8)))
 
 (defun make-encoder (size)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type fixnum size))
   (make-array size :element-type '(unsigned-byte 8) :fill-pointer 0 :adjustable t))
 
+(declaim (inline encoder-length))
+(defun encoder-length (encoder)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type byte-vector encoder))
+  (fill-pointer encoder))
+
+(declaim (inline encoder-avail))
 (defun encoder-avail (encoder)
-  (declare (type byte-vector encoder))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type byte-vector encoder))
   (- (array-total-size encoder)
      (fill-pointer encoder)))
 
+(declaim (inline encoder-ensure))
 (defun encoder-ensure (encoder size)
-  (declare (type byte-vector encoder))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type byte-vector encoder)
+           (type fixnum size))
   (when (< (encoder-avail encoder) size)
     (adjust-array encoder (+ (fill-pointer encoder) size))))
 
+(declaim (inline encoder-put8))
 (defun encoder-put8 (encoder value)
-  (declare (type byte-vector encoder)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type byte-vector encoder)
            (type (unsigned-byte 8) value))
   (assert (> (encoder-avail encoder) 1))
   (vector-push value encoder)
   encoder)
 
+(declaim (inline encoder-putn))
 (defun encoder-putn (encoder vector)
-  (declare (type byte-vector encoder)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (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")
+(defconstant +varint-max32+ 5 "Maximum varint encoding length for uint32")
+(declaim (inline encoder-put-varint32))
 (defun encoder-put-varint32 (encoder v)
-  (declare (type byte-vector encoder)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type byte-vector encoder)
            (type (unsigned-byte 32) v))
   (let ((b 128))
     (cond
@@ -476,9 +706,10 @@
        (encoder-put8 encoder (logand 255 (ash v -28))))))
   encoder)
 
-(defparameter +varint-max64+ 10 "Maximum varint encoding length for uint64")
+(defconstant +varint-max64+ 10 "Maximum varint encoding length for uint64")
 (defun encoder-put-varint64 (encoder v)
-  (declare (type byte-vector encoder)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type byte-vector encoder)
            (type (unsigned-byte 64) v))
   (if (< v (ash 1 28))
       (encoder-put-varint32 encoder v)
@@ -504,20 +735,25 @@
           (when initial-element `(:initial-element ,initial-element))
           (when displaced-to `(:displaced-to ,displaced-to :displaced-index-offset ,(fill-pointer displaced-to))))))
 
+(declaim (inline decoder-avail))
 (defun decoder-avail (decoder)
-  (declare (type byte-vector decoder))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type byte-vector decoder))
   (- (array-total-size decoder)
      (fill-pointer decoder)))
 
+(declaim (inline decoder-skip))
 (defun decoder-skip (decoder &optional (count 1))
-  (declare (type byte-vector decoder)
-           (type (unsigned-byte 32) count))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type byte-vector decoder)
+           (type fixnum count))
   (incf (fill-pointer decoder) count))
 
+(declaim (inline decoder-reset))
 (defun decoder-reset (decoder)
-  (declare (type byte-vector decoder))
-  (setf (fill-pointer decoder) 0)
-  decoder)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type byte-vector decoder))
+  (setf (fill-pointer decoder) 0))
 
 (defun make-sub-decoder (decoder size &optional will-decode)
   (when (>= (decoder-avail decoder) size)
@@ -535,39 +771,57 @@
               ,@body)
          (setf (fill-pointer ,dc) ,fp)))))
 
+(declaim (inline decoder-get8))
 (defun decoder-get8 (decoder)
-  (declare (type byte-vector decoder))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type byte-vector decoder))
   (prog1 (aref decoder (fill-pointer decoder))
     (decoder-skip decoder)))
 
+(declaim (inline decoder-peek))
 (defun decoder-peek (decoder &optional (offset 0))
-  (declare (type byte-vector decoder)
-           (type (unsigned-byte 32) offset))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type byte-vector decoder)
+           (type fixnum offset))
   (aref decoder (+ (fill-pointer decoder) offset)))
 
+(deftype varint-length () `(integer 0 ,+varint-max64+))
 (defun decoder-get-varint64 (decoder)
-  (declare (type byte-vector decoder))
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (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)))
+     for i of-type varint-length from 0 upto (decoder-avail decoder)
+     with result of-type (unsigned-byte 64) = 0
+     do (incf result (the (unsigned-byte 64) (ash (if (zerop i) b (1- b)) (* i 7))))
+     until (< b 128)
+     finally (return result)))
+
+(defun decoder-get-varint-fixnum (decoder)
+  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
+           (type byte-vector decoder))
+  (loop
+     for b = (decoder-get8 decoder) then (decoder-get8 decoder)
+     for i of-type varint-length from 0 upto (decoder-avail decoder)
+     with result of-type fixnum = 0
+     do (incf result (the fixnum (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))
+(template (<t>)
+  (defun encode-uint-with-length (value length encoder)
+    (declare (type <t> 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)