sizeof() cleanup. the occasional void * didn't make this easier.
[platform/upstream/libvorbis.git] / lib / sharedbook.c
1 /********************************************************************
2  *                                                                  *
3  * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE.   *
4  * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
5  * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
6  * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
7  *                                                                  *
8  * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2001             *
9  * by the XIPHOPHORUS Company http://www.xiph.org/                  *
10
11  ********************************************************************
12
13  function: basic shared codebook operations
14  last mod: $Id: sharedbook.c,v 1.18 2001/10/02 00:14:32 segher Exp $
15
16  ********************************************************************/
17
18 #include <stdlib.h>
19 #include <math.h>
20 #include <string.h>
21 #include <ogg/ogg.h>
22 #include "os.h"
23 #include "vorbis/codec.h"
24 #include "codebook.h"
25 #include "scales.h"
26
27 /**** pack/unpack helpers ******************************************/
28 int _ilog(unsigned int v){
29   int ret=0;
30   while(v){
31     ret++;
32     v>>=1;
33   }
34   return(ret);
35 }
36
37 /* 32 bit float (not IEEE; nonnormalized mantissa +
38    biased exponent) : neeeeeee eeemmmmm mmmmmmmm mmmmmmmm 
39    Why not IEEE?  It's just not that important here. */
40
41 #define VQ_FEXP 10
42 #define VQ_FMAN 21
43 #define VQ_FEXP_BIAS 768 /* bias toward values smaller than 1. */
44
45 /* doesn't currently guard under/overflow */
46 long _float32_pack(float val){
47   int sign=0;
48   long exp;
49   long mant;
50   if(val<0){
51     sign=0x80000000;
52     val= -val;
53   }
54   exp= floor(log(val)/log(2));
55   mant=rint(ldexp(val,(VQ_FMAN-1)-exp));
56   exp=(exp+VQ_FEXP_BIAS)<<VQ_FMAN;
57
58   return(sign|exp|mant);
59 }
60
61 float _float32_unpack(long val){
62   double mant=val&0x1fffff;
63   int    sign=val&0x80000000;
64   long   exp =(val&0x7fe00000L)>>VQ_FMAN;
65   if(sign)mant= -mant;
66   return(ldexp(mant,exp-(VQ_FMAN-1)-VQ_FEXP_BIAS));
67 }
68
69 /* given a list of word lengths, generate a list of codewords.  Works
70    for length ordered or unordered, always assigns the lowest valued
71    codewords first.  Extended to handle unused entries (length 0) */
72 long *_make_words(long *l,long n){
73   long i,j;
74   long marker[33];
75   long *r=_ogg_malloc(n*sizeof(*r));
76   memset(marker,0,sizeof(marker));
77
78   for(i=0;i<n;i++){
79     long length=l[i];
80     if(length>0){
81       long entry=marker[length];
82       
83       /* when we claim a node for an entry, we also claim the nodes
84          below it (pruning off the imagined tree that may have dangled
85          from it) as well as blocking the use of any nodes directly
86          above for leaves */
87       
88       /* update ourself */
89       if(length<32 && (entry>>length)){
90         /* error condition; the lengths must specify an overpopulated tree */
91         _ogg_free(r);
92         return(NULL);
93       }
94       r[i]=entry;
95     
96       /* Look to see if the next shorter marker points to the node
97          above. if so, update it and repeat.  */
98       {
99         for(j=length;j>0;j--){
100           
101           if(marker[j]&1){
102             /* have to jump branches */
103             if(j==1)
104               marker[1]++;
105             else
106               marker[j]=marker[j-1]<<1;
107             break; /* invariant says next upper marker would already
108                       have been moved if it was on the same path */
109           }
110           marker[j]++;
111         }
112       }
113       
114       /* prune the tree; the implicit invariant says all the longer
115          markers were dangling from our just-taken node.  Dangle them
116          from our *new* node. */
117       for(j=length+1;j<33;j++)
118         if((marker[j]>>1) == entry){
119           entry=marker[j];
120           marker[j]=marker[j-1]<<1;
121         }else
122           break;
123     }    
124   }
125     
126   /* bitreverse the words because our bitwise packer/unpacker is LSb
127      endian */
128   for(i=0;i<n;i++){
129     long temp=0;
130     for(j=0;j<l[i];j++){
131       temp<<=1;
132       temp|=(r[i]>>j)&1;
133     }
134     r[i]=temp;
135   }
136
137   return(r);
138 }
139
140 /* build the decode helper tree from the codewords */
141 decode_aux *_make_decode_tree(codebook *c){
142   const static_codebook *s=c->c;
143   long top=0,i,j,n;
144   decode_aux *t=_ogg_malloc(sizeof(*t));
145   long *ptr0=t->ptr0=_ogg_calloc(c->entries*2,sizeof(*ptr0));
146   long *ptr1=t->ptr1=_ogg_calloc(c->entries*2,sizeof(*ptr1));
147   long *codelist=_make_words(s->lengthlist,s->entries);
148
149   if(codelist==NULL)return(NULL);
150   t->aux=c->entries*2;
151
152   for(i=0;i<c->entries;i++){
153     if(s->lengthlist[i]>0){
154       long ptr=0;
155       for(j=0;j<s->lengthlist[i]-1;j++){
156         int bit=(codelist[i]>>j)&1;
157         if(!bit){
158           if(!ptr0[ptr])
159             ptr0[ptr]= ++top;
160           ptr=ptr0[ptr];
161         }else{
162           if(!ptr1[ptr])
163             ptr1[ptr]= ++top;
164           ptr=ptr1[ptr];
165         }
166       }
167       if(!((codelist[i]>>j)&1))
168         ptr0[ptr]=-i;
169       else
170         ptr1[ptr]=-i;
171     }
172   }
173   _ogg_free(codelist);
174
175   t->tabn = _ilog(c->entries)-4; /* this is magic */
176   if(t->tabn<5)t->tabn=5;
177   n = 1<<t->tabn;
178   t->tab = _ogg_malloc(n*sizeof(*t->tab));
179   t->tabl = _ogg_malloc(n*sizeof(*t->tabl));
180   for (i = 0; i < n; i++) {
181     long p = 0;
182     for (j = 0; j < t->tabn && (p > 0 || j == 0); j++) {
183       if (i & (1 << j))
184         p = ptr1[p];
185       else
186         p = ptr0[p];
187     }
188     /* now j == length, and p == -code */
189     t->tab[i] = p;
190     t->tabl[i] = j;
191   }
192
193   return(t);
194 }
195
196 /* there might be a straightforward one-line way to do the below
197    that's portable and totally safe against roundoff, but I haven't
198    thought of it.  Therefore, we opt on the side of caution */
199 long _book_maptype1_quantvals(const static_codebook *b){
200   long vals=floor(pow(b->entries,1.f/b->dim));
201
202   /* the above *should* be reliable, but we'll not assume that FP is
203      ever reliable when bitstream sync is at stake; verify via integer
204      means that vals really is the greatest value of dim for which
205      vals^b->bim <= b->entries */
206   /* treat the above as an initial guess */
207   while(1){
208     long acc=1;
209     long acc1=1;
210     int i;
211     for(i=0;i<b->dim;i++){
212       acc*=vals;
213       acc1*=vals+1;
214     }
215     if(acc<=b->entries && acc1>b->entries){
216       return(vals);
217     }else{
218       if(acc>b->entries){
219         vals--;
220       }else{
221         vals++;
222       }
223     }
224   }
225 }
226
227 /* unpack the quantized list of values for encode/decode ***********/
228 /* we need to deal with two map types: in map type 1, the values are
229    generated algorithmically (each column of the vector counts through
230    the values in the quant vector). in map type 2, all the values came
231    in in an explicit list.  Both value lists must be unpacked */
232 float *_book_unquantize(const static_codebook *b){
233   long j,k;
234   if(b->maptype==1 || b->maptype==2){
235     int quantvals;
236     float mindel=_float32_unpack(b->q_min);
237     float delta=_float32_unpack(b->q_delta);
238     float *r=_ogg_calloc(b->entries*b->dim,sizeof(*r));
239
240     /* maptype 1 and 2 both use a quantized value vector, but
241        different sizes */
242     switch(b->maptype){
243     case 1:
244       /* most of the time, entries%dimensions == 0, but we need to be
245          well defined.  We define that the possible vales at each
246          scalar is values == entries/dim.  If entries%dim != 0, we'll
247          have 'too few' values (values*dim<entries), which means that
248          we'll have 'left over' entries; left over entries use zeroed
249          values (and are wasted).  So don't generate codebooks like
250          that */
251       quantvals=_book_maptype1_quantvals(b);
252       for(j=0;j<b->entries;j++){
253         float last=0.f;
254         int indexdiv=1;
255         for(k=0;k<b->dim;k++){
256           int index= (j/indexdiv)%quantvals;
257           float val=b->quantlist[index];
258           val=fabs(val)*delta+mindel+last;
259           if(b->q_sequencep)last=val;     
260           r[j*b->dim+k]=val;
261           indexdiv*=quantvals;
262         }
263       }
264       break;
265     case 2:
266       for(j=0;j<b->entries;j++){
267         float last=0.f;
268         for(k=0;k<b->dim;k++){
269           float val=b->quantlist[j*b->dim+k];
270           val=fabs(val)*delta+mindel+last;
271           if(b->q_sequencep)last=val;     
272           r[j*b->dim+k]=val;
273         }
274       }
275       break;
276     }
277
278     return(r);
279   }
280   return(NULL);
281 }
282
283 void vorbis_staticbook_clear(static_codebook *b){
284   if(b->allocedp){
285     if(b->quantlist)_ogg_free(b->quantlist);
286     if(b->lengthlist)_ogg_free(b->lengthlist);
287     if(b->nearest_tree){
288       _ogg_free(b->nearest_tree->ptr0);
289       _ogg_free(b->nearest_tree->ptr1);
290       _ogg_free(b->nearest_tree->p);
291       _ogg_free(b->nearest_tree->q);
292       memset(b->nearest_tree,0,sizeof(*b->nearest_tree));
293       _ogg_free(b->nearest_tree);
294     }
295     if(b->thresh_tree){
296       _ogg_free(b->thresh_tree->quantthresh);
297       _ogg_free(b->thresh_tree->quantmap);
298       memset(b->thresh_tree,0,sizeof(*b->thresh_tree));
299       _ogg_free(b->thresh_tree);
300     }
301
302     memset(b,0,sizeof(*b));
303   }
304 }
305
306 void vorbis_staticbook_destroy(static_codebook *b){
307   if(b->allocedp){
308     vorbis_staticbook_clear(b);
309     _ogg_free(b);
310   }
311 }
312
313 void vorbis_book_clear(codebook *b){
314   /* static book is not cleared; we're likely called on the lookup and
315      the static codebook belongs to the info struct */
316   if(b->decode_tree){
317     _ogg_free(b->decode_tree->tab);
318     _ogg_free(b->decode_tree->tabl);
319
320     _ogg_free(b->decode_tree->ptr0);
321     _ogg_free(b->decode_tree->ptr1);
322     memset(b->decode_tree,0,sizeof(*b->decode_tree));
323     _ogg_free(b->decode_tree);
324   }
325   if(b->valuelist)_ogg_free(b->valuelist);
326   if(b->codelist)_ogg_free(b->codelist);
327   memset(b,0,sizeof(*b));
328 }
329
330 int vorbis_book_init_encode(codebook *c,const static_codebook *s){
331   long j,k;
332   memset(c,0,sizeof(*c));
333   c->c=s;
334   c->entries=s->entries;
335   c->dim=s->dim;
336   c->codelist=_make_words(s->lengthlist,s->entries);
337   c->valuelist=_book_unquantize(s);
338
339   /* set the 'zero entry' */
340   c->zeroentry=-1;
341   if(c->valuelist){
342     for(j=0;j<s->entries;j++){
343       int flag=1;
344       for(k=0;k<s->dim;k++){
345         if(fabs(c->valuelist[j*s->dim+k])>1e-12f){
346           flag=0;
347           break;
348         }
349       }
350       if(flag)
351         c->zeroentry=j;
352     }
353   }
354
355   return(0);
356 }
357
358 int vorbis_book_init_decode(codebook *c,const static_codebook *s){
359   memset(c,0,sizeof(*c));
360   c->c=s;
361   c->entries=s->entries;
362   c->dim=s->dim;
363   c->valuelist=_book_unquantize(s);
364   c->decode_tree=_make_decode_tree(c);
365   if(c->decode_tree==NULL)goto err_out;
366   return(0);
367  err_out:
368   vorbis_book_clear(c);
369   return(-1);
370 }
371
372 static float _dist(int el,float *ref, float *b,int step){
373   int i;
374   float acc=0.f;
375   for(i=0;i<el;i++){
376     float val=(ref[i]-b[i*step]);
377     acc+=val*val;
378   }
379   return(acc);
380 }
381
382 #include <stdio.h>
383 int _best(codebook *book, float *a, int step){
384   encode_aux_nearestmatch *nt=book->c->nearest_tree;
385   encode_aux_threshmatch *tt=book->c->thresh_tree;
386   encode_aux_pigeonhole *pt=book->c->pigeon_tree;
387   int dim=book->dim;
388   int ptr=0,k,o;
389   /*int savebest=-1;
390     float saverr;*/
391
392   /* do we have a threshhold encode hint? */
393   if(tt){
394     int index=0;
395     /* find the quant val of each scalar */
396     for(k=0,o=step*(dim-1);k<dim;k++,o-=step){
397       int i;
398       /* linear search the quant list for now; it's small and although
399          with > ~8 entries, it would be faster to bisect, this would be
400          a misplaced optimization for now */
401       for(i=0;i<tt->threshvals-1;i++)
402         if(a[o]<tt->quantthresh[i])break;
403
404       index=(index*tt->quantvals)+tt->quantmap[i];
405     }
406     /* regular lattices are easy :-) */
407     if(book->c->lengthlist[index]>0) /* is this unused?  If so, we'll
408                                         use a decision tree after all
409                                         and fall through*/
410       return(index);
411   }
412
413   /* do we have a pigeonhole encode hint? */
414   if(pt){
415     const static_codebook *c=book->c;
416     int i,besti=-1;
417     float best;
418     int entry=0;
419
420     /* dealing with sequentialness is a pain in the ass */
421     if(c->q_sequencep){
422       int pv;
423       long mul=1;
424       float qlast=0;
425       for(k=0,o=0;k<dim;k++,o+=step){
426         pv=(int)((a[o]-qlast-pt->min)/pt->del);
427         if(pv<0 || pv>=pt->mapentries)break;
428         entry+=pt->pigeonmap[pv]*mul;
429         mul*=pt->quantvals;
430         qlast+=pv*pt->del+pt->min;
431       }
432     }else{
433       for(k=0,o=step*(dim-1);k<dim;k++,o-=step){
434         int pv=(int)((a[o]-pt->min)/pt->del);
435         if(pv<0 || pv>=pt->mapentries)break;
436         entry=entry*pt->quantvals+pt->pigeonmap[pv];
437       }
438     }
439
440     /* must be within the pigeonholable range; if we quant outside (or
441        in an entry that we define no list for), brute force it */
442     if(k==dim && pt->fitlength[entry]){
443       /* search the abbreviated list */
444       long *list=pt->fitlist+pt->fitmap[entry];
445       for(i=0;i<pt->fitlength[entry];i++){
446         float this=_dist(dim,book->valuelist+list[i]*dim,a,step);
447         if(besti==-1 || this<best){
448           best=this;
449           besti=list[i];
450         }
451       }
452
453       return(besti); 
454     }
455   }
456
457   if(nt){
458     /* optimized using the decision tree */
459     while(1){
460       float c=0.f;
461       float *p=book->valuelist+nt->p[ptr];
462       float *q=book->valuelist+nt->q[ptr];
463       
464       for(k=0,o=0;k<dim;k++,o+=step)
465         c+=(p[k]-q[k])*(a[o]-(p[k]+q[k])*.5);
466       
467       if(c>0.f) /* in A */
468         ptr= -nt->ptr0[ptr];
469       else     /* in B */
470         ptr= -nt->ptr1[ptr];
471       if(ptr<=0)break;
472     }
473     return(-ptr);
474   }
475
476   /* brute force it! */
477   {
478     const static_codebook *c=book->c;
479     int i,besti=-1;
480     float best;
481     float *e=book->valuelist;
482     for(i=0;i<book->entries;i++){
483       if(c->lengthlist[i]>0){
484         float this=_dist(dim,e,a,step);
485         if(besti==-1 || this<best){
486           best=this;
487           besti=i;
488         }
489       }
490       e+=dim;
491     }
492
493     /*if(savebest!=-1 && savebest!=besti){
494       fprintf(stderr,"brute force/pigeonhole disagreement:\n"
495               "original:");
496       for(i=0;i<dim*step;i+=step)fprintf(stderr,"%g,",a[i]);
497       fprintf(stderr,"\n"
498               "pigeonhole (entry %d, err %g):",savebest,saverr);
499       for(i=0;i<dim;i++)fprintf(stderr,"%g,",
500                                 (book->valuelist+savebest*dim)[i]);
501       fprintf(stderr,"\n"
502               "bruteforce (entry %d, err %g):",besti,best);
503       for(i=0;i<dim;i++)fprintf(stderr,"%g,",
504                                 (book->valuelist+besti*dim)[i]);
505       fprintf(stderr,"\n");
506       }*/
507     return(besti);
508   }
509 }
510
511 /* returns the entry number and *modifies a* to the remainder value ********/
512 int vorbis_book_besterror(codebook *book,float *a,int step,int addmul){
513   int dim=book->dim,i,o;
514   int best=_best(book,a,step);
515   switch(addmul){
516   case 0:
517     for(i=0,o=0;i<dim;i++,o+=step)
518       a[o]-=(book->valuelist+best*dim)[i];
519     break;
520   case 1:
521     for(i=0,o=0;i<dim;i++,o+=step){
522       float val=(book->valuelist+best*dim)[i];
523       if(val==0){
524         a[o]=0;
525       }else{
526         a[o]/=val;
527       }
528     }
529     break;
530   }
531   return(best);
532 }
533
534 long vorbis_book_codeword(codebook *book,int entry){
535   return book->codelist[entry];
536 }
537
538 long vorbis_book_codelen(codebook *book,int entry){
539   return book->c->lengthlist[entry];
540 }
541
542 #ifdef _V_SELFTEST
543
544 /* Unit tests of the dequantizer; this stuff will be OK
545    cross-platform, I simply want to be sure that special mapping cases
546    actually work properly; a bug could go unnoticed for a while */
547
548 #include <stdio.h>
549
550 /* cases:
551
552    no mapping
553    full, explicit mapping
554    algorithmic mapping
555
556    nonsequential
557    sequential
558 */
559
560 static long full_quantlist1[]={0,1,2,3,    4,5,6,7, 8,3,6,1};
561 static long partial_quantlist1[]={0,7,2};
562
563 /* no mapping */
564 static_codebook test1={
565   4,16,
566   NULL,
567   0,
568   0,0,0,0,
569   NULL,
570   NULL,NULL
571 };
572 static float *test1_result=NULL;
573   
574 /* linear, full mapping, nonsequential */
575 static_codebook test2={
576   4,3,
577   NULL,
578   2,
579   -533200896,1611661312,4,0,
580   full_quantlist1,
581   NULL,NULL
582 };
583 static float test2_result[]={-3,-2,-1,0, 1,2,3,4, 5,0,3,-2};
584
585 /* linear, full mapping, sequential */
586 static_codebook test3={
587   4,3,
588   NULL,
589   2,
590   -533200896,1611661312,4,1,
591   full_quantlist1,
592   NULL,NULL
593 };
594 static float test3_result[]={-3,-5,-6,-6, 1,3,6,10, 5,5,8,6};
595
596 /* linear, algorithmic mapping, nonsequential */
597 static_codebook test4={
598   3,27,
599   NULL,
600   1,
601   -533200896,1611661312,4,0,
602   partial_quantlist1,
603   NULL,NULL
604 };
605 static float test4_result[]={-3,-3,-3, 4,-3,-3, -1,-3,-3,
606                               -3, 4,-3, 4, 4,-3, -1, 4,-3,
607                               -3,-1,-3, 4,-1,-3, -1,-1,-3, 
608                               -3,-3, 4, 4,-3, 4, -1,-3, 4,
609                               -3, 4, 4, 4, 4, 4, -1, 4, 4,
610                               -3,-1, 4, 4,-1, 4, -1,-1, 4,
611                               -3,-3,-1, 4,-3,-1, -1,-3,-1,
612                               -3, 4,-1, 4, 4,-1, -1, 4,-1,
613                               -3,-1,-1, 4,-1,-1, -1,-1,-1};
614
615 /* linear, algorithmic mapping, sequential */
616 static_codebook test5={
617   3,27,
618   NULL,
619   1,
620   -533200896,1611661312,4,1,
621   partial_quantlist1,
622   NULL,NULL
623 };
624 static float test5_result[]={-3,-6,-9, 4, 1,-2, -1,-4,-7,
625                               -3, 1,-2, 4, 8, 5, -1, 3, 0,
626                               -3,-4,-7, 4, 3, 0, -1,-2,-5, 
627                               -3,-6,-2, 4, 1, 5, -1,-4, 0,
628                               -3, 1, 5, 4, 8,12, -1, 3, 7,
629                               -3,-4, 0, 4, 3, 7, -1,-2, 2,
630                               -3,-6,-7, 4, 1, 0, -1,-4,-5,
631                               -3, 1, 0, 4, 8, 7, -1, 3, 2,
632                               -3,-4,-5, 4, 3, 2, -1,-2,-3};
633
634 void run_test(static_codebook *b,float *comp){
635   float *out=_book_unquantize(b);
636   int i;
637
638   if(comp){
639     if(!out){
640       fprintf(stderr,"_book_unquantize incorrectly returned NULL\n");
641       exit(1);
642     }
643
644     for(i=0;i<b->entries*b->dim;i++)
645       if(fabs(out[i]-comp[i])>.0001){
646         fprintf(stderr,"disagreement in unquantized and reference data:\n"
647                 "position %d, %g != %g\n",i,out[i],comp[i]);
648         exit(1);
649       }
650
651   }else{
652     if(out){
653       fprintf(stderr,"_book_unquantize returned a value array: \n"
654               " correct result should have been NULL\n");
655       exit(1);
656     }
657   }
658 }
659
660 int main(){
661   /* run the nine dequant tests, and compare to the hand-rolled results */
662   fprintf(stderr,"Dequant test 1... ");
663   run_test(&test1,test1_result);
664   fprintf(stderr,"OK\nDequant test 2... ");
665   run_test(&test2,test2_result);
666   fprintf(stderr,"OK\nDequant test 3... ");
667   run_test(&test3,test3_result);
668   fprintf(stderr,"OK\nDequant test 4... ");
669   run_test(&test4,test4_result);
670   fprintf(stderr,"OK\nDequant test 5... ");
671   run_test(&test5,test5_result);
672   fprintf(stderr,"OK\n\n");
673   
674   return(0);
675 }
676
677 #endif