share/numeric/fft-core.lisp

   1 ;; -*- Lisp -*-
   2
   3 (in-package :maxima-fft)
   4 (use-package :maxima)
   5
   6 (defun mgetarray (marg)
   7 "Return the lisp array which is somehow attached to MARG."
   8   (or (and (symbolp marg) (symbol-array (mget marg 'maxima::array)))
   9       (and ($listp marg) (make-array ($length marg) :initial-contents (rest marg)))
  10       (and (arrayp marg) marg)))
  11
  12 (defun fft-arg-check (user-fcn-name ary)
  13   ;; I don't check here if this is really a floating point array.  For maxima
  14   ;; arrays which are symbols this would be no problem since the type is on
  15   ;; the property list.  On the other hand, for "fast" arrays (i.e. lisp
  16   ;; arrays), using ARRAY-ELEMENT-TYPE might not be too useful.
  17   (or (mgetarray ary)
  18       (merror "~M: argument must a list or array; instead found ~:M" user-fcn-name ary)))
  19
  20 (defun make-empty-copy (a)
  21   (cond
  22     (($listp a)
  23      (cons '(mlist) (make-list ($length a) :initial-element 0e0)))
  24     ((arrayp a)
  25      (make-array (length a) :initial-element 0e0))
  26     ((symbolp a)
  27      (meval
  28        `(($array)
  29          ,(intern (symbol-name (gensym "$G")))
  30          $float
  31          ,(1- (length (mgetarray a))))))))
  32
  33 (defun complex-to-polar (re im)
  34   (dotimes (k (min (length re) (length im)))
  35     (let ((rp (aref re k))
  36           (ip (aref im k)))
  37       (setf (aref re k) (abs (complex rp ip)))
  38       (setf (aref im k) (atan ip rp)))))
  39
  40 (defun polar-to-complex (mag phase)
  41   (dotimes (k (min (length mag) (length phase)))
  42     (let ((r (aref mag k))
  43           (p (aref phase k)))
  44       (setf (aref mag k) (* r (cos p)))
  45       (setf (aref phase k) (* r (sin p))))))
  46
  47 (declaim (inline log-base2))
  48 (defun log-base2 (n)
  49   "Return the M such that 2^m <= N.  If N is a power of two, M =
  50   log2(N).  Expected usage is for the case of N a power of two."
  51   (declare (type (and fixnum (integer 0)) n)
  52            (optimize (speed 3)))
  53   ;; Just find m such that 2^m <= n < 2^(m+1).  It's up to the caller
  54   ;; to make sure that n is a power of two.  (The previous
  55   ;; implementation using (/ (log n) (log 2)) has roundoff errors.
  56   ;; This doesn't.)
  57   (1- (integer-length n)))
  58
  59 (defvar *sincos-tables*
  60   (make-hash-table)
  61   "Hash table mapping log2 of the FFT size to an
  62   array of exp(2*pi*i/N), where N is the FFT size.")
  63
  64 (defun sincos-table (m)
  65   "For an FFT order of M, return an array of complex twiddle factors
  66   for the FFT. The array contains the values exp(i*pi*k/2^(m-1))."
  67   (declare (type (and fixnum unsigned-byte) m))
  68   (cond ((gethash m *sincos-tables*))
  69         (t
  70          ;; Need to create the sincos table.  Only need to have a half
  71          ;; period.
  72          (let* ((n (ash 1 (1- m)))
  73                 (p (/ #+(and cmu flonum-double-double) kernel:dd-pi
  74                       #-flonum-double-double (coerce pi 'double-float)
  75                       n))
  76                 (table (make-array n :element-type
  77                                    #+(and cmu flonum-double-double) '(complex double-double-float)
  78                                    #-flonum-double-double '(complex double-float))))
  79            (dotimes (k n)
  80              (setf (aref table k) (cis (* k p))))
  81            ;; Make the half point exactly correct
  82            (when (> n 1)
  83              (setf (aref table (ash n -1))
  84                    (coerce #c(0 1) #+(and cmu flonum-double-double) '(complex double-double-float)
  85                            #-flonum-double-double '(complex double-float))))
  86            (setf (gethash m *sincos-tables*) table)
  87            table))))
  88
  89 (defun mlist->complex-cl-vector (object)
  90   "Convert Maxima list to a (simple-array (complex double-float) (*))
  91   with the same values."
  92   (let ((z (make-array (1- (length object)) :element-type '(complex double-float))))
  93     (loop for k of-type fixnum from 0
  94           for x in (cdr object)
  95           while x
  96           do
  97              (let ((fl (risplit ($float x))))
  98                (setf (aref z k) (complex (car fl) (cdr fl)))))
  99     z))
 100
 101 (defun complex-cl-vector->mlist (array)
 102   "Convert a CL array of complex values to a Maxima list containing
 103   the same values."
 104   (declare (type (simple-array (complex double-float) (*)) array))
 105   (cons '(mlist simp)
 106         (loop for w of-type (complex double-float) across array
 107               collect (add (realpart w)
 108                            (mul (imagpart w) '$%i)))))
 109
 110 (defun array->complex-cl-vector (array)
 111   "Convert a CL vector of maxima values to a (simsple-array (complex
 112   double-float) (*)) with the same complex values."
 113   (let* ((n (length array))
 114          (z (make-array n :element-type '(complex double-float))))
 115     (dotimes (k n)
 116       (let ((fl (risplit ($float (aref array k)))))
 117         (setf (aref z k) (complex (car fl) (cdr fl)))))
 118     z))
 119
 120 (defun complex-cl-vector->vector (array)
 121   "Convert (simple-array (complex double-float) (*)) to a CL vector of
 122   maxima expressions of the same value."
 123   (declare (type (simple-array (complex double-float) (*)) array))
 124   (let* ((n (length array))
 125          (z (make-array n)))
 126     (dotimes (k n)
 127       (let ((item (aref array k)))
 128         (setf (aref z k) (add (realpart item)
 129                               (mul '$%i (imagpart item))))))
 130     z))
 131
 132 (defun find-complex-fft-converters (object &optional (maxima-function-name "fft"))
 133   "Convert a Maxima OBJECT to a specialized CL vector suitable for the
 134   complex FFT routines.  Two values are returned: The specialized
 135   vector and a function that will convert the result of the FFT
 136   routine to a Maxima object of the same type as
 137   OBJECT. MAXIMA-FUNCTION-NAME is a string to use for error messages."
 138   ;;(format t "object = ~S~%" object)
 139   ;;(format t "arrayp = ~S~%" (arrayp object))
 140   ;;(describe 'arrayp)
 141   (cond (($listp object)
 142          (values
 143           (mlist->complex-cl-vector object)
 144           #'complex-cl-vector->mlist))
 145         ((arrayp object)
 146          (values
 147           (array->complex-cl-vector object)
 148           #'complex-cl-vector->vector))
 149         ((and (symbolp object) (symbol-array (mget object 'maxima::array)))
 150          (values
 151           (let* ((sym-array (symbol-array (mget object 'maxima::array))))
 152             (array->complex-cl-vector sym-array))
 153           #'(lambda (array)
 154               (let ((ar (complex-cl-vector->vector array))
 155                     (sym (meval `(($array)
 156                                   ,(intern (symbol-name (gensym "$G")) "MAXIMA")
 157                                   $float
 158                                   ,(1- (length array))))))
 159                 (setf (symbol-array (mget sym 'maxima::array))
 160                       ar)
 161                 sym))))
 162         (t
 163          (merror "~A: input is not a list or an array: ~M." maxima-function-name object))))
 164
 165 ;; Fast Fourier Transform using a simple radix-2 implementation with
 166 ;; inputs and outputs in natural order.  The definition of the forward
 167 ;; FFT follows Maxima's definition of the forward FFT.
 168 (defun fft-r2-nn (x &key (inverse-fft-p nil))
 169   "Compute the FFT of X which MUST be a specialzed vector of complex
 170   double-float's whose length MUST be a power of 2.  If INVERSE-FFT-P
 171   is non-NIL, the inverse FFT is computed.  The scaling by 1/N is
 172   included in the inverse FFT.
 173
 174   The contents of the input X may be destroyed."
 175   (declare (type (simple-array (complex double-float) (*)) x))
 176   (let* ((n (length x))
 177          (half-n (ash n -1))
 178          (pairs-in-group (ash n -1))
 179          (number-of-groups 1)
 180          (distance (ash n -1))
 181          (not-switch-input t)
 182          (sincos (sincos-table (maxima-fft:log-base2 n)))
 183          (a x)
 184          (b (make-array (length x) :element-type '(complex double-float))))
 185     (declare (fixnum n half-n pairs-in-group number-of-groups distance)
 186              (type (simple-array (complex double-float) (*)) a b sincos))
 187     (flet ((fft ()
 188              (declare (optimize (speed 3) (safety 0)))
 189              (let ((index 0))
 190                (declare (fixnum index))
 191                (dotimes (k number-of-groups)
 192                  (declare (fixnum k))
 193                  (let* ((jfirst (* 2 k pairs-in-group))
 194                         (jlast (+ jfirst pairs-in-group -1))
 195                         (jtwiddle (* k pairs-in-group))
 196                         (w (let ((w (aref sincos jtwiddle)))
 197                              (if inverse-fft-p
 198                                (conjugate w)
 199                                w))))
 200                    (declare (fixnum jfirst jlast jtwiddle)
 201                             (type (complex double-float) w))
 202                    #+nil
 203                    (format t  "k = ~D, jfirst/last = ~D ~D jtwiddle = ~D dist ~D index ~D, W ~S~%"
 204                            k jfirst jlast jtwiddle distance index w)
 205                    (loop for j of-type fixnum from jfirst upto jlast do
 206                      (let ((temp (* w (aref a (+ j distance)))))
 207                        (setf (aref b index) (+ (aref a j) temp))
 208                        (setf (aref b (+ index half-n)) (- (aref a j) temp))
 209                        (incf index))))))))
 210       (loop while (< number-of-groups n) do
 211         (fft)
 212
 213         #+nil
 214         (progn
 215           (format t "number-of-groups = ~D~%" number-of-groups)
 216           (format t "Output = ~S~%" b))
 217
 218         (rotatef a b)
 219         (setf not-switch-input (not not-switch-input))
 220         (setf pairs-in-group (ash pairs-in-group -1))
 221         (setf number-of-groups (ash number-of-groups 1))
 222         (setf distance (ash distance -1)))
 223
 224       (if inverse-fft-p
 225           a
 226           (dotimes (k n a)
 227             (declare (fixnum k)
 228                      (optimize (speed 3) (safety 0)))
 229             (let ((w (aref a k)))
 230               (setf (aref a k) (/ w n))))))))
 231
 232 ;;; RFFT
 233 ;;;
 234 ;;; The FFT of a real signal of length N can be computed using complex
 235 ;;; FFT of length N/2.  For large values of N, this can represent
 236 ;;; significant savings in computation.
 237 ;;;
 238 ;;; See http://www.engineeringproductivitytools.com/stuff/T0001/PT10.HTM.
 239
 240 (defun mlist->rfft-vector (object)
 241   "Convert Maxima list to a (simple-array (complex double-float) (*))
 242   with the same values suitable for as the input for the RFFT routine.
 243   The maxima list is assumed to consist only of real values."
 244   (let* ((n (length (cdr object)))
 245          (z (make-array (ash n -1) :element-type '(complex double-float))))
 246     (loop for k from 0
 247           for (re im) on (cdr object) by #'cddr
 248           while im
 249           do
 250              (setf (aref z k) (complex ($float re) ($float im))))
 251     z))
 252
 253 (defun array->rfft-vector (array)
 254   "Convert CL array of real (Maxima) values to a (simple-array
 255   (complex double-float) (*)) with the same values suitable for as the
 256   input for the RFFT routine.  The CL array is assumed to consist only
 257   of real values."
 258   (let* ((n (length array))
 259          (z (make-array (ash n -1) :element-type '(complex double-float))))
 260     (loop for k from 0 below n by 2
 261           for m from 0
 262           do
 263              (setf (aref z m) (complex ($float (aref array k))
 264                                        ($float (aref array (1+ k))))))
 265     z))
 266
 267 (defun find-rfft-converters (object)
 268   "Convert a Maxima OBJECT to a specialized CL vector suitable for the
 269   RFFT routines.  Two values are returned: The specialized vector and
 270   a function that will convert the result of the RFFT routine to a
 271   Maxima object of the same type as OBJECT. MAXIMA-FUNCTION-NAME is a
 272   string to use for error messages."
 273   (cond (($listp object)
 274          (values
 275           (mlist->rfft-vector object)
 276           #'complex-cl-vector->mlist
 277           (1- (length object))))
 278         ((arrayp object)
 279          (values
 280           (let ((z (make-array  (length object) :element-type '(complex double-float))))
 281             (loop for k of-type fixnum from 0
 282                   for x across object
 283                   do
 284                      (let ((fl (risplit ($float x))))
 285                        (setf (aref z k) (complex (car fl) (cdr fl)))))
 286             z)
 287           #'complex-cl-vector->vector
 288           (length object)))
 289         ((and (symbolp object) (symbol-array (mget object 'maxima::array)))
 290          (let ((sym-array (symbol-array (mget object 'maxima::array))))
 291            (values
 292             (array->rfft-vector sym-array)
 293             #'(lambda (array)
 294                 (let ((ar (complex-cl-vector->vector array))
 295                       (sym (meval `(($array)
 296                                     ,(intern (symbol-name (gensym "$G")))
 297                                     $float
 298                                     ,(1- (length array))))))
 299                   (setf (symbol-array (mget sym 'maxima::array))
 300                         ar)))
 301             (length sym-array))))
 302         (t
 303          (merror "real_fft: input is not a list or an array: ~M." object))))
 304
 305 (defun find-irfft-converters (object)
 306   "Convert a Maxima OBJECT to a specialized CL vector suitable for the
 307   inverse RFFT routines.  Two values are returned: The specialized
 308   vector and a function that will convert the result of the inverse
 309   RFFT routine to a Maxima object of the same type as
 310   OBJECT. MAXIMA-FUNCTION-NAME is a string to use for error messages."
 311   (cond (($listp object)
 312          (values
 313           (mlist->complex-cl-vector object)
 314           #'(lambda (obj)
 315               (let (result)
 316                 (map nil #'(lambda (z)
 317                              (push (realpart z) result)
 318                              (push (imagpart z) result))
 319                      obj)
 320                 (cons '(mlist simp) (nreverse result))))))
 321         ((arrayp object)
 322          (values
 323           (array->complex-cl-vector object)
 324           #'(lambda (obj)
 325               (let ((result (make-array (* 2 (length obj)))))
 326                 (loop for k from 0 by 2
 327                       for z across obj
 328                       do
 329                          (progn
 330                            (setf (aref result k) (realpart z))
 331                            (setf (aref result (1+ k)) (imagpart z))))
 332                 result))))
 333         (t
 334          (merror "inverse_real_fft: input is not a list or an array: ~M." object))))
 335
 336 \f
 337 ;;; Bigfloat FFT
 338 ;;;
 339 ;;; The same set of FFT routines as for floats, but modified to return
 340 ;;; a bigfloat result.
 341
 342 ;; Convert a sequence to an array of complex bigfloat numbers.  These
 343 ;; probably don't have to be very fast because the conversions to
 344 ;; bigfloat and back are relatively slow.
 345 (defun seq->bfft-vector (seq)
 346   (map 'vector #'(lambda (z)
 347                    (destructuring-bind (rp . ip)
 348                        (risplit ($bfloat z))
 349                      (bigfloat:bigfloat rp ip)))
 350        seq))
 351
 352 (defun mlist->bfft-vector (mlist)
 353   (seq->bfft-vector (cdr mlist)))
 354
 355 (defun lisp-array->bfft-vector (arr)
 356   (seq->bfft-vector arr))
 357
 358 (defun bfft-vector->seq (arr seqtype)
 359     (map seqtype #'to arr))
 360
 361 (defun bfft-vector->mlist (arr)
 362   (cons '(mlist simp) (bfft-vector->seq arr 'list)))
 363
 364 (defun bfft-vector->lisp-array (arr)
 365   (bfft-vector->seq arr 'vector))
 366
 367 (defun bfft-vector->maxima-symbol-array (arr)
 368   "Outputs a Maxima array as does Maxima's 'array()' function."
 369   (let ((lisp-array (bfft-vector->lisp-array arr))
 370         (maxima-symbol (meval `(($array)
 371                                 ,(intern (symbol-name (gensym "$G")))
 372                                 $float
 373                                 ,(1- (length arr))))))
 374     (setf (symbol-array (mget maxima-symbol 'maxima::array)) lisp-array)
 375     maxima-symbol))
 376
 377 (defun find-bf-fft-converter (input &optional (name "bf_fft"))
 378   (cond (($listp input)
 379          (values
 380           (mlist->bfft-vector input)
 381           #'bfft-vector->mlist))
 382         ((arrayp input)
 383          (values
 384           (lisp-array->bfft-vector input)
 385           #'bfft-vector->lisp-array))
 386         ((and (symbolp input) (symbol-array (mget input 'maxima::array)))
 387          (values
 388           (lisp-array->bfft-vector (symbol-array (mget input 'maxima::array)))
 389           #'bfft-vector->maxima-symbol-array))
 390         (t
 391          (merror "~A: input is not a list or an array: ~M." name input))))
 392
 393
 394
 395 (defun mlist->bf-rfft-vector (object)
 396   (let* ((n (length (cdr object)))
 397          (z (make-array (ash n -1))))
 398     (loop for k from 0
 399           for (re im) on (cdr object) by #'cddr
 400           while im
 401           do
 402              (setf (aref z k) (complex ($float re) ($float im))))
 403     z))
 404
 405 (defun lisp-array->bf-rfft-vector (object)
 406   (let ((z (make-array (length object))))
 407     (loop for k of-type fixnum from 0
 408           for x across object
 409           do
 410              (let ((fl (risplit ($float x))))
 411                (setf (aref z k) (complex (car fl) (cdr fl)))))
 412     z))
 413
 414 (defun find-bf-rfft-converters (object)
 415   (cond (($listp object)
 416          (values
 417           (mlist->bf-rfft-vector object)
 418           #'bfft-vector->mlist))
 419         ((arrayp object)
 420          (values
 421           (lisp-array->bf-rfft-vector object)
 422           #'bfft-vector->lisp-array))
 423         ((and (symbolp object) (symbol-array (mget object 'maxima::array)))
 424          (values
 425           (lisp-array->bf-rfft-vector (symbol-array (mget object 'maxima::array)))
 426           #'(lambda (array)
 427               (let ((ar (bfft-vector->lisp-array array))
 428                     (sym (meval `(($array)
 429                                   ,(intern (symbol-name (gensym "$G")))
 430                                   $float
 431                                   ,(1- (length array))))))
 432                 (setf (symbol-array (mget sym 'maxima::array))
 433                       ar)))))
 434         (t
 435          (merror "bf_rfft: input is not a list or an array: ~M." object))))
 436
 437 (defun find-bf-irfft-converters (object)
 438   (cond (($listp object)
 439          (values
 440           (mlist->bfft-vector object)
 441           #'(lambda (obj)
 442               (let (result)
 443                 (map nil #'(lambda (z)
 444                              (push (to (bigfloat:realpart z)) result)
 445                              (push (to (bigfloat:imagpart z)) result))
 446                      obj)
 447                 (cons '(mlist simp) (nreverse result))))))
 448         ((arrayp object)
 449          (values
 450           (lisp-array->bfft-vector object)
 451           #'(lambda (obj)
 452               (let ((result (make-array (* 2 (length obj)))))
 453                 (loop for k from 0 by 2
 454                       for z across obj
 455                       do
 456                          (progn
 457                            (setf (aref result k) (realpart z))
 458                            (setf (aref result (1+ k)) (imagpart z))))
 459                 result))))
 460         (t
 461          (merror "bf_inverse_real_fft: input is not a list or an array: ~M." object))))
 462
 463 ;;; Bigfloat implementation of Fast Fourier Transform using a radix-2
 464 ;;; algorithm with inputs and outputs in natural order.
 465 (in-package "BIGFLOAT")
 466
 467 ;; This is an equal hash table with a key being a list of the order of
 468 ;; the FFT and fpprec.  We need fpprec in case the caller changes the
 469 ;; requested precision.
 470 (defvar *bf-sincos-tables*
 471   (make-hash-table :test 'equal)
 472   "Hash table mapping log2 of the FFT size to an
 473   array of exp(2*pi*i/N), where N is the FFT size.")
 474
 475 (defun sincos-table (m)
 476   "For an FFT order of M, return an array of complex bigfloat twiddle
 477   factors for the FFT. The array contains the values
 478   exp(i*pi*k/2^(m-1))."
 479   (cond ((gethash (list m maxima::fpprec) *bf-sincos-tables*))
 480         (t
 481          ;; Need to create the sincos table.  Only need to have a half
 482          ;; period.
 483          (let* ((n (ash 1 (1- m)))
 484                 (p (/ (%pi (bigfloat:bigfloat 1))
 485                       n))
 486                 (table (make-array n)))
 487            (dotimes (k n)
 488              (setf (aref table k) (cis (* k p))))
 489            ;; Make the half point exactly correct
 490            (when (> n 1)
 491              (setf (aref table (ash n -1))
 492                    (bigfloat:bigfloat 0 1)))
 493            (setf (gethash (list m maxima::fpprec) *bf-sincos-tables*)
 494                  table)
 495            table))))
 496
 497
 498 ;; Fast Fourier Transform using a simple radix-2 implementation with
 499 ;; inputs and outputs in natural order.  The definition of the forward
 500 ;; FFT follows Maxima's definition of the forward FFT.  The input and
 501 ;; output are assumed to be bigfloats objects.
 502 (defun fft-r2-nn (x &key (inverse-fft-p nil))
 503   "Compute the FFT of X which MUST be a specialzed vector of complex
 504   bigfloat's whose length MUST be a power of 2.  If INVERSE-FFT-P is
 505   non-NIL, the inverse FFT is computed.  The scaling by 1/N is
 506   included in the inverse FFT.
 507
 508   The contents of the input X may be destroyed."
 509   (let* ((n (length x))
 510          (half-n (ash n -1))
 511          (pairs-in-group (ash n -1))
 512          (number-of-groups 1)
 513          (distance (ash n -1))
 514          (not-switch-input t)
 515          (sincos (sincos-table (maxima-fft:log-base2 n)))
 516          (a (copy-seq x))
 517          (b (make-array (length x))))
 518
 519     (flet ((fft ()
 520              (let ((index 0))
 521                (dotimes (k number-of-groups)
 522                  (let* ((jfirst (* 2 k pairs-in-group))
 523                         (jlast (+ jfirst pairs-in-group -1))
 524                         (jtwiddle (* k pairs-in-group))
 525                         (w (let ((w (aref sincos jtwiddle)))
 526                              (if inverse-fft-p
 527                                (conjugate w)
 528                                w))))
 529
 530                    (loop for j from jfirst upto jlast do
 531                      (let ((temp (* w (aref a (+ j distance)))))
 532                        (setf (aref b index) (+ (aref a j) temp))
 533                        (setf (aref b (+ index half-n)) (- (aref a j) temp))
 534                        (incf index))))))))
 535       (loop while (< number-of-groups n) do
 536         (fft)
 537         (rotatef a b)
 538         (setf not-switch-input (not not-switch-input))
 539         (setf pairs-in-group (ash pairs-in-group -1))
 540         (setf number-of-groups (ash number-of-groups 1))
 541         (setf distance (ash distance -1)))
 542       (if inverse-fft-p
 543           a
 544           (map-into a #'(lambda (z)
 545                                (/ z n))
 546                     a)))))
 547