Napa-FFT3/support.lisp at master · pkhuong/Napa-FFT3 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
(in-package "NAPA-FFT.SUPPORT")

(deftype index ()
  `(unsigned-byte #. (min (1- (integer-length most-positive-fixnum))
                          (integer-length (1- array-dimension-limit)))))

(deftype half-index ()
  `(unsigned-byte #.(truncate (integer-length most-positive-fixnum) 2)))

(deftype size ()
  `(and (integer 1) index))

(deftype half-size ()
  `(and (integer 1) half-index))

(deftype complex-sample ()
  `(complex double-float))

(deftype complex-sample-array (&optional size)
  `(simple-array complex-sample (,size)))

(deftype real-sample ()
  'double-float)

(deftype real-sample-array (&optional size)
  `(simple-array real-sample (,size)))

(defvar *optimization-policy* '(optimize speed (safety 0)))

(defun bit-reverse-integer (x width)
  (let ((acc 0))
    (loop repeat width
          for bit = (logand x 1)
          do (setf x (ash x -1))
             (setf acc (logior (ash acc 1) bit))
          finally (return acc))))

(defconstant +twiddle-offset+ -1)

(defun make-twiddle (n &optional (dir 1d0))
  (assert (power-of-two-p n))
  (check-type dir (member 1d0 -1d0))
  (let ((vec (make-array n :element-type 'complex-sample
                           :initial-element (complex 0d0 0d0))))
    (loop for size = 4 then (* 2 size)
          while (<= size n)
          do (let ((start (+ (truncate size 2) +twiddle-offset+))
                   (base  (/ (* dir 2 pi) size)))
               (dotimes (i (truncate size 4))
                 (let* ((theta (* -1 i base))
                        (t1 (cis theta))
                        (t2 (cis (* 3 theta))))
                   (setf (aref vec (+ start (* 2 i)))   t1
                         (aref vec (+ start 1 (* 2 i))) t2)))))
    vec))

(declaim (inline power-of-two-p lb))
(defun lb (n)
  (integer-length (1- n)))

(defun power-of-two-p (x)
  (= 1 (logcount x)))

(declaim (ftype (function (sequence &optional size)
                          (values complex-sample-array &optional))
                complex-samplify))
(defun complex-samplify (vec &optional (size (length vec)))
  (etypecase vec
    (complex-sample-array vec)
    ((simple-array (complex single-float) 1)
     (map-into (make-array size
                           :element-type 'complex-sample)
               (lambda (x)
                 (coerce x 'complex-sample))
               vec))
    (real-sample-array
     (map-into (make-array size
                           :element-type 'complex-sample)
               (lambda (x)
                 (coerce x 'complex-sample))
               vec))
    ((simple-array single-float 1)
     (map-into (make-array size
                           :element-type 'complex-sample)
               (lambda (x)
                 (coerce x 'complex-sample))
               vec))
    (sequence
     (map-into (make-array size
                           :element-type 'complex-sample)
               (lambda (x)
                 (coerce x 'complex-sample))
               vec))))

(declaim (ftype (function (sequence &optional size)
                          (values real-sample-array &optional))
                real-samplify))
(defun real-samplify (vec &optional (size (length vec)))
  (etypecase vec
    (real-sample-array vec)
    ((simple-array single-float 1)
     (map-into (make-array size
                           :element-type 'real-sample)
               (lambda (x)
                 (coerce x 'real-sample))
               vec))
    (sequence
     (map-into (make-array size
                           :element-type 'real-sample)
               (lambda (x)
                 (coerce x 'real-sample))
               vec))))