xue@1: //---------------------------------------------------------------------------
xue@1: #include <math.h>
xue@1: #include "hs.h"
xue@1: #include "align8.h"
xue@1: #include "arrayalloc.h"
xue@1: #include "procedures.h"
Chris@2: #include "sinest.h"
Chris@2: #include "sinsyn.h"
xue@1: #include "splines.h"
xue@1: #include "xcomplex.h"
xue@1: #ifdef I
xue@1: #undef I
xue@1: #endif
xue@1: 
Chris@5: /** \file hs.h */
Chris@5: 
xue@1: //---------------------------------------------------------------------------
xue@1: //stiffcandid methods
xue@1: 
Chris@5: /**
xue@1:   method stiffcandid::stiffcandid: creates a harmonic atom with minimal info, i.e. fundamantal frequency
xue@1:   between 0 and Nyqvist frequency, stiffness coefficient between 0 and STIFF_B_MAX.
xue@1: 
xue@1:   In: Wid: DFT size (frame size, frequency resolution)
xue@1: */
xue@1: stiffcandid::stiffcandid(int Wid)
xue@1: {
xue@1:   F=new double[6];
xue@1:   G=&F[3];
xue@1:   double Fm=Wid*Wid/4; //NyqvistF^2
xue@1:   F[0]=0, G[0]=0;
xue@1:   F[1]=Fm, G[1]=STIFF_B_MAX*Fm;
xue@1:   F[2]=Fm, G[2]=0;
xue@1:   N=3;
xue@1:   P=0;
xue@1:   s=0;
xue@1:   p=NULL;
xue@1:   f=NULL;
xue@1: }//stiffcandid
xue@1: 
Chris@5: /**
xue@1:   method stiffcandid::stiffcandid: creates a harmonic atom with a frequency range for the $ap-th partial,
xue@1:   without actually taking this partial as "known".
xue@1: 
xue@1:   In; Wid: DFT size
xue@1:       ap: partial index
xue@1:       af, errf: centre and half-width of the frequency range of the ap-th partial, in bins
xue@1: */
xue@1: stiffcandid::stiffcandid(int Wid, int ap, double af, double errf)
xue@1: {
xue@1:   F=new double[10];
xue@1:   G=&F[5];
xue@1:   double Fm=Wid*Wid/4;
xue@1:   F[0]=0, G[0]=0;
xue@1:   F[1]=Fm, G[1]=STIFF_B_MAX*Fm;
xue@1:   F[2]=Fm, G[2]=0;
xue@1:   N=3;
xue@1:   P=0;
xue@1:   s=0;
xue@1:   p=NULL;
xue@1:   f=NULL;
xue@1:   double k=ap*ap-1;
xue@1:   double g1=(af-errf)/ap, g2=(af+errf)/ap;
xue@1:   if (g1<0) g1=0;
xue@1:   g1=g1*g1, g2=g2*g2;
xue@1:   //apply constraints F+kG-g2<0 and -F-kG+g1<0
xue@1:   cutcvpoly(N, F, G, 1, k, -g2);
xue@1:   cutcvpoly(N, F, G, -1, -k, g1);
xue@1: }//stiffcandid
xue@1: 
Chris@5: /**
xue@1:   method stiffcandid::stiffcandid: creates a harmonic atom from one atom.
xue@1: 
xue@1:   In; Wid: DFT size
xue@1:       ap: partial index of the atom.
xue@1:       af, errf: centre and half-width of the frequency range of the atom, in bins.
xue@1:       ds: contribution to candidate score from this atom.
xue@1: */
xue@1: stiffcandid::stiffcandid(int Wid, int ap, double af, double errf, double ds)
xue@1: {
xue@1:   //the starting polygon implements the trivial constraints 0<f0<NyqvistF,
xue@1:   //  0<B<Bmax. in terms of F and G, 0<F<NF^2, 0<G<FBmax. this is a triangle
xue@1:   //  with vertices (0, 0), (NF^2, Bmax*NF^2), (NF^2, 0)
xue@1:   F=new double[12];
xue@1:   G=&F[5], f=&F[10], p=(int*)&F[11];
xue@1:   double Fm=Wid*Wid/4; //NyqvistF^2
xue@1:   F[0]=0, G[0]=0;
xue@1:   F[1]=Fm, G[1]=STIFF_B_MAX*Fm;
xue@1:   F[2]=Fm, G[2]=0;
xue@1: 
xue@1:   N=3;
xue@1:   P=1;
xue@1:   p[0]=ap;
xue@1:   f[0]=af;
xue@1:   s=ds;
xue@1: 
xue@1:   double k=ap*ap-1;
xue@1:   double g1=(af-errf)/ap, g2=(af+errf)/ap;
xue@1:   if (g1<0) g1=0;
xue@1:   g1=g1*g1, g2=g2*g2;
xue@1:   //apply constraints F+kG-g2<0 and -F-kG+g1<0
xue@1:   cutcvpoly(N, F, G, 1, k, -g2);
xue@1:   cutcvpoly(N, F, G, -1, -k, g1);
xue@1: }//stiffcandid
xue@1: 
xue@1: /*
xue@1:   method stiffcandid::stiffcandid: creates a harmonic atom by adding a new partial to an existing
xue@1:   harmonic atom.
xue@1: 
xue@1:   In; prev: initial harmonic atom
xue@1:       ap: partial index of new atom
xue@1:       af, errf: centre and half-width of the frequency range of the new atom, in bins.
xue@1:       ds: contribution to candidate score from this atom.
xue@1: */
xue@1: stiffcandid::stiffcandid(stiffcandid* prev, int ap, double af, double errf, double ds)
xue@1: {
xue@1:   N=prev->N, P=prev->P, s=prev->s;
xue@1:   int Np2=N+2, Pp1=P+1;
xue@1:   F=new double[(Np2+Pp1)*2];
xue@1:   G=&F[Np2]; f=&G[Np2]; p=(int*)&f[Pp1];
xue@1:   memcpy(F, prev->F, sizeof(double)*N);
xue@1:   memcpy(G, prev->G, sizeof(double)*N);
xue@1:   if (P>0)
xue@1:   {
xue@1:     memcpy(f, prev->f, sizeof(double)*P);
xue@1:     memcpy(p, prev->p, sizeof(int)*P);
xue@1:   }
xue@1:   //see if partial ap is already involved
xue@1:   int i=0; while (i<P && p[i]!=ap) i++;
xue@1:   //if it is not:
xue@1:   if (i>=P)
xue@1:   {
xue@1:     p[P]=ap;
xue@1:     f[P]=af;
xue@1:     s+=ds;
xue@1:     P++;
xue@1: 
xue@1:     double k=ap*ap-1;
xue@1:     double g1=(af-errf)/ap, g2=(af+errf)/ap;
xue@1:     if (g1<0) g1=0;
xue@1:     g1=g1*g1, g2=g2*g2;
xue@1:     //apply constraints F+kG-g2<0 and -F-kG+g1<0
xue@1:     cutcvpoly(N, F, G, 1, k, -g2);
xue@1:     cutcvpoly(N, F, G, -1, -k, g1);
xue@1:   }
xue@1:   //otherwise, the new candid is simply a copy of prev
xue@1: }//stiffcandid
xue@1: 
xue@1: //method stiffcandid::~stiffcandid: destructor
xue@1: stiffcandid::~stiffcandid()
xue@1: {
xue@1:   delete[] F;
xue@1: }//~stiffcandid
xue@1: 
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: //THS methods
xue@1: 
xue@1: //method THS::THS: default constructor
xue@1: THS::THS()
xue@1: {
xue@1:   memset(this, 0, sizeof(THS));
xue@1:   memset(BufM, 0, sizeof(void*)*128);
xue@1: }//THS
xue@1: 
xue@1: /*
xue@1:   method THS::THS: creates an HS with given number of partials and frames.
xue@1: 
xue@1:   In: aM, aFr: number of partials and frames
xue@1: */
xue@1: THS::THS(int aM, int aFr)
xue@1: {
xue@1:   memset(this, 0, sizeof(THS));
xue@1:   M=aM; Fr=aFr; Allocate2(atom, M, Fr, Partials);
xue@1:   memset(Partials[0], 0, sizeof(atom)*M*Fr);
xue@1:   memset(BufM, 0, sizeof(void*)*128);
xue@1: }//THS
xue@1: 
xue@1: /*
xue@1:   method THS::THS: creates a duplicate HS. This does not duplicate contents of BufM.
xue@1: 
xue@1:   In: HS: the HS to duplicate
xue@1: */
xue@1: THS::THS(THS* HS)
xue@1: {
xue@1:   memcpy(this, HS, sizeof(THS));
xue@1:   Allocate2(atom, M, Fr, Partials);
xue@1:   for (int m=0; m<M; m++) memcpy(Partials[m], HS->Partials[m], sizeof(atom)*Fr);
xue@1:   Allocate2(double, M, st_count, startamp);
xue@1:   if (st_count) for (int m=0; m<M; m++) memcpy(startamp[m], HS->startamp[m], sizeof(double)*st_count);
xue@1:   memset(BufM, 0, sizeof(void*)*128);
xue@1: }//THS
xue@1: 
xue@1: /*
xue@1:   method THS::THS: create a new HS which is a trunction of an existing HS.
xue@1: 
xue@1:   In: HS: the exinting HS from which a sub-HS is to be created
xue@1:       start, end: truncation points. Atoms of *HS beyond these points are discarded.
xue@1: */
xue@1: THS::THS(THS* HS, double start, double end)
xue@1: {
xue@1:   memset(this, 0, sizeof(THS));
xue@1:   M=HS->M; Fr=HS->Fr; isconstf=HS->isconstf;
xue@1:   int frst=0, fren=Fr;
xue@1:   while (HS->Partials[0][frst].t<start && frst<fren) frst++;
xue@1:   while (HS->Partials[0][fren-1].t>end && fren>frst) fren--;
xue@1:   Fr=fren-frst;
xue@1:   Allocate2(atom, M, Fr, Partials);
xue@1:   for (int m=0; m<M; m++)
xue@1:   {
xue@1:     memcpy(Partials[m], &HS->Partials[m][frst], sizeof(atom)*Fr);
xue@1:     for (int fr=0; fr<Fr; fr++) Partials[m][fr].t-=start;
xue@1:   }
xue@1:   if (frst>0){DeAlloc2(startamp); startamp=0;}
xue@1: }//THS
xue@1: 
xue@1: //method THS::~THS: default descructor.
xue@1: THS::~THS()
xue@1: {
xue@1:   DeAlloc2(Partials);
xue@1:   DeAlloc2(startamp);
xue@1:   ClearBufM();
xue@1: }//~THS
xue@1: 
xue@1: /*
xue@1:   method THS::ClearBufM: free buffers pointed to by members of BufM.
xue@1: */
xue@1: void THS::ClearBufM()
xue@1: {
xue@1:   for (int m=0; m<128; m++) delete[] BufM[m];
xue@1:   memset(BufM, 0, sizeof(void*)*128);
xue@1: }//ClearBufM
xue@1: 
xue@1: /*
xue@1:   method THS::EndPos: the end position of an HS.
xue@1: 
xue@1:   Returns the time of the last frame of the first partial.
xue@1: */
xue@1: int THS::EndPos()
xue@1: {
xue@1:   return Partials[0][Fr-1].t;
xue@1: }//EndPos
xue@1: 
xue@1: /*
xue@1:   method THS::EndPosEx: the extended end position of an HS.
xue@1: 
xue@1:   Returns the time later than the last frame of the first partial by the interval between the first and
xue@1:   second frames of that partial.
xue@1: */
xue@1: int THS::EndPosEx()
xue@1: {
xue@1:   return Partials[0][Fr-1].t+Partials[0][1].t-Partials[0][0].t;
xue@1: }//EndPosEx
xue@1: 
xue@1: /*
xue@1:   method THS::ReadAtomFromStream: reads an atom from a stream.
xue@1: 
xue@1:   Returns the number of bytes read, or 0 on failure. The stream pointer is shifted by this value.
xue@1: */
xue@1: int THS::ReadAtomFromStream(TStream* Stream, atom* atm)
xue@1: {
xue@1:   int StartPosition=Stream->Position, atomlen; char s[4];
xue@1:   if (Stream->Read(s, 4)!=4) goto ret0;
xue@1:   if (strcmp(s, "ATM")) goto ret0;
xue@1:   if (Stream->Read(&atomlen, sizeof(int))!=sizeof(int)) goto ret0;
xue@1:   if (Stream->Read(atm, atomlen)!=atomlen) goto ret0;
xue@1:   return atomlen+4+sizeof(int);
xue@1: ret0:
xue@1:   Stream->Position=StartPosition;
xue@1:   return 0;
xue@1: }//ReadAtomFromStream
xue@1: 
xue@1: /*
xue@1:   method THS::ReadFromStream: reads an HS from a stream.
xue@1: 
xue@1:   Returns the number of bytes read, or 0 on failure. The stream pointer is shifted by this value.
xue@1: */
xue@1: int THS::ReadFromStream(TStream* Stream)
xue@1: {
xue@1:   int StartPosition=Stream->Position, lenevt; char s[4];
xue@1:   if (Stream->Read(s, 4)!=4) goto ret0;
xue@1:   if (strcmp(s, "EVT")) goto ret0;
xue@1:   if (Stream->Read(&lenevt, sizeof(int))!=sizeof(int)) goto ret0;
xue@1:   if (!ReadHdrFromStream(Stream)) goto ret0;
xue@1:   Resize(M, Fr, false, true);
xue@1:   for (int m=0; m<M; m++) for (int fr=0; fr<Fr; fr++) if (!ReadAtomFromStream(Stream, &Partials[m][fr])) goto ret0;
xue@1:   lenevt=lenevt+4+sizeof(int);
xue@1:   if (lenevt!=Stream->Position-StartPosition) goto ret0;
xue@1:   return lenevt;
xue@1: ret0:
xue@1:   Stream->Position=StartPosition;
xue@1:   return 0;
xue@1: }//ReadFromStream
xue@1: 
xue@1: /*
xue@1:   method THS::ReadHdrFromStream: reads header from a stream into this HS.
xue@1: 
xue@1:   Returns the number of bytes read, or 0 on failure. The stream pointer is shifted by this value.
xue@1: */
xue@1: int THS::ReadHdrFromStream(TStream* Stream)
xue@1: {
xue@1:   int StartPosition=Stream->Position, hdrlen, tags; char s[4];
xue@1:   if (Stream->Read(s, 4)!=4) goto ret0;
xue@1:   if (strcmp(s, "HDR")) goto ret0;
xue@1:   if (Stream->Read(&hdrlen, sizeof(int))!=sizeof(int)) goto ret0;
xue@1:   if (Stream->Read(&Channel, sizeof(int))!=sizeof(int)) goto ret0;
xue@1:   if (Stream->Read(&M, sizeof(int))!=sizeof(int)) goto ret0;
xue@1:   if (Stream->Read(&Fr, sizeof(int))!=sizeof(int)) goto ret0;
xue@1:   if (Stream->Read(&tags, sizeof(int))!=sizeof(int)) goto ret0;
xue@1:   isconstf=tags&HS_CONSTF;
xue@1:   hdrlen=hdrlen+4+sizeof(int); //expected read count
xue@1:   if (hdrlen!=Stream->Position-StartPosition) goto ret0;
xue@1:   return hdrlen;
xue@1: ret0:
xue@1:   Stream->Position=StartPosition;
xue@1:   return 0;
xue@1: }//ReadHdrFromStream
xue@1: 
xue@1: /*
xue@1:   method THS::Resize: change the number of partials or frames of an HS structure. Unless forcealloc is
xue@1:   set to true, this allocates new buffers to host HS data only when the new dimensions are larger than
xue@1:   current ones.
xue@1: 
xue@1:   In: aM, aFr: new number of partials and frames
xue@1:       copydata: specifies if HS data is to be copied to new buffers.
xue@1:       forcealloc: specifies if new buffers are to be allocated in all cases.
xue@1: */
xue@1: void THS::Resize(int aM, int aFr, bool copydata, bool forcealloc)
xue@1: {
xue@1:   if (forcealloc || M<aM || Fr<aFr)
xue@1:   {
xue@1:     atom** Allocate2(atom, aM, aFr, NewPartials);
xue@1:     if (copydata)
xue@1:     {
xue@1:       int minM=(M<aM)?M:aM, minFr=(Fr<aFr)?Fr:aFr;
xue@1:       for (int m=0; m<minM; m++) memcpy(NewPartials[m], Partials[m], sizeof(atom)*minFr);
xue@1:     }
xue@1:     DeAlloc2(Partials);
xue@1:     Partials=NewPartials;
xue@1:   }
xue@1:   if (forcealloc || M<aM)
xue@1:   {
xue@1:     double** Allocate2(double, aM, st_count, newstartamp);
xue@1:     if (copydata)
xue@1:     {
xue@1:       for (int m=0; m<M; m++) memcpy(newstartamp[m], startamp[m], sizeof(double)*st_count);
xue@1:     }
xue@1:     DeAlloc2(startamp);
xue@1:     startamp=newstartamp;
xue@1:   }
xue@1:   M=aM, Fr=aFr;
xue@1: }//Resize
xue@1: 
xue@1: /*
xue@1:   method THS::StartPos: the start position of an HS.
xue@1: 
xue@1:   Returns the time of the first frame of the first partial.
xue@1: */
xue@1: int THS::StartPos()
xue@1: {
xue@1:   return Partials[0][0].t;
xue@1: }//StartPos
xue@1: 
xue@1: /*
xue@1:   method THS::StdOffst: standard hop size of an HS.
xue@1:   Returns the interval between the timing of the first and second frames of the first partial, which is
xue@1:   the "standard" hop size if all measurement points are uniformly positioned.
xue@1: */
xue@1: int THS::StdOffst()
xue@1: {
xue@1:   return Partials[0][1].t-Partials[0][0].t;
xue@1: }//StdOffst
xue@1: 
xue@1: /*
xue@1:   method THS::WriteAtomToStream: writes an atom to a stream. An atom is stored in a stream as an RIFF
xue@1:   chunk identified by "ATM\0".
xue@1: 
xue@1:   Returns the number of bytes written. The stream pointer is shifted by this value.
xue@1: */
xue@1: int THS::WriteAtomToStream(TStream* Stream, atom* atm)
xue@1: {
xue@1:   Stream->Write((void*)"ATM", 4);
xue@1:   int atomlen=sizeof(atom);
xue@1:   Stream->Write(&atomlen, sizeof(int));
xue@1:   atomlen=Stream->Write(atm, atomlen);
xue@1:   return atomlen+4+sizeof(int);
xue@1: }//WriteAtomToStream
xue@1: 
xue@1: /*
xue@1:   method THS::WriteHdrToStream: writes header to a stream. HS header is stored in a stream as an RIFF
xue@1:   chunk, identified by "HDR\0".
xue@1: 
xue@1:   Returns the number of bytes written. The stream pointer is shifted by this value.
xue@1: */
xue@1: int THS::WriteHdrToStream(TStream* Stream)
xue@1: {
xue@1:   Stream->Write((void*)"HDR", 4);
xue@1:   int hdrlen=0, tags=0;
xue@1:   if (isconstf) tags=tags|HS_CONSTF;
xue@1:   Stream->Write(&hdrlen, sizeof(int));
xue@1:   Stream->Write(&Channel, sizeof(int)); hdrlen+=sizeof(int);
xue@1:   Stream->Write(&M, sizeof(int));  hdrlen+=sizeof(int);
xue@1:   Stream->Write(&Fr, sizeof(int)); hdrlen+=sizeof(int);
xue@1:   Stream->Write(&tags, sizeof(int)); hdrlen+=sizeof(int);
xue@1:   Stream->Seek(-hdrlen-sizeof(int), soFromCurrent); Stream->Write(&hdrlen, sizeof(int));
xue@1:   Stream->Seek(hdrlen, soFromCurrent);
xue@1:   return hdrlen+4+sizeof(int);
xue@1: }//WriteHdrToStream
xue@1: 
xue@1: /*
xue@1:   method THS::WriteToStream: write an HS to a stream. An HS is stored in a stream as an RIFF chunk
xue@1:   identified by "EVT\0". Its chunk data contains an HS header chunk followed by M*Fr atom chunks, in
xue@1:   rising order of m then fr.
xue@1: 
xue@1:   Returns the number of bytes written. The stream pointer is shifted by this value.
xue@1: */
xue@1: int THS::WriteToStream(TStream* Stream)
xue@1: {
xue@1:   Stream->Write((void*)"EVT", 4);
xue@1:   int lenevt=0;
xue@1:   Stream->Write(&lenevt, sizeof(int));
xue@1:   lenevt+=WriteHdrToStream(Stream);
xue@1: 
xue@1:   for (int m=0; m<M; m++) for (int fr=0; fr<Fr; fr++) lenevt+=WriteAtomToStream(Stream, &Partials[m][fr]);
xue@1:   Stream->Seek(-lenevt-sizeof(int), soFromCurrent);
xue@1:   Stream->Write(&lenevt, sizeof(int));
xue@1:   Stream->Seek(lenevt, soFromCurrent);
xue@1:   return lenevt+4+sizeof(int);
xue@1: }//WriteToStream
xue@1: 
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: //TPolygon methods
xue@1: 
xue@1: /*
xue@1:   method TPolygon::TPolygon: create an empty polygon with a storage capacity
xue@1: 
xue@1:   In: cap: number of vertices to allocate memory for.
xue@1: */
xue@1: TPolygon::TPolygon(int cap)
xue@1: {
xue@1:   X=(double*)malloc8(sizeof(double)*cap*2);
xue@1:   Y=&X[cap];
xue@1: }//TPolygon
xue@1: 
xue@1: /*
xue@1:   method TPolygon::TPolygon: create a duplicate polygon.
xue@1: 
xue@1:   In: cap: number of vertices to allocate memory for.
xue@1:       R: the polygon to duplicate.
xue@1: 
xue@1:   If cap is smaller than the number of vertices of R, the latter is used for memory allocation.
xue@1: */
xue@1: TPolygon::TPolygon(int cap, TPolygon* R)
xue@1: {
xue@1:   if (cap<R->N) cap=R->N; X=(double*)malloc8(sizeof(double)*cap*2); Y=&X[cap]; N=R->N;
xue@1:   memcpy(X, R->X, sizeof(double)*R->N); memcpy(Y, R->Y, sizeof(double)*R->N);
xue@1: }//TPolygon
xue@1: 
xue@1: //method TPolygon::~TPolygon: default destructor.
xue@1: TPolygon::~TPolygon()
xue@1: {
xue@1:   free8(X);
xue@1: }//~TPolygon
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: //TTempAtom methods
xue@1: 
xue@1: /*
xue@1:   method TTempAtom::TTempAtom: initializes an empty atom with a fundamental frequency range.
xue@1: 
xue@1:   In: af, ef: centre and half width of the fundamental frequency
xue@1:       maxB: upper bound of stiffness coefficient
xue@1: */
xue@1: TTempAtom::TTempAtom(double af, double ef, double maxB)
xue@1: {
xue@1:   Prev=NULL; pind=f=a=s=acce=0;
xue@1:   R=new TPolygon(4);
xue@1:   InitializeR(R, af, ef, maxB);
xue@1: }//TTempAtom
xue@1: 
xue@1: /*
xue@1:   method TTempAtom::TTempAtom: initialize an empty atom with a frequency range for a certain partial.
xue@1: 
xue@1:   In: apin: partial index
xue@1:       af, ef: centre and half width of the fundamental frequency
xue@1:       maxB: upper bound of stiffness coefficient
xue@1: */
xue@1: TTempAtom::TTempAtom(int apin, double af, double ef, double maxB)
xue@1: {
xue@1:   Prev=NULL; pind=f=a=s=acce=0;
xue@1:   R=new TPolygon(4);
xue@1:   InitializeR(R, apin, af, ef, maxB);
xue@1: }//TTempAtom
xue@1: 
xue@1: /*
xue@1:   method TTempAtom::TTempAtom: initialize an empty atom whose F-G polygon is the extension of AR
xue@1: 
xue@1: In: AR: the F-G polygon to extend
xue@1:       delf1, delf2: amount of extension, in semitones, of fundamental frequency
xue@1:       minf: minimal fundamental frequency: extension will not go below this value
xue@1: */
xue@1: TTempAtom::TTempAtom(TPolygon* AR, double delf1, double delf2, double minf)
xue@1: {
xue@1:   Prev=NULL; pind=f=a=s=acce=0;
xue@1:   R=new TPolygon(AR->N+2);
xue@1:   R->N=AR->N;
xue@1:   memcpy(R->X, AR->X, sizeof(double)*AR->N); memcpy(R->Y, AR->Y, sizeof(double)*AR->N);
xue@1:   ExtendR(R, delf1, delf2, minf);
xue@1: }//TTempAtom
xue@1: 
xue@1: /*
xue@1:   method TTempAtom::TTempAtom: initialize a atom as the only partial.
xue@1: 
xue@1:   In: apind: partial index
xue@1:       af, ef: partial frequency and its error range
xue@1:       aa, as: partial amplitude and measurement scale
xue@1:       maxB: stiffness coefficient upper bound
xue@1: */
xue@1: TTempAtom::TTempAtom(int apind, double af, double ef, double aa, double as, double maxB)
xue@1: {
xue@1:   Prev=NULL; pind=apind; f=af; a=aa; s=as; acce=aa*aa;
xue@1:   R=new TPolygon(4);
xue@1:   InitializeR(R, apind, af, ef, maxB);
xue@1: }//TTempAtom
xue@1: 
xue@1: /*
xue@1:   method TTempAtom::TTempAtom: creates a duplicate atom. R can be duplicated or snatched according to
xue@1:   DupR.
xue@1: 
xue@1:   In: APrev: the atom to duplicate
xue@1:       DupR: specifies if the newly created atom is to have its F-G polygon duplicated from that of APrev
xue@1:         or to snatch the F-G polygon from APrev (in the latter case APrev losts its polygon).
xue@1: */
xue@1: TTempAtom::TTempAtom(TTempAtom* APrev, bool DupR)
xue@1: {
xue@1:   Prev=APrev; pind=APrev->pind; f=APrev->f; a=APrev->a; s=APrev->s; acce=APrev->acce;
xue@1:   TPolygon* PrevR=APrev->R;
xue@1:   if (DupR)
xue@1:   {//duplicate R
xue@1:     R=new TPolygon(PrevR->N);
xue@1:     R->N=PrevR->N; memcpy(R->X, PrevR->X, sizeof(double)*PrevR->N); memcpy(R->Y, PrevR->Y, sizeof(double)*PrevR->N);
xue@1:   }
xue@1:   else
xue@1:   {//snatch R from APrev
xue@1:     R=APrev->R;
xue@1:     APrev->R=0;
xue@1:   }
xue@1: }//TTempAtom
xue@1: 
xue@1: /*
xue@1:   method TTempAtom::TTempAtom:: initializes an atom by adding a new partial to an existing group.
xue@1: 
xue@1:   In: APrev: the existing atom.
xue@1:       apind: partial index
xue@1:       af, ef: partial frequency and its error range
xue@1:       aa, as: partial amplitude and measurement scale
xue@1:       updateR: specifies if the F-G polygon is updated using (af, ef).
xue@1: */
xue@1: TTempAtom::TTempAtom(TTempAtom* APrev, int apind, double af, double ef, double aa, double as, bool updateR)
xue@1: {
xue@1:   Prev=APrev; pind=apind; f=af; a=aa;
xue@1:   s=as; acce=APrev->acce+aa*aa;
xue@1:   TPolygon* PrevR=APrev->R;
xue@1:   R=new TPolygon(PrevR->N+2);                         //  create R
xue@1:   R->N=PrevR->N;                                      //
xue@1:   memcpy(R->X, PrevR->X, sizeof(double)*PrevR->N);    // copy R
xue@1:   memcpy(R->Y, PrevR->Y, sizeof(double)*PrevR->N);    //
xue@1:   if (updateR) CutR(R, apind, af, ef);
xue@1: }//TTempAtom
xue@1: 
xue@1: //method TTempAtom::~TTempAtom: default destructor
xue@1: TTempAtom::~TTempAtom()
xue@1: {
xue@1:   delete R;
xue@1: }//~TTempAtom
xue@1: 
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: //functions
xue@1: 
Chris@5: /**
xue@1:   function areaandcentroid: calculates the area and centroid of a convex polygon.
xue@1: 
xue@1:   In: x[N], y[N]: x- and y-coordinates of vertices of a polygon
xue@1:   Out: A: area
xue@1:        (cx, cy): coordinate of the centroid
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void areaandcentroid(double& A, double& cx, double& cy, int N, double* x, double* y)
xue@1: {
xue@1:   if (N==0) {A=0;}
xue@1:   else if (N==1) {A=0; cx=x[0], cy=y[0];}
xue@1:   else if (N==2) {A=0; cx=(x[0]+x[1])/2, cy=(y[0]+y[1])/2;}
xue@1:   A=cx=cy=0;
xue@1:   for (int i=0; i<N; i++)
xue@1:   {
xue@1:     double xi1, yi1; //x[i+1], y[i+1], index modular N
xue@1:     if (i+1==N) xi1=x[0], yi1=y[0];
xue@1:     else xi1=x[i+1], yi1=y[i+1];
xue@1:     double tmp=x[i]*yi1-xi1*y[i];
xue@1:     A+=tmp;
xue@1:     cx+=(x[i]+xi1)*tmp;
xue@1:     cy+=(y[i]+yi1)*tmp;
xue@1:   }
xue@1:   A/=2;
xue@1:   cx=cx/6/A;
xue@1:   cy=cy/6/A;
xue@1: }//areaandcentroid
xue@1: 
Chris@5: /**
xue@1:   function AtomsToPartials: sort a list of atoms as a number of partials. It is assumed that the atoms
xue@1:   are simply those from a harmonic sinusoid in arbitrary order with uniform timing and partial index
xue@1:   clearly marked, so that it is easy to sort them into a list of partials. However, the sorting process
xue@1:   does not check for harmonicity, missing or duplicate atoms, uniformity of timing, etc.
xue@1: 
xue@1:   In: part[k]: a list of atoms
xue@1:       offst: interval between adjacent measurement points (hop size)
xue@1:   Out: M, Fr, partials[M][Fr]: a list of partials containing the atoms.
xue@1: 
xue@1:   No return value. partials[][] is allocated anew and must be freed by caller.
xue@1: */
xue@1: void AtomsToPartials(int k, atom* part, int& M, int& Fr, atom**& partials, int offst)
xue@1: {
xue@1:   if (k>0)
xue@1:   {
xue@1:     int t1, t2, i=0;
xue@1:     while (i<k && part[i].f<=0) i++;
xue@1:     if (i<k)
xue@1:     {
xue@1:       t1=part[i].t, t2=part[i].t, M=part[i].pin; i++;
xue@1:       while (i<k)
xue@1:       {
xue@1:         if (part[i].f>0)
xue@1:         {
xue@1:           if (M<part[i].pin) M=part[i].pin;
xue@1:           if (t1>part[i].t) t1=part[i].t;
xue@1:           if (t2<part[i].t) t2=part[i].t;
xue@1:         }
xue@1:         i++;
xue@1:       }
xue@1:       Fr=(t2-t1)/offst+1;
xue@1:       Allocate2(atom, M, Fr, partials); memset(partials[0], 0, sizeof(atom)*M*Fr);
xue@1:       for (int i=0; i<k; i++)
xue@1:         if (part[i].f>0)
xue@1:         {
xue@1:           int fr=(part[i].t-t1)/offst;
xue@1:           int pp=part[i].pin;
xue@1:           partials[pp-1][fr]=part[i];
xue@1:         }
xue@1:     }
xue@1:     else M=0, Fr=0;
xue@1:   }
xue@1:   else M=0, Fr=0;
xue@1: }//AtomsToPartials
xue@1: 
Chris@5: /**
xue@1:   function conta: a continuity measure between two set of amplitudes
xue@1: 
xue@1:   In: a1[N], a2[N]: the amplitudes
xue@1: 
xue@1:   Return the continuity measure, between 0 and 1
xue@1: */
xue@1: double conta(int N, double* a1, double* a2)
xue@1: {
xue@1:   double e11=0, e22=0, e12=0;
xue@1:   for (int n=0; n<N; n++)
xue@1:   {
xue@1:     double la1=(a1[n]>0)?a1[n]:0, la2=(a2[n]>0)?a2[n]:0;
xue@1:     if (la1>1e10) la1=la2;
xue@1:     e11+=la1*la1;
xue@1:     e12+=la1*la2;
xue@1:     e22+=la2*la2;
xue@1:   }
xue@1:   return 2*e12/(e11+e22);
xue@1: }//conta
xue@1: 
Chris@5: /**
xue@1:   function cutcvpoly: severs a polygon by a straight line
xue@1: 
xue@1:   In: x[N], y[N]: x- and y-coordinates of vertices of a polygon, starting from the leftmost (x[0]=
xue@1:         min x[n]) point clockwise; in case of two leftmost points, x[0] is the upper one and x[N-1] is
xue@1:         the lower one.
xue@1:       A, B, C: coefficients of a straight line Ax+By+C=0.
xue@1:       protect: specifies what to do if the whole polygon satisfy Ax+By+C>0, true=do noting,
xue@1:         false=eliminate.
xue@1:   Out: N, x[N], y[N]: the polygon severed by the line, retaining that half for which Ax+By+C<=0.
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void cutcvpoly(int& N, double* x, double* y, double A, double B, double C, bool protect)
xue@1: {
xue@1:   int n=0, ns;
xue@1:   while (n<N && A*x[n]+B*y[n]+C<=0) n++;
xue@1:   if (n==N) //all points satisfies the constraint, do nothing
xue@1:   {}
xue@1:   else if (n>0) //points 0, 1, ..., n-1 satisfies the constraint, point n does not
xue@1:   {
xue@1:     ns=n;
xue@1:     n++;
xue@1:     while (n<N && A*x[n]+B*y[n]+C>0) n++;
xue@1:     //points 0, ..., ns-1 and n, ..., N-1 satisfy the constraint,
xue@1:     //points ns, ..., n-1 do not satisfy the constraint,
xue@1:     //  ns>0, n>ns, however, it's possible that n-1=ns
xue@1:     int pts, pte; //indicates (by 0/1) whether or now a new point is to be added for xs/xe
xue@1:     double xs, ys, xe, ye;
xue@1: 
xue@1:     //find intersection of x:y[ns-1:ns] and A:B:C
xue@1:     double x1=x[ns-1], x2=x[ns], y1=y[ns-1], y2=y[ns];
xue@1:     double z1=A*x1+B*y1+C, z2=A*x2+B*y2+C; //z1<=0, z2>0
xue@1:     if (z1==0) pts=0; //as point ns-1 IS point s
xue@1:     else
xue@1:     {
xue@1:       pts=1, xs=(x1*z2-x2*z1)/(z2-z1), ys=(y1*z2-y2*z1)/(z2-z1);
xue@1:     }
xue@1: 
xue@1:     //find intersection of x:y[n-1:n] and A:B:C
xue@1:     x1=x[n-1], y1=y[n-1];
xue@1:     if (n==N) x2=x[0], y2=y[0];
xue@1:     else x2=x[n], y2=y[n];
xue@1:     z1=A*x1+B*y1+C, z2=A*x2+B*y2+C; //z1>0, z2<=0
xue@1:     if (z2==0) pte=0; //as point n is point e
xue@1:     else
xue@1:     {
xue@1:       pte=1, xe=(x1*z2-x2*z1)/(z2-z1), ye=(y1*z2-y2*z1)/(z2-z1);
xue@1:     }
xue@1: 
xue@1:     //the new polygon is formed by points 0, 1, ..., ns-1, (s), (e), n, ..., N-1
xue@1:     memmove(&x[ns+pts+pte], &x[n], sizeof(double)*(N-n));
xue@1:     memmove(&y[ns+pts+pte], &y[n], sizeof(double)*(N-n));
xue@1:     if (pts) x[ns]=xs, y[ns]=ys;
xue@1:     if (pte) x[ns+pts]=xe, y[ns+pts]=ye;
xue@1:     N=ns+pts+pte+N-n;
xue@1:   }
xue@1:   else  //n=0, point 0 does not satisfy the constraint
xue@1:   {
xue@1:     n++;
xue@1:     while (n<N && A*x[n]+B*y[n]+C>0) n++;
xue@1:     if (n==N) //no point satisfies the constraint
xue@1:     {
xue@1:       if (!protect) N=0;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       //points 0, 1, ..., n-1 do not satisfy the constraint, point n does
xue@1:       ns=n;
xue@1:       n++;
xue@1:       while (n<N && A*x[n]+B*y[n]+C<=0) n++;
xue@1:       //points 0, ..., ns-1 and n, ..., N-1 do not satisfy constraint,
xue@1:       //points ns, ..., n-1 satisfy the constraint
xue@1:       //  ns>0, n>ns, however ns can be equal to n-1
xue@1:       int pts, pte; //indicates (by 0/1) whether or now a new point is to be added for xs/xe
xue@1:       double xs, ys, xe, ye;
xue@1: 
xue@1:       //find intersection of x:y[ns-1:ns] and A:B:C
xue@1:       double x1=x[ns-1], x2=x[ns], y1=y[ns-1], y2=y[ns];
xue@1:       double z1=A*x1+B*y1+C, z2=A*x2+B*y2+C; //z1>0, z2<=0
xue@1:       if (z2==0) pts=0; //as point ns IS point s
xue@1:       else pts=1;
xue@1:       xs=(x1*z2-x2*z1)/(z2-z1), ys=(y1*z2-y2*z1)/(z2-z1);
xue@1: 
xue@1:       //find intersection of x:y[n-1:n] and A:B:C
xue@1:       x1=x[n-1], y1=y[n-1];
xue@1:       if (n==N) x2=x[0], y2=y[0];
xue@1:       else x2=x[n], y2=y[n];
xue@1:       z1=A*x1+B*y1+C, z2=A*x2+B*y2+C; //z1<=0, z2>0
xue@1:       if (z1==0) pte=0; //as point n-1 is point e
xue@1:       else pte=1;
xue@1:       xe=(x1*z2-x2*z1)/(z2-z1), ye=(y1*z2-y2*z1)/(z2-z1);
xue@1: 
xue@1:       //the new polygon is formed of points (s), ns, ..., n-1, (e)
xue@1:       if (xs<=xe)
xue@1:       {
xue@1:         //the point sequence comes as (s), ns, ..., n-1, (e)
xue@1:         memmove(&x[pts], &x[ns], sizeof(double)*(n-ns));
xue@1:         memmove(&y[pts], &y[ns], sizeof(double)*(n-ns));
xue@1:         if (pts) x[0]=xs, y[0]=ys;
xue@1:         N=n-ns+pts+pte;
xue@1:         if (pte) x[N-1]=xe, y[N-1]=ye;
xue@1:       }
xue@1:       else //xe<xs
xue@1:       {
xue@1:         //the sequence comes as e, (s), ns, ..., n-2 if pte=0 (notice point e<->n-1)
xue@1:         //or e, (s), ns, ..., n-1 if pte=1
xue@1:         if (pte==0)
xue@1:         {
xue@1:           memmove(&x[pts+1], &x[ns], sizeof(double)*(n-ns-1));
xue@1:           memmove(&y[pts+1], &y[ns], sizeof(double)*(n-ns-1));
xue@1:           if (pts) x[1]=xs, y[1]=ys;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           memmove(&x[pts+1], &x[ns], sizeof(double)*(n-ns));
xue@1:           memmove(&y[pts+1], &y[ns], sizeof(double)*(n-ns));
xue@1:           if (pts) x[1]=xs, y[1]=ys;
xue@1:         }
xue@1:         x[0]=xe, y[0]=ye;
xue@1:         N=n-ns+pts+pte;
xue@1:       }
xue@1:     }
xue@1:   }
xue@1: }//cutcvpoly
xue@1: 
xue@1: /*
xue@1:   funciton CutR: update an F-G polygon with an new partial
xue@1: 
xue@1:   In: R: F-G polygon
xue@1:       apind: partial index
xue@1:       af, ef: partial frequency and error bound
xue@1:       protect: specifies what to do if the new partial has no intersection with R, true=do noting,
xue@1:         false=eliminate R
xue@1:   Out: R: the updated F-G polygon
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void CutR(TPolygon* R, int apind, double af, double ef, bool protect)
xue@1: {
xue@1:   double k=apind*apind-1;
xue@1:   double g1=(af-ef)/apind, g2=(af+ef)/apind;
xue@1:   if (g1<0) g1=0;
xue@1:   g1=g1*g1, g2=g2*g2;
xue@1:   //apply constraints F+kG-g2<0 and -F-kG+g1<0
xue@1:   cutcvpoly(R->N, R->X, R->Y, 1, k, -g2, protect);          //  update R
xue@1:   cutcvpoly(R->N, R->X, R->Y, -1, -k, g1, protect);         //
xue@1: }//CutR
xue@1: 
Chris@5: /**
xue@1:   function DeleteByHalf: reduces a list of stiffcandid objects by half, retaining those with higher s
xue@1:   and delete the others, freeing their memory.
xue@1: 
xue@1:   In: cands[pcs]: the list of stiffcandid objects
xue@1:   Out: cands[return value]: the list of retained ones
xue@1: 
xue@1:   Returns the size of the new list.
xue@1: */
xue@1: int DeleteByHalf(stiffcandid** cands, int pcs)
xue@1: {
xue@1:   int newpcs=pcs/2;
xue@1:   double* tmp=new double[newpcs];
xue@1:   memset(tmp, 0, sizeof(double)*newpcs);
xue@1:   for (int j=0; j<pcs; j++) InsertDec(cands[j]->s, tmp, newpcs);
xue@1:   int k=0;
xue@1:   for (int j=0; j<pcs; j++)
xue@1:   {
xue@1:     if (cands[j]->s>=tmp[newpcs-1]) cands[k++]=cands[j];
xue@1:     else delete cands[j];
xue@1:   }
xue@1:   return k;
xue@1: }//DeleteByHalf
xue@1: 
Chris@5: /**
xue@1:   function DeleteByHalf: reduces a list of TTempAtom objects by half, retaining those with higher s and
xue@1:   delete the others, freeing their memory.
xue@1: 
xue@1:   In: cands[pcs]: the list of TTempAtom objects
xue@1:   Out: cands[return value]: the list of retained ones
xue@1: 
xue@1:   Returns the size of the new list.
xue@1: */
xue@1: int DeleteByHalf(TTempAtom** cands, int pcs)
xue@1: {
xue@1:   int newpcs=pcs/2;
xue@1:   double* tmp=new double[newpcs];
xue@1:   memset(tmp, 0, sizeof(double)*newpcs);
xue@1:   for (int j=0; j<pcs; j++) InsertDec(cands[j]->s, tmp, newpcs);
xue@1:   int k=0;
xue@1:   for (int j=0; j<pcs; j++)
xue@1:   {
xue@1:     if (cands[j]->s>=tmp[newpcs-1]) cands[k++]=cands[j];
xue@1:     else delete cands[j];
xue@1:   }
xue@1:   delete[] tmp;
xue@1:   return k;
xue@1: }//DeleteByHalf
xue@1: 
Chris@5: /**
xue@1:   function ds0: atom score 0 - energy
xue@1: 
xue@1:   In: a: atom amplitude
xue@1: 
xue@1:   Returns a^2, or s[0]+a^2 is s is not NULL, as atom score.
xue@1: */
xue@1: double ds0(double a, void* s)
xue@1: {
xue@1:   if (s) return	*(double*)s+a*a;
xue@1:   else return a*a;
xue@1: }//ds0
xue@1: 
Chris@5: /**
xue@1:   function ds2: atom score 1 - cross-correlation coefficient with another HA, e.g. from previous frame
xue@1: 
xue@1:   In: a: atom amplitude
xue@1:       params: pointer to dsprams1 structure supplying other inputs.
xue@1:   Out: cross-correlation coefficient as atom score.
xue@1: 
xue@1:   Returns the cross-correlation coefficient.
xue@1: */
xue@1: double ds1(double a, void* params)
xue@1: {
xue@1:   dsparams1* lparams=(dsparams1*)params;
xue@1:   double hs=lparams->lastene+lparams->currentacce, hsaa=hs+a*a;
xue@1:   if (lparams->p<=lparams->lastP) return (lparams->s*hs+a*lparams->lastvfp[lparams->p-1])/hsaa;
xue@1:   else return (lparams->s*hs+a*a)/hsaa;
xue@1: }//ds1
xue@1: 
Chris@5: /**
xue@1:   function ExBStiff: finds the minimal and maximal values of stiffness coefficient B given a F-G polygon
xue@1: 
xue@1:   In: F[N], G[N]: vertices of a F-G polygon
xue@1:   Out: Bmin, Bmax: minimal and maximal values of the stiffness coefficient
xue@1: 
xue@1:   No reutrn value.
xue@1: */
xue@1: void ExBStiff(double& Bmin, double& Bmax, int N, double* F, double* G)
xue@1: {
xue@1:   Bmax=G[0]/F[0];
xue@1:   Bmin=Bmax;
xue@1:   for (int i=1; i<N; i++)
xue@1:   {
xue@1:     double vi=G[i]/F[i];
xue@1:     if (Bmin>vi) Bmin=vi;
xue@1:     else if (Bmax<vi) Bmax=vi;
xue@1:   }
xue@1: }//ExBStiff
xue@1: 
Chris@5: /**
xue@1:   function ExFmStiff: finds the minimal and maximal frequecies of partial m given a F-G polygon
xue@1: 
xue@1:   In: F[N], G[N]: vertices of a F-G polygon
xue@1:       m: partial index, fundamental=1
xue@1:   Out: Fmin, Fmax: minimal and maximal frequencies of partial m
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void ExFmStiff(double& Fmin, double& Fmax, int m, int N, double* F, double* G)
xue@1: {
xue@1:   int k=m*m-1;
xue@1:   double vmax=F[0]+k*G[0];
xue@1:   double vmin=vmax;
xue@1:   for (int i=1; i<N; i++)
xue@1:   {
xue@1:     double vi=F[i]+k*G[i];
xue@1:     if (vmin>vi) vmin=vi;
xue@1:     else if (vmax<vi) vmax=vi;
xue@1:   }
xue@1:   testnn(vmin); Fmin=m*sqrt(vmin);
xue@1:   testnn(vmax); Fmax=m*sqrt(vmax);
xue@1: }//ExFmStiff
xue@1: 
Chris@5: /**
xue@1:   function ExtendR: extends a F-G polygon on the f axis (to allow a larger pitch range)
xue@1: 
xue@1:   In: R: the F-G polygon
xue@1:       delp1: amount of extension to the low end, in semitones
xue@1:       delpe: amount of extension to the high end, in semitones
xue@1:       minf: minimal fundamental frequency
xue@1:   Out: R: the extended F-G polygon
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void ExtendR(TPolygon* R, double delp1, double delp2, double minf)
xue@1: {
xue@1:   double mdelf1=pow(2, -delp1/12), mdelf2=pow(2, delp2/12);
xue@1:   int N=R->N;
xue@1:   double *G=R->Y, *F=R->X, slope, prevslope, f, ftmp;
xue@1:   if (N==0) {}
xue@1:   else if (N==1)
xue@1:   {
xue@1:     R->N=2;
xue@1:     slope=G[0]/F[0];
xue@1:     testnn(F[0]); f=sqrt(F[0]);
xue@1:     ftmp=f*mdelf2;
xue@1:     F[1]=ftmp*ftmp;
xue@1:     ftmp=f*mdelf1;
xue@1:     if (ftmp<minf) ftmp=minf;
xue@1:     F[0]=ftmp*ftmp;
xue@1:     G[0]=F[0]*slope;
xue@1:     G[1]=F[1]*slope;
xue@1:   }
xue@1:   else if (N==2)
xue@1:   {
xue@1:     prevslope=G[0]/F[0];
xue@1:     slope=G[1]/F[1];
xue@1:     if (fabs((slope-prevslope)/prevslope)<1e-10)
xue@1:     {
xue@1:       testnn(F[0]); ftmp=sqrt(F[0])*mdelf1; if (ftmp<0) ftmp=0; F[0]=ftmp*ftmp; G[0]=F[0]*prevslope;
xue@1:       testnn(F[1]); ftmp=sqrt(F[1])*mdelf2; F[1]=ftmp*ftmp; G[1]=F[1]*slope;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       if (prevslope>slope)
xue@1:       {
xue@1:         R->N=4;
xue@1:         testnn(F[1]); f=sqrt(F[1]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[3]=ftmp*ftmp; G[3]=F[3]*slope;
xue@1:         ftmp=f*mdelf2; F[2]=ftmp*ftmp; G[2]=F[2]*slope;
xue@1:         testnn(F[0]); f=sqrt(F[0]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[0]=ftmp*ftmp; G[0]=F[0]*prevslope;
xue@1:         ftmp=f*mdelf2; F[1]=ftmp*ftmp; G[1]=F[1]*prevslope;
xue@1:         if (F[0]==0 && F[3]==0) R->N=3;
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         R->N=4;
xue@1:         testnn(F[1]); f=sqrt(F[1]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[1]=ftmp*ftmp; G[1]=F[1]*slope;
xue@1:         ftmp=f*mdelf2; F[2]=ftmp*ftmp; G[2]=F[2]*slope;
xue@1:         testnn(F[0]); f=sqrt(F[0]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[0]=ftmp*ftmp; G[0]=F[0]*prevslope;
xue@1:         ftmp=f*mdelf2; F[4]=ftmp*ftmp; G[4]=F[4]*prevslope;
xue@1:         if (F[0]==0 && F[1]==0) {R->N=3; F[1]=F[2]; F[2]=F[3]; G[1]=G[2]; G[3]=G[3];}
xue@1:       }
xue@1:     }
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     //find the minimal and maximal angles of R
xue@1:     //maximal slope is taken at Ms if Mt=1, or Ms-1 and Ms if Mt=0
xue@1:     //minimal slope is taken at ms if mt=1, or ms-1 and ms if mt=0
xue@1:     int Ms, ms, Mt=-1, mt=-1;
xue@1:     double prevslope=G[0]/F[0], dslope;
xue@1:     for (int n=1; n<N; n++)
xue@1:     {
xue@1:       double slope=G[n]/F[n];
xue@1:       dslope=atan(slope)-atan(prevslope);
xue@1:       if (Mt==-1)
xue@1:       {
xue@1:         if (dslope<-1e-10) Ms=n-1, Mt=1;
xue@1:         else if (dslope<1e-10) Ms=n, Mt=0;
xue@1:       }
xue@1:       else if (mt==-1)
xue@1:       {
xue@1:         if (dslope>1e-10) ms=n-1, mt=1;
xue@1:         else if (dslope>-1e-10) ms=n, mt=0;
xue@1:       }
xue@1:       prevslope=slope;
xue@1:     }
xue@1:     if (mt==-1)
xue@1:     {
xue@1:       slope=G[0]/F[0];
xue@1:       dslope=atan(slope)-atan(prevslope);
xue@1:       if (dslope>1e-10) ms=N-1, mt=1;
xue@1:       else if (dslope>-1e-10) ms=0, mt=0;
xue@1:     }
xue@1:     R->N=N+mt+Mt;
xue@1:     int newn=R->N-1;
xue@1:     if (ms==0 && mt==1)
xue@1:     {
xue@1:       slope=G[0]/F[0];
xue@1:       testnn(F[0]); ftmp=sqrt(F[0])*mdelf2; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--;
xue@1:       mt=-1;
xue@1:     }
xue@1:     else if (ms==0)
xue@1:       mt=-1;
xue@1:     for (int n=N-1; n>=0; n--)
xue@1:     {
xue@1:       if (mt!=-1)
xue@1:       {
xue@1:         slope=G[n]/F[n];
xue@1:         testnn(F[n]); f=sqrt(F[n]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--;
xue@1:         if (ms==n && mt==1)
xue@1:         {
xue@1:           ftmp=f*mdelf2; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--;
xue@1:           mt=-1;
xue@1:         }
xue@1:         else if (ms==n) //mt=0
xue@1:           mt=-1;
xue@1:       }
xue@1:       else if (Mt!=-1)
xue@1:       {
xue@1:         slope=G[n]/F[n];
xue@1:         testnn(F[n]); f=sqrt(F[n]); ftmp=f*mdelf2; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--;
xue@1:         if (Ms==n && Mt==1)
xue@1:         {
xue@1:           ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--;
xue@1:           Mt=-1;
xue@1:         }
xue@1:         else if (Ms==n)
xue@1:           Mt=-1;
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         slope=G[n]/F[n];
xue@1:         testnn(F[n]); f=sqrt(F[n]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--;
xue@1:       }
xue@1:     }
xue@1:     //merge multiple vertices at the origin
xue@1:     N=R->N;
xue@1:     while (N>0 && F[N-1]==0) N--;
xue@1:     R->N=N;
xue@1:     int n=1;
xue@1:     while (n<N && F[n]==0) n++;
xue@1:     if (n!=1)
xue@1:     {
xue@1:       R->N=N-(n-1);
xue@1:       memcpy(&F[1], &F[n], sizeof(double)*(N-n));
xue@1:       memcpy(&G[1], &G[n], sizeof(double)*(N-n));
xue@1:     }
xue@1:   }
xue@1: }//ExtendR
xue@1: 
Chris@5: /**
xue@1:   function FGFrom2: solves the following equation system given m[2], f[2], norm[2]. This is interpreted
xue@1:   as searching from (F0, G0) down direction (dF, dG) until the first equation is satisfied, where
xue@1:   k[*]=m[*]^2-1.
xue@1: 
xue@1:     m[0](F+k[0]G)^0.5-f[0]     m[1](F+k[1]G)^0.5-f[1]
xue@1:    ------------------------ = ------------------------ >0
xue@1:             norm[0]                    norm[1]
xue@1: 
xue@1:    F=F0+lmd*dF
xue@1:    G=G0+lmd*dG
xue@1: 
xue@1:   In: (F0, G0): search origin
xue@1:       (dF, dG): search direction
xue@1:       m[2], f[2], norm[2]: coefficients in the first equation
xue@1:   Out: F[return value], G[return value]: solutions.
xue@1: 
xue@1:   Returns the number of solutions for (F, G).
xue@1: */
xue@1: int FGFrom2(double* F, double* G, double F0, double dF, double G0, double dG, int* m, double* f, double* norm)
xue@1: {
xue@1:   double m1=m[0]/norm[0], m2=m[1]/norm[1],
xue@1:          f1=f[0]/norm[0], f2=f[1]/norm[1],
xue@1:          k1=m[0]*m[0]-1, k2=m[1]*m[1]-1;
xue@1:   double A=m1*m1-m2*m2*(dF+k2*dG)/(dF+k1*dG),
xue@1:          B=2*m1*(f2-f1),
xue@1:          C=(f2-f1)*(f2-f1)-(k1-k2)*(F0*dG-G0*dF)/(dF+k1*dG)*m2*m2;
xue@1:   if (A==0)
xue@1:   {
xue@1:     double x=-C/B;
xue@1:     if (x<0) return 0;
xue@1:     else if (m1*x-f1<0) return 0;
xue@1:     else
xue@1:     {
xue@1:       double lmd=(x*x-(F0+k1*G0))/(dF+k1*dG);
xue@1:       F[0]=F0+lmd*dF;
xue@1:       G[0]=G0+lmd*dG;
xue@1:       return 1;
xue@1:     }
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     double delta=B*B-4*A*C;
xue@1:     if (delta<0) return 0;
xue@1:     else if (delta==0)
xue@1:     {
xue@1:       double x=-B/2/A;
xue@1:       if (x<0) return 0;
xue@1:       else if (m1*x-f1<0) return 0;
xue@1:       else
xue@1:       {
xue@1:         double lmd=(x*x-(F0+k1*G0))/(dF+k1*dG);
xue@1:         F[0]=F0+lmd*dF;
xue@1:         G[0]=G0+lmd*dG;
xue@1:         return 1;
xue@1:       }
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       int roots=0;
xue@1:       double x=(-B+sqrt(delta))/2/A;
xue@1:       if (x<0) {}
xue@1:       else if (m1*x-f1<0) {}
xue@1:       else
xue@1:       {
xue@1:         double lmd=(x*x-(F0+k1*G0))/(dF+k1*dG);
xue@1:         F[0]=F0+lmd*dF;
xue@1:         G[0]=G0+lmd*dG;
xue@1:         roots++;
xue@1:       }
xue@1:       x=(-B-sqrt(delta))/2/A;
xue@1:       if (x<0) {}
xue@1:       else if (m1*x-f1<0) {}
xue@1:       else
xue@1:       {
xue@1:         double lmd=(x*x-(F0+k1*G0))/(dF+k1*dG);
xue@1:         F[roots]=F0+lmd*dF;
xue@1:         G[roots]=G0+lmd*dG;
xue@1:         roots++;
xue@1:       }
xue@1:       return roots;
xue@1:     }
xue@1:   }
xue@1: }//FGFrom2
xue@1: 
Chris@5: /**
xue@1:   function FGFrom2: solves the following equation system given m[2], f[2], k[2]. This is interpreted as
xue@1:   searching from (F0, G0) down direction (dF, dG) until the first equation is satisfied. Functionally
xue@1:   this is the same as the version using norm[2], with m[] anf f[] normalized by norm[] beforehand so
xue@1:   that norm[] is no longer needed. However, k[2] cannot be computed from normalized m[2] so that it must
xue@1:   be specified explicitly.
xue@1: 
xue@1:    m[0](F+k[0]G)^0.5-f[0] = m[1](F+k[1]G)^0.5-f[1] > 0
xue@1: 
xue@1:    F=F0+lmd*dF
xue@1:    G=G0+lmd*dG
xue@1: 
xue@1:   In: (F0, G0): search origin
xue@1:       (dF, dG): search direction
xue@1:       m[2], f[2], k[2]: coefficients in the first equation
xue@1:   Out: F[return value], G[return value]: solutions.
xue@1: 
xue@1:   Returns the number of solutions for (F, G).
xue@1: */
xue@1: int FGFrom2(double* F, double* G, double* lmd, double F0, double dF, double G0, double dG, double* m, double* f, int* k)
xue@1: {
xue@1:   double m1=m[0], m2=m[1],
xue@1:          f1=f[0], f2=f[1],
xue@1:          k1=k[0], k2=k[1];
xue@1:   double A=m1*m1-m2*m2*(dF+k2*dG)/(dF+k1*dG),
xue@1:          B=2*m1*(f2-f1),
xue@1:          C=(f2-f1)*(f2-f1)-(k1-k2)*(F0*dG-G0*dF)/(dF+k1*dG)*m2*m2;
xue@1:   //x is obtained by solving Axx+Bx+C=0
xue@1:   if (fabs(A)<1e-6) //A==0
xue@1:   {
xue@1:     double x=-C/B;
xue@1:     if (x<0) return 0;
xue@1:     else if (m1*x-f1<0) return 0;
xue@1:     else
xue@1:     {
xue@1:       lmd[0]=(x*x-(F0+k1*G0))/(dF+k1*dG);
xue@1:       F[0]=F0+lmd[0]*dF;
xue@1:       G[0]=G0+lmd[0]*dG;
xue@1:       return 1;
xue@1:     }
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     int roots=0;
xue@1:     double delta=B*B-4*A*C;
xue@1:     if (delta<0) return 0;
xue@1:     else if (delta==0)
xue@1:     {
xue@1:       double x=-B/2/A;
xue@1:       if (x<0) return 0;
xue@1:       else if (m1*x-f1<0) return 0;
xue@1:       else if ((m1*x-f1+f2)*m2<0) {}
xue@1:       else
xue@1:       {
xue@1:         lmd[0]=(x*x-(F0+k1*G0))/(dF+k1*dG);
xue@1:         F[0]=F0+lmd[0]*dF;
xue@1:         G[0]=G0+lmd[0]*dG;
xue@1:         roots++;
xue@1:       }
xue@1:       return roots;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       int roots=0;
xue@1:       double x=(-B+sqrt(delta))/2/A;
xue@1:       if (x<0) {}
xue@1:       else if (m1*x-f1<0) {}
xue@1:       else if ((m1*x-f1+f2)*m2<0) {}
xue@1:       else
xue@1:       {
xue@1:         lmd[0]=(x*x-(F0+k1*G0))/(dF+k1*dG);
xue@1:         F[0]=F0+lmd[0]*dF;
xue@1:         G[0]=G0+lmd[0]*dG;
xue@1:         roots++;
xue@1:       }
xue@1:       x=(-B-sqrt(delta))/2/A;
xue@1:       if (x<0) {}
xue@1:       else if (m1*x-f1<0) {}
xue@1:       else if ((m1*x-f1+f2)*m2<0) {}
xue@1:       else
xue@1:       {
xue@1:         lmd[roots]=(x*x-(F0+k1*G0))/(dF+k1*dG);
xue@1:         F[roots]=F0+lmd[roots]*dF;
xue@1:         G[roots]=G0+lmd[roots]*dG;
xue@1:         roots++;
xue@1:       }
xue@1:       return roots;
xue@1:     }
xue@1:   }
xue@1: }//FGFrom2
xue@1: 
Chris@5: /**
xue@1:   function FGFrom3: solves the following equation system given m[3], f[3], norm[3].
xue@1: 
xue@1:     m[0](F+k[0]G)^0.5-f[0]       m[1](F+k[1]G)^0.5-f[1]       m[2](F+k[2]G)^0.5-f[2]
xue@1:    ------------------------  =  ------------------------  =  ------------------------ > 0
xue@1:            norm[0]                      norm[1]                      norm[2]
xue@1: 
xue@1:   In: m[3], f[3], norm[3]: coefficients in the above
xue@1:   Out: F[return value], G[return value]: solutions.
xue@1: 
xue@1:   Returns the number of solutions for (F, G).
xue@1: */
xue@1: int FGFrom3(double* F, double* G, int* m, double* f, double* norm)
xue@1: {
xue@1:   double m1=m[0]/norm[0], m2=m[1]/norm[1], m3=m[2]/norm[2],
xue@1:          f1=f[0]/norm[0], f2=f[1]/norm[1], f3=f[2]/norm[2],
xue@1:          k1=m[0]*m[0]-1, k2=m[1]*m[1]-1, k3=m[2]*m[2]-1;
xue@1:   double h21=k2-k1, h31=k3-k1;             //h21 and h31
xue@1:   double h12=m1*m1-m2*m2, h13=m1*m1-m3*m3; //h12' and h13'
xue@1:   double a=m2*m2*h13*h21-m3*m3*h12*h31;
xue@1:   if (a==0)
xue@1:   {
xue@1:     double x=h12*(f3-f1)*(f3-f1)-h13*(f2-f1)*(f2-f1),
xue@1:            b=h13*(f2-f1)-h12*(f3-f1);
xue@1:     x=x/(2*m1*b);
xue@1:     if (x<0) return 0; //x must the square root of something
xue@1:     else if (m1*x-f1<0) return 0; //discarded because we are solving e1=e2=e3>0
xue@1:     else
xue@1:     {
xue@1:       if (h21!=0)
xue@1:       {
xue@1:         G[0]=(h12*x*x+2*m1*(f2-f1)*x+(f2-f1)*(f2-f1))/(m2*m2*h21);
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         G[0]=(h13*x*x+2*m1*(f3-f1)*x+(f3-f1)*(f3-f1))/(m3*m3*h31);
xue@1:       }
xue@1:       F[0]=x*x-k1*G[0];
xue@1:       return 1;
xue@1:     }
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     double b=(h13*(f2-f1)*(f2-f1)-h12*(f3-f1)*(f3-f1))/a;
xue@1:     a=2*m1*(h13*(f2-f1)-h12*(f3-f1))/a;
xue@1:     double A=h12,
xue@1:            B=2*m1*(f2-f1)-m2*m2*h21*a,
xue@1:            C=(f2-f1)*(f2-f1)-m2*m2*h21*b;
xue@1:     double delta=B*B-4*A*C;
xue@1:     if (delta<0) return 0;
xue@1:     else if (delta==0)
xue@1:     {
xue@1:       double x=-B/2/A;
xue@1:       if (x<0) return 0; //x must the square root of something
xue@1:       else if (m1*x-f1<0) return 0; //discarded because we are solving e1=e2=e3>0
xue@1:       else
xue@1:       {
xue@1:         G[0]=a*x+b;
xue@1:         F[0]=x*x-k1*G[0];
xue@1:         return 1;
xue@1:       }
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       int roots=0;
xue@1:       double x=(-B+sqrt(delta))/2/A;
xue@1:       if (x<0) {}
xue@1:       else if (m1*x-f1<0) {}
xue@1:       else
xue@1:       {
xue@1:         G[0]=a*x+b;
xue@1:         F[0]=x*x-k1*G[0];
xue@1:         roots++;
xue@1:       }
xue@1:       x=(-B-sqrt(delta))/2/A;
xue@1:       if (x<0) {}
xue@1:       else if (m1*x-f1<0) {}
xue@1:       else
xue@1:       {
xue@1:         G[roots]=a*x+b;
xue@1:         F[roots]=x*x-k1*G[roots];
xue@1:         roots++;
xue@1:       }
xue@1:       return roots;
xue@1:     }
xue@1:   }
xue@1: }//FGFrom3
xue@1: 
Chris@5: /**
xue@1:   function FGFrom3: solves the following equation system given m[3], f[3], k[3]. Functionally this is
xue@1:   the same as the version using norm[3], with m[] anf f[] normalized by norm[] beforehand so that norm[]
xue@1:   is no longer needed. However, k[3] cannot be computed from normalized m[3] so that it must be
xue@1:   specified explicitly.
xue@1: 
xue@1:   m[0](F+k[0]G)^0.5-f[0] = m[1](F+k[1]G)^0.5-f[1] = m[2](F+k[2]G)^0.5-f[2] >0
xue@1: 
xue@1:   In: m[3], f[3], k[3]: coefficients in the above
xue@1:   Out: F[return value], G[return value]: solutions.
xue@1: 
xue@1:   Returns the number of solutions for (F, G).
xue@1: */
xue@1: int FGFrom3(double* F, double* G, double* e, double* m, double* f, int* k)
xue@1: {
xue@1:   double m1=m[0], m2=m[1], m3=m[2],
xue@1:          f1=f[0], f2=f[1], f3=f[2],
xue@1:          k1=k[0], k2=k[1], k3=k[2];
xue@1:   double h21=k2-k1, h31=k3-k1;             //h21 and h31
xue@1:   double h12=m1*m1-m2*m2, h13=m1*m1-m3*m3; //h12' and h13'
xue@1:   double a=m2*m2*h13*h21-m3*m3*h12*h31;
xue@1:   if (a==0)
xue@1:   {
xue@1:     //a==0 implies two the e's are conjugates
xue@1:     double x=h12*(f3-f1)*(f3-f1)-h13*(f2-f1)*(f2-f1),
xue@1:            b=h13*(f2-f1)-h12*(f3-f1);
xue@1:     x=x/(2*m1*b);
xue@1:     if (x<0) return 0; //x must the square root of something
xue@1:     else if (m1*x-f1<-1e-6) return 0; //discarded because we are solving e1=e2=e3>0
xue@1:     else if ((m1*x-f1+f2)*m2<0) return 0;
xue@1:     else if ((m1*x-f1+f3)*m3<0) return 0;
xue@1:     else
xue@1:     {
xue@1:       if (h21!=0)
xue@1:       {
xue@1:         G[0]=(h12*x*x+2*m1*(f2-f1)*x+(f2-f1)*(f2-f1))/(m2*m2*h21);
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         G[0]=(h13*x*x+2*m1*(f3-f1)*x+(f3-f1)*(f3-f1))/(m3*m3*h31);
xue@1:       }
xue@1:       F[0]=x*x-k1*G[0];
xue@1:       double tmp=F[0]+G[0]*k[0]; testnn(tmp);
xue@1:       e[0]=m1*sqrt(tmp)-f[0];
xue@1:       return 1;
xue@1:     }
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     double b=(h13*(f2-f1)*(f2-f1)-h12*(f3-f1)*(f3-f1))/a;
xue@1:     a=2*m1*(h13*(f2-f1)-h12*(f3-f1))/a;
xue@1:     double A=h12,
xue@1:            B=2*m1*(f2-f1)-m2*m2*h21*a,
xue@1:            C=(f2-f1)*(f2-f1)-m2*m2*h21*b;
xue@1:     double delta=B*B-4*A*C;
xue@1:     if (delta<0) return 0;
xue@1:     else if (delta==0)
xue@1:     {
xue@1:       int roots=0;
xue@1:       double x=-B/2/A;
xue@1:       if (x<0) return 0; //x must the square root of something
xue@1:       else if (m1*x-f1<0) return 0; //discarded because we are solving e1=e2=e3>0
xue@1:       else if ((m1*x-f1+f2)*m2<0) return 0;
xue@1:       else if ((m1*x-f1+f3)*m3<0) return 0;
xue@1:       else
xue@1:       {
xue@1:         G[0]=a*x+b;
xue@1:         F[0]=x*x-k1*G[0];
xue@1:         double tmp=F[0]+G[0]*k[0]; testnn(tmp);
xue@1:         e[0]=m1*sqrt(tmp)-f[0];
xue@1:         roots++;
xue@1:       }
xue@1:       return roots;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       int roots=0;
xue@1:       double x=(-B+sqrt(delta))/2/A;
xue@1:       if (x<0) {}
xue@1:       else if (m1*x-f1<0) {}
xue@1:       else if ((m1*x-f1+f2)*m2<0) {}
xue@1:       else if ((m1*x-f1+f3)*m3<0) {}
xue@1:       else
xue@1:       {
xue@1:         G[0]=a*x+b;
xue@1:         F[0]=x*x-k1*G[0];
xue@1:         double tmp=F[0]+G[0]*k1; testnn(tmp);
xue@1:         e[0]=m1*sqrt(tmp)-f1;
xue@1:         roots++;
xue@1:       }
xue@1:       x=(-B-sqrt(delta))/2/A;
xue@1:       if (x<0) {}
xue@1:       else if (m1*x-f1<0) {}
xue@1:       else if ((m1*x-f1+f2)*m2<0) {}
xue@1:       else if ((m1*x-f1+f3)*m3<0) {}
xue@1:       else
xue@1:       {
xue@1:         G[roots]=a*x+b;
xue@1:         F[roots]=x*x-k1*G[roots];
xue@1:         double tmp=F[roots]+G[roots]*k1; testnn(tmp);
xue@1:         e[roots]=m1*sqrt(tmp)-f1;
xue@1:         roots++;
xue@1:       }
xue@1:       return roots;
xue@1:     }
xue@1:   }
xue@1: }//FGFrom3
xue@1: 
Chris@5: /**
xue@1:   function FindNote: harmonic sinusoid tracking from a starting point in time-frequency plane forward
xue@1:   and backward.
xue@1: 
xue@1:   In: _t, _f: start time and frequency
xue@1:       frst, fren: tracking range, in frames
xue@1:       Spec: spectrogram
xue@1:       wid, offst: atom scale and hop size, must be consistent with Spec
xue@1:       settings: note match settings
xue@1:       brake: tracking termination threshold
xue@1:   Out: M, Fr, partials[M][Fr]: HS partials
xue@1: 
xue@1:   Returns 0 if tracking is done by FindNoteF(), 1 if tracking is done by FindNoteConst()
xue@1: */
xue@1: int FindNote(int _t, double _f, int& M, int& Fr, atom**& partials, int frst, int fren, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings)
xue@1: {
xue@1:   double brake=0.02;
xue@1:   if (settings.delp==0) return FindNoteConst(_t, _f, M, Fr, partials, frst, fren, wid, offst, Spec, settings, brake*5);
xue@1: 
xue@1:   atom* part=new atom[(fren-frst)*settings.maxp];
xue@1:   double fpp[1024]; double *vfpp=&fpp[256], *pfpp=&fpp[512]; atomtype *ptype=(atomtype*)&fpp[768]; NMResults results={fpp, vfpp, pfpp, ptype};
xue@1: 
xue@1:   int trst=floor((_t-wid/2)*1.0/offst+0.5);
xue@1:   if (trst<0) trst=0;
xue@1: 
xue@1:   TPolygon* R=new TPolygon(1024); R->N=0;
xue@1: 
xue@1:   cmplx<QSPEC_FORMAT>* spec=Spec->Spec(trst);
xue@1:   double f0=_f*wid;
xue@1:   cdouble *x=new cdouble[wid/2+1]; for (int i=0; i<=wid/2; i++) x[i]=spec[i];
xue@1: 
xue@1:   settings.forcepin0=true; settings.pcount=0; settings.pin0asanchor=true;
xue@1:   double B, starts=NoteMatchStiff3(R, f0, B, 1, &x, wid, 0, &settings, &results, 0, 0, ds0);
xue@1: 
xue@1:   int k=0;
xue@1:   if (starts>0)
xue@1:   {
xue@1:     int startp=NMResultToAtoms(settings.maxp, part, trst*offst+wid/2, wid, results);
xue@1:     settings.pin0asanchor=false;
xue@1:     double* startvfp=new double[startp]; memcpy(startvfp, vfpp, sizeof(double)*startp);
xue@1:     k=startp;
xue@1:     k+=FindNoteF(&part[k], starts, R, startp, startvfp, trst, fren, wid, offst, Spec, settings, brake);
xue@1:     k+=FindNoteF(&part[k], starts, R, startp, startvfp, trst, frst-1, wid, offst, Spec, settings, brake);
xue@1:     delete[] startvfp;
xue@1:   }
xue@1:   delete R; delete[] x;
xue@1: 
xue@1:   AtomsToPartials(k, part, M, Fr, partials, offst);
xue@1:   delete[] part;
xue@1: 
xue@1:   //	ReEst1(M, frst, fren, partials, WV->Data16[channel], wid, offst);
xue@1: 
xue@1:   return 0;
xue@1: }//Findnote*/
xue@1: 
Chris@5: /**
xue@1:   function FindNoteConst: constant-pitch harmonic sinusoid tracking
xue@1: 
xue@1:   In: _t, _f: start time and frequency
xue@1:       frst, fren: tracking range, in frames
xue@1:       Spec: spectrogram
xue@1:       wid, offst: atom scale and hop size, must be consistent with Spec
xue@1:       settings: note match settings
xue@1:       brake: tracking termination threshold
xue@1:   Out: M, Fr, partials[M][Fr]: HS partials
xue@1: 
xue@1:   Returns 1.
xue@1: */
xue@1: int FindNoteConst(int _t, double _f, int& M, int& Fr, atom**& partials, int frst, int fren, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings, double brake)
xue@1: {
xue@1:   atom* part=new atom[(fren-frst)*settings.maxp];
xue@1:   double fpp[1024]; double *vfpp=&fpp[256], *pfpp=&fpp[512]; atomtype *ptype=(atomtype*)&fpp[768]; NMResults results={fpp, vfpp, pfpp, ptype};
xue@1: 
xue@1:   //trst: the frame index to start tracking
xue@1:   int trst=floor((_t-wid/2)*1.0/offst+0.5), maxp=settings.maxp;
xue@1:   if (trst<0) trst=0;
xue@1: 
xue@1:   //find a note candidate for the starting frame at _t
xue@1:   TPolygon* R=new TPolygon(1024); R->N=0;
xue@1:   cmplx<QSPEC_FORMAT>* spec=Spec->Spec(trst);
xue@1:   double f0=_f*wid;
xue@1:   cdouble *x=new cdouble[wid/2+1]; for (int i=0; i<=wid/2; i++) x[i]=spec[i];
xue@1: 
xue@1:   settings.forcepin0=true; settings.pcount=0; settings.pin0asanchor=true;
xue@1:   double B, *starts=new double[maxp];
xue@1:   NoteMatchStiff3(R, f0, B, 1, &x, wid, 0, &settings, &results, 0, 0);
xue@1: 
xue@1:   //read the energy vector of this candidate HA to starts[]. starts[] will contain the highest single-frame
xue@1:   //energy of each partial during the tracking.
xue@1:   int P=0; while(P<maxp && f0*(P+1)*sqrt(1+B*((P+1)*(P+1)-1))<wid/2) P++;
xue@1:   for (int m=0; m<P; m++) starts[m]=results.vfp[m]*results.vfp[m];
xue@1:   int atomcount=0;
xue@1: 
xue@1:   cdouble* ipw=new cdouble[maxp];
xue@1: 
xue@1:   //find the ends of tracking by constant-pitch extension of the starting HA until
xue@1:   //at a frame at least half of the partials fall below starts[]*brake.
xue@1: 
xue@1:   //first find end of the note by forward extension from the frame at _t
xue@1:   int fr1=trst;
xue@1:   while (fr1<fren)
xue@1:   {
xue@1:     spec=Spec->Spec(fr1);
xue@1:     for (int i=0; i<=wid/2; i++) x[i]=spec[i];
xue@1:     double ls=0;
xue@1:     for (int m=0; m<P; m++)
xue@1:     {
xue@1:       double fm=f0*(m+1)*sqrt(1+B*((m+1)*(m+1)-1));
xue@1:       int K1=floor(fm-settings.hB), K2=ceil(fm+settings.hB);
xue@1:       if (K1<0) K1=0; if (K2>=wid/2) K2=wid/2-1;
xue@1:       ipw[m]=IPWindowC(fm, x, wid, settings.M, settings.c, settings.iH2, K1, K2);
xue@1:       ls+=~ipw[m];
xue@1:       if (starts[m]<~ipw[m]) starts[m]=~ipw[m];
xue@1:     }
xue@1:     double sumstart=0, sumstop=0;
xue@1:     for (int m=0; m<P; m++)
xue@1:     {
xue@1:       if (~ipw[m]<starts[m]*brake) sumstop+=1;// sqrt(starts[m]);
xue@1:       sumstart+=1; //sqrt(starts[m]);
xue@1:     }
xue@1:     double lstop=sumstop/sumstart;
xue@1:     if (lstop>0.5) break;
xue@1: 
xue@1:     fr1++;
xue@1:   }
xue@1:   //note find the start of note by backward extension from the frame at _t
xue@1:   int fr0=trst-1;
xue@1:   while(fr0>frst)
xue@1:   {
xue@1:     spec=Spec->Spec(fr0);
xue@1:     for (int i=0; i<=wid/2; i++) x[i]=spec[i];
xue@1:     double ls=0;
xue@1:     for (int m=0; m<P; m++)
xue@1:     {
xue@1:       double fm=f0*(m+1)*sqrt(1+B*((m+1)*(m+1)-1));
xue@1:       int K1=floor(fm-settings.hB), K2=ceil(fm+settings.hB);
xue@1:       if (K1<0) K1=0; if (K2>=wid/2) K2=wid/2-1;
xue@1:       ipw[m]=IPWindowC(fm, x, wid, settings.M, settings.c, settings.iH2, K1, K2);
xue@1:       ls+=~ipw[m];
xue@1:       if (starts[m]<~ipw[m]) starts[m]=~ipw[m];
xue@1:     }
xue@1: 
xue@1:     double sumstart=0, sumstop=0;
xue@1:     for (int m=0; m<P; m++)
xue@1:     {
xue@1:       if (~ipw[m]<starts[m]*brake) sumstop+=1; //sqrt(starts[m]);
xue@1:       sumstart+=1; //sqrt(starts[m]);
xue@1:     }
xue@1:     double lstop=sumstop/sumstart;
xue@1:     if (lstop>0.5) {fr0++; break;}
xue@1: 
xue@1:     fr0--;
xue@1:   }
xue@1: 
xue@1:   //now fr0 and fr1 are the first and last (excl.) frame indices
xue@1:   Fr=fr1-fr0;
xue@1:   cdouble* ipfr=new cdouble[Fr];
xue@1:   double *as=new double[Fr*2], *phs=&as[Fr];
xue@1:   cdouble** Allocate2(cdouble, Fr, wid/2+1, xx);
xue@1:   for (int fr=0; fr<Fr; fr++)
xue@1:   {
xue@1:     spec=Spec->Spec(fr0+fr);
xue@1:     for (int i=0; i<=wid/2; i++) xx[fr][i]=spec[i];
xue@1:   }
xue@1: 
xue@1:   //reestimate partial frequencies, amplitudes and phase angles using all frames. In case of frequency estimation
xue@1:   //failure, the original frequency is used and all atoms of that partial are typed "atInfered".
xue@1:   for (int m=0; m<P; m++)
xue@1:   {
xue@1:     double fm=f0*(m+1)*sqrt(1+B*((m+1)*(m+1)-1)), dfm=(settings.delm<1)?settings.delm:1;
xue@1:     double errf=LSESinusoidMP(fm, fm-dfm, fm+dfm, xx, Fr, wid, 3, settings.M, settings.c, settings.iH2, as, phs, 1e-6);
xue@1:   //    LSESinusoidMPC(fm, fm-1, fm+1, xx, Fr, wid, offst, 3, settings.M, settings.c, settings.iH2, as, phs, 1e-6);
xue@1:     atomtype type=(errf<0)?atInfered:atPeak;
xue@1:     for (int fr=0; fr<Fr; fr++)
xue@1:     {
xue@1:       double tfr=(fr0+fr)*offst+wid/2;
xue@1:       atom tmpatom={tfr, wid, fm/wid, as[fr], phs[fr], m+1, type, 0};
xue@1:       part[atomcount++]=tmpatom;
xue@1:     }
xue@1:   }
xue@1: 
xue@1:   //Tag the atom at initial input (_t, _f) as anchor.
xue@1:   AtomsToPartials(atomcount, part, M, Fr, partials, offst);
xue@1:   partials[settings.pin0-1][trst-fr0].type=atAnchor;
xue@1:   delete[] ipfr;
xue@1:   delete[] ipw;
xue@1:   delete[] part;
xue@1:   delete R; delete[] x; delete[] as; DeAlloc2(xx);
xue@1:   delete[] starts;
xue@1:   return 1;
xue@1: }//FindNoteConst
xue@1: 
Chris@5: /**
xue@1:   function FindNoteF: forward harmonic sinusoid tracking starting from a given harmonic atom until an
xue@1:   endpoint is detected or a search boundary is reached.
xue@1: 
xue@1:   In: starts: harmonic atom score of the start HA
xue@1:       startvfp[startp]: amplitudes of partials of start HA
xue@1:       R: F-G polygon of the start HA
xue@1:       frst, frst: frame index of the start and end frames of tracking.
xue@1:       Spec: spectrogram
xue@1:       wid, offst: atom scale and hop size, must be consistent with Spec
xue@1:       settings: note match settings
xue@1:       brake: tracking termination threshold.
xue@1:   Out: part[return value]: list of atoms tracked.
xue@1:        starts: maximal HA score of tracked frames. Its product with brake is used as the tracking threshold.
xue@1: 
xue@1:   Returns the number of atoms found.
xue@1: */
xue@1: int FindNoteF(atom* part, double& starts, TPolygon* R, int startp, double* startvfp, int frst, int fren, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings, double brake)
xue@1: {
xue@1:   settings.forcepin0=false, settings.pin0=0;
xue@1: 
xue@1:   int k=0, maxp=settings.maxp;
xue@1: 
xue@1:   TPolygon* RR=new TPolygon(1024, R);
xue@1:   int lastp=startp;
xue@1:   double *lastvfp=new double[settings.maxp]; memcpy(lastvfp, startvfp, sizeof(double)*startp);
xue@1: 
xue@1:   cdouble *x=new cdouble[wid/2+1];
xue@1:   double *fpp=new double[maxp*4], *vfpp=&fpp[maxp], *pfpp=&fpp[maxp*2]; atomtype* ptype=(atomtype*)&fpp[maxp*3];
xue@1:   NMResults results={fpp, vfpp, pfpp, ptype};
xue@1:   int step=(fren>frst)?1:(-1);
xue@1:   for (int h=frst+step; (h-fren)*step<0; h+=step)
xue@1:   {
xue@1:     cmplx<QSPEC_FORMAT>* spec=Spec->Spec(h);
xue@1:     for (int i=0; i<=wid/2; i++) x[i]=spec[i];
xue@1:     double f0=0, B=0;
xue@1:     double tmp=NoteMatchStiff3(RR, f0, B, 1, &x, wid, 0, &settings, &results, lastp, lastvfp);
xue@1:     if (starts<tmp) starts=tmp;
xue@1:     if (tmp<starts*brake) break;  //end condition: power drops by ??dB
xue@1: 
xue@1:     lastp=NMResultToAtoms(settings.maxp, &part[k], h*offst+wid/2, wid, results); k+=lastp;
xue@1:     memcpy(lastvfp, vfpp, sizeof(double)*lastp);
xue@1:   }
xue@1:   delete RR; delete[] lastvfp; delete[] x; delete[] fpp;
xue@1:   return k;
xue@1: }//FindNoteF
xue@1: 
Chris@5: /**
xue@1:   function FindNoteFB: forward-backward harmonic sinusid tracking.
xue@1: 
xue@1:   In: frst, fren: index of start and end frames
xue@1:       Rst, Ren: F-G polygons of start and end harmonic atoms
xue@1:       vfpst[M], vfpen[M]: amplitude vectors of start and end harmonic atoms
xue@1:       Spec: spectrogram
xue@1:       wid, offst: atom scale and hop size, must be consistent with Spec
xue@1:       settings: note match settings
xue@1:   Out: partials[0:fren-frst][M], hosting tracked HS between frst and fren (inc).
xue@1: 
xue@1:   Returns 0 if successful, 1 if failure in pitch tracking stage (no feasible HA candidate at some frame).
xue@1:   On start partials[0:fren-frst][M] shall be prepared to receive fren-frst+1 harmonic atoms
xue@1: */
xue@1: int FindNoteFB(int frst, TPolygon* Rst, double* vfpst, int fren, TPolygon* Ren, double* vfpen, int M, atom** partials, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings)
xue@1: {           //*
xue@1:   if (frst>fren) return -1;
xue@1:   int result=0;
xue@1:   if (settings.maxp>M) settings.maxp=M;
xue@1:   else M=settings.maxp;
xue@1:   int Fr=fren-frst+1;
xue@1:   double B, fpp[1024], delm=settings.delm, delp=settings.delp, iH2=settings.iH2;
xue@1:   double *vfpp=&fpp[256], *pfpp=&fpp[512]; atomtype *ptype=(atomtype*)&fpp[768];
xue@1:   int maxpitch=50;
xue@1: 
xue@1:   NMResults results={fpp, vfpp, pfpp, ptype};
xue@1: 
xue@1:   cmplx<QSPEC_FORMAT>* spec;
xue@1: 
xue@1:   cdouble *x=new cdouble[wid/2+1];
xue@1:   TPolygon **Ra=new TPolygon*[maxpitch], **Rb=new TPolygon*[maxpitch], ***R; memset(Ra, 0, sizeof(TPolygon*)*maxpitch), memset(Rb, 0, sizeof(TPolygon*)*maxpitch);
xue@1:   double *sca=new double[maxpitch], *scb=new double[maxpitch], **sc; sca[0]=scb[0]=0;
xue@1:   double** Allocate2(double, maxpitch, M, vfpa); memcpy(vfpa[0], vfpst, sizeof(double)*M);
xue@1:   double** Allocate2(double, maxpitch, M, vfpb); memcpy(vfpb[0], vfpen, sizeof(double)*M);
xue@1:   double*** vfp;
xue@1: 
xue@1:   double** Allocate2(double, Fr, wid/2, fps);
xue@1:   double** Allocate2(double, Fr, wid/2, vps);
xue@1:   int* pc=new int[Fr];
xue@1: 
xue@1:   Ra[0]=new TPolygon(M*2+4, Rst);
xue@1:   Rb[0]=new TPolygon(M*2+4, Ren);
xue@1:   int pitchcounta=1, pitchcountb=1, pitchcount;
xue@1: 
xue@1:   //pitch tracking
xue@1:   int** Allocate2(int, Fr, maxpitch, prev);
xue@1:   int** Allocate2(int, Fr, maxpitch, pitches);
xue@1:   int* pitchres=new int[Fr];
xue@1:   int fra=frst, frb=fren, fr;
xue@1:   while (fra<frb-1)
xue@1:   {
xue@1:     if (pitchcounta<pitchcountb){fra++; fr=fra; pitchcount=pitchcounta; vfp=&vfpa; sc=&sca; R=&Ra;}
xue@1:     else {frb--; fr=frb; pitchcount=pitchcountb; vfp=&vfpb; sc=&scb, R=&Rb;}
xue@1:     int fr_frst=fr-frst;
xue@1: 
xue@1:     int absfr=(partials[0][fr].t-wid/2)/offst;
xue@1:     spec=Spec->Spec(absfr); for (int i=0; i<=wid/2; i++) x[i]=spec[i];
xue@1:     pc[fr_frst]=QuickPeaks(fps[fr_frst], vps[fr_frst], wid, x, settings.M, settings.c, iH2, 0.0005);
xue@1:     NoteMatchStiff3FB(pitchcount, *R, *vfp, *sc, pitches[fr_frst], prev[fr_frst], pc[fr_frst], fps[fr_frst], vps[fr_frst], x, wid, maxpitch, &settings);
xue@1:     if (fr==fra) pitchcounta=pitchcount;
xue@1:     else pitchcountb=pitchcount;
xue@1:     if (pitchcount<=0)
xue@1:     {result=1; goto cleanup;}
xue@1:   }
xue@1:   {
xue@1:    //now fra=frb-1
xue@1:     int fra_frst=fra-frst, frb_frst=frb-frst, maxpitcha=0, maxpitchb=0;
xue@1:     double maxs=0;
xue@1:     for (int i=0; i<pitchcounta; i++)
xue@1:     {
xue@1:       double f0a, f0b;
xue@1:       if (fra==frst) f0a=partials[0][frst].f*wid;
xue@1:       else
xue@1:       {
xue@1:         int peak0a=((__int16*)&pitches[fra_frst][i])[0], pin0a=((__int16*)&pitches[fra_frst][i])[1];
xue@1:         f0a=fps[fra_frst][peak0a]/pin0a;
xue@1:       }
xue@1:       for (int j=0; j<pitchcountb; j++)
xue@1:       {
xue@1:         if (frb==fren) f0b=partials[0][fren].f*wid;
xue@1:         else
xue@1:         {
xue@1:           int peak0b=((__int16*)&pitches[frb_frst][j])[0], pin0b=((__int16*)&pitches[frb_frst][j])[1];
xue@1:           f0b=fps[frb_frst][peak0b]/pin0b;
xue@1:         }
xue@1:         double pow2delp=pow(2, delp/12);
xue@1:         if (f0b<f0a*pow2delp+delm && f0b>f0a/pow2delp-delm)
xue@1:         {
xue@1:           double s=sca[i]+scb[j]+conta(M, vfpa[i], vfpb[j]);
xue@1:           if (maxs<s) maxs=s, maxpitcha=i, maxpitchb=j;
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     //get pitch tracking result
xue@1:     pitchres[fra_frst]=pitches[fra_frst][maxpitcha];
xue@1:     for (int fr=fra; fr>frst; fr--)
xue@1:     {
xue@1:       maxpitcha=prev[fr-frst][maxpitcha];
xue@1:       pitchres[fr-frst-1]=pitches[fr-frst-1][maxpitcha];
xue@1:     }
xue@1:     pitchres[frb_frst]=pitches[frb_frst][maxpitchb];
xue@1:     for (int fr=frb; fr<fren; fr++)
xue@1:     {
xue@1:       maxpitchb=prev[fr-frst][maxpitchb];
xue@1:       pitchres[fr-frst+1]=pitches[fr-frst+1][maxpitchb];
xue@1:     }
xue@1:   }
xue@1: {
xue@1:   double f0s[1024]; memset(f0s, 0, sizeof(double)*1024);
xue@1:   for (int i=frst+1; i<fren; i++)
xue@1:   {
xue@1:     int pitch=pitchres[i-frst];
xue@1:     int peak0=((__int16*)&pitch)[0], pin0=((__int16*)&pitch)[1];
xue@1:     f0s[i]=fps[i-frst][peak0]/pin0;
xue@1:   }
xue@1:   f0s[frst]=sqrt(Rst->X[0]), f0s[fren]=sqrt(Ren->X[0]);
xue@1: 
xue@1:   //partial tracking
xue@1:   int lastp=0; while (lastp<M && vfpst[lastp]>0) lastp++;
xue@1:   double* lastvfp=new double[M]; memcpy(lastvfp, vfpst, sizeof(double)*M);
xue@1:   delete Ra[0]; Ra[0]=new TPolygon(M*2+4, Rst);
xue@1:   for (int h=frst+1; h<fren; h++)
xue@1:   {
xue@1:     int absfr=(partials[0][h].t-wid/2)/offst;
xue@1:     int h_frst=h-frst, pitch=pitchres[h_frst];
xue@1:     int peak0=((__int16*)&pitch)[0], pin0=((__int16*)&pitch)[1];
xue@1:     spec=Spec->Spec(absfr);
xue@1:     double f0=fpp[0];//, B=Form11->BEdit->Text.ToDouble();
xue@1:     //double deltaf=f0*(pow(2, settings.delp/12)-1);
xue@1:     for (int i=0; i<=wid/2; i++) x[i]=spec[i];
xue@1:     memset(fpp, 0, sizeof(double)*1024);
xue@1:     //settings.epf0=deltaf,
xue@1:     settings.forcepin0=true; settings.pin0=pin0; f0=fps[h_frst][peak0]; settings.pin0asanchor=false;
xue@1:     if (NoteMatchStiff3(Ra[0], f0, B, pc[h_frst], fps[h_frst], vps[h_frst], 1, &x, wid, 0, &settings, &results, lastp, lastvfp)>0)
xue@1:     {
xue@1:       lastp=NMResultToPartials(M, h, partials, partials[0][h].t, wid, results);
xue@1:       memcpy(lastvfp, vfpp, sizeof(double)*lastp);
xue@1:     }
xue@1:   }
xue@1:   delete[] lastvfp;
xue@1: }
xue@1: cleanup:
xue@1:   delete[] x;
xue@1:   for (int i=0; i<pitchcounta; i++) delete Ra[i]; delete[] Ra;
xue@1:   for (int i=0; i<pitchcountb; i++) delete Rb[i]; delete[] Rb;
xue@1:   delete[] sca; delete[] scb;
xue@1:   DeAlloc2(vfpa); DeAlloc2(vfpb);
xue@1:   DeAlloc2(fps); DeAlloc2(vps);
xue@1:   DeAlloc2(pitches); DeAlloc2(prev);
xue@1:   delete[] pitchres; delete[] pc;
xue@1:   return result;  //*/
xue@1: }//FindNoteFB
xue@1: 
Chris@5: /**
xue@1:   function GetResultsSingleFr: used by NoteMatchStiff3 to obtain final note match results after harmonic
xue@1:   grouping of peaks with rough frequency estimates, including a screening of found peaks based on shape
xue@1:   and consistency with other peak frequencies, reestimating sinusoid parameters using LSE, estimating
xue@1:   atoms without peaks by inferring their frequencies, and translating internal peak type to atom type.
xue@1: 
xue@1:   In: R0: F-G polygon of primitive (=rough estimates) partial frequencies
xue@1:       x[N]: spectrum
xue@1:       ps[P], fs[P]; partial indices and frequencies, may miss partials
xue@1:       rsrs[P]: peak shape factors, used for evaluating peak validity
xue@1:       psb[P]: peak type flags, related to atom::ptype.
xue@1:       settings: note match settings
xue@1:       numsam: number of partials to evaluate (=number of atoms to return)
xue@1:   Out: results: note match results as a NMResult structure
xue@1:        f0, B: fundamental frequency and stiffness coefficient
xue@1:        Rret: F-G polygon of reestimated set of partial frequencies
xue@1: 
xue@1:   Return the total energy of the harmonic atom.
xue@1: */
xue@1: double GetResultsSingleFr(double& f0, double& B, TPolygon* Rret, TPolygon* R0, int P, int* ps, double* fs, double* rsrs, cdouble* x, int N, int* psb, int numsam, NMSettings* settings, NMResults* results)
xue@1: {
xue@1:   Rret->N=0;
xue@1:   double delm=settings->delm, *c=settings->c, iH2=settings->iH2, epf=settings->epf, maxB=settings->maxB, hB=settings->hB;
xue@1:   int M=settings->M, maxp=settings->maxp;
xue@1:   double *fp=results->fp; memset(fp, 0, sizeof(double)*maxp);
xue@1: 
xue@1:   double result=0;
xue@1:   if (P>0)
xue@1:   {
xue@1:     double *vfp=results->vfp, *pfp=results->pfp;
xue@1:     atomtype *ptype=results->ptype; //memset(ptype, 0, sizeof(int)*maxp);
xue@1: 
xue@1:       double *f1=new double[(maxp+1)*4], *f2=&f1[maxp+1], *ft=&f2[maxp+1], *fdep=&ft[maxp+1], tmpa, cF, cG;
xue@1:       areaandcentroid(tmpa, cF, cG, R0->N, R0->X, R0->Y);
xue@1:       for (int p=1; p<=maxp; p++) ExFmStiff(f1[p], f2[p], p, R0->N, R0->X, R0->Y), ft[p]=p*sqrt(cF+(p*p-1)*cG);
xue@1:       //sort peaks by rsr and departure from model so that most reliable ones are found at the start
xue@1:       double* values=new double[P]; int* indices=new int[P];
xue@1:       for (int i=0; i<P; i++)
xue@1:       {
xue@1:         values[i]=1e200;
xue@1:         int lp=ps[i]; double lf=fs[i];
xue@1:         if (lf>=f1[lp] && lf<=f2[lp]) fdep[lp]=0;
xue@1:         else if (lf<f1[lp]) fdep[lp]=f1[lp]-lf;
xue@1:         else if (lf>f2[lp]) fdep[lp]=lf-f2[lp];
xue@1:         double tmpv=(fdep[lp]>0.5)?(fdep[lp]-0.5)*2:0;
xue@1:         tmpv+=rsrs?rsrs[i]:0;
xue@1:         tmpv*=pow(lp, 0.2);
xue@1:         InsertInc(tmpv, i, values, indices, i+1);
xue@1:       }
xue@1: 
xue@1:     for (int i=0; i<P; i++)
xue@1:     {
xue@1:       int ind=indices[i];
xue@1:       int lp=ps[ind]; //partial index
xue@1:         //Get LSE estimation of atoms
xue@1:         double lf=fs[ind], f1, f2, la, lph;//, lrsr=rsrs?rsrs[ind]:0
xue@1:         if (lp==0 || lf==0) continue;
xue@1:         ExFmStiff(f1, f2, lp, R0->N, R0->X, R0->Y);
xue@1:         LSESinusoid(lf, f1-delm, f2+delm, x, N, 3, M, c, iH2, la, lph, epf);
xue@1:           //lrsr=PeakShapeC(lf, 1, N, &x, 6, M, c, iH2);
xue@1:         if (Rret->N>0) ExFmStiff(f1, f2, lp, Rret->N, Rret->X, Rret->Y);
xue@1:         if (Rret->N==0 || (lf>=f1 && lf<=f2))
xue@1:         {
xue@1:           fp[lp-1]=lf;
xue@1:           vfp[lp-1]=la;
xue@1:           pfp[lp-1]=lph;
xue@1:           if (psb[lp]==1 || (psb[lp]==2 && settings->pin0asanchor)) ptype[lp-1]=atAnchor; //0 for anchor points
xue@1:           else ptype[lp-1]=atPeak; //1 for local maximum
xue@1:           //update R using found partails with amplitude>1
xue@1:           if (Rret->N==0) InitializeR(Rret, lp, lf, delm, maxB);
xue@1:           else if (la>1) CutR(Rret, lp, lf, delm, true);
xue@1:         }
xue@1:     }
xue@1:     //*
xue@1:     for (int p=1; p<=maxp; p++)
xue@1:     {
xue@1:       if (fp[p-1]>0)
xue@1:       {
xue@1:         double f1, f2;
xue@1:         ExFmStiff(f1, f2, p, Rret->N, Rret->X, Rret->Y);
xue@1:         if (fp[p-1]<f1-0.3 || fp[p-1]>f2+0.3)
xue@1:           fp[p-1]=0;
xue@1:       }
xue@1:     }// */
xue@1: 
xue@1: 
xue@1:     //estimate f0 and B
xue@1:     double norm[1024]; for (int i=0; i<1024; i++) norm[i]=1;
xue@1:     areaandcentroid(tmpa, cF, cG, Rret->N, Rret->X, Rret->Y); //minimaxstiff(cF, cG, P, ps, fs, norm, R->N, R->X, R->Y);
xue@1:     testnn(cF); f0=sqrt(cF); B=cG/cF;
xue@1: 
xue@1:     //Get LSE estimates for unfound partials
xue@1:     for (int i=0; i<numsam; i++)
xue@1:     {
xue@1:       if (fp[i]==0) //no peak is found for this partial in lcand
xue@1:       {
xue@1:         int m=i+1;
xue@1:         double tmp=cF+(m*m-1)*cG; testnn(tmp);
xue@1:         double lf=m*sqrt(tmp);
xue@1:         if (lf<N/2.1)
xue@1:         {
xue@1:           fp[i]=lf;
xue@1:           ptype[i]=atInfered; //2 for non peak
xue@1:           cdouble r=IPWindowC(lf, x, N, M, c, iH2, lf-hB, lf+hB);
xue@1:           vfp[i]=sqrt(r.x*r.x+r.y*r.y);
xue@1:           pfp[i]=Atan2(r.y, r.x);//*/
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     if (f0>0) for (int i=0; i<numsam; i++) if (fp[i]>0) result+=vfp[i]*vfp[i];
xue@1: 
xue@1:       delete[] f1; delete[] values; delete[] indices;
xue@1:   }
xue@1:   return result;
xue@1: }//GetResultsSingleFr
xue@1: 
Chris@5: /**
xue@1:   function GetResultsMultiFr: the constant-pitch multi-frame version of GetResultsSingleFr.
xue@1: 
xue@1:   In: x[Fr][N]: spectrogram
xue@1:       ps[P], fs[P]; partial indices and frequencies, may miss partials
xue@1:       psb[P]: peak type flags, related to atom::ptype.
xue@1:       settings: note match settings
xue@1:       forceinputlocalfr: specifies if partial settings->pin0 is taken for granted ("pinned")
xue@1:       numsam: number of partials to evaluate (=number of atoms to return)
xue@1:   Out: results[Fr]: note match results as Fr NMResult structures
xue@1:        f0, B: fundamental frequency and stiffness coefficient
xue@1:        Rret: F-G polygon of reestimated set of partial frequencies
xue@1: 
xue@1:   Return the total energy of the harmonic sinusoid.
xue@1: */
xue@1: double GetResultsMultiFr(double& f0, double& B, TPolygon* Rret, TPolygon* R0, int P, int* ps, double* fs, double* rsrs, int Fr, cdouble** x, int N, int offst, int* psb, int numsam, NMSettings* settings, NMResults* results, int forceinputlocalfr)
xue@1: {
xue@1:   Rret->N=0;
xue@1:   double delm=settings->delm, *c=settings->c, iH2=settings->iH2, epf=settings->epf, maxB=settings->maxB, hB=settings->hB;// *pinf=settings->pinf;
xue@1: 
xue@1:   int M=settings->M, maxp=settings->maxp, pincount=settings->pcount, *pin=settings->pin, *pinfr=settings->pinfr;
xue@1: //	double *fp=results->fp, *vfp=results->vfp, *pfp=results->pfp;
xue@1: //	int *ptype=results->ptype;
xue@1:   for (int fr=0; fr<Fr; fr++)	memset(results[fr].fp, 0, sizeof(double)*maxp);
xue@1:   int* pclass=new int[maxp+1]; memset(pclass, 0, sizeof(int)*(maxp+1));
xue@1:   for (int i=0; i<pincount; i++) if (pinfr[i]>=0) pclass[pin[i]]=1;
xue@1:   if (forceinputlocalfr>=0) pclass[settings->pin0]=1;
xue@1:   double result=0;
xue@1:   if (P>0)
xue@1:   {
xue@1:     double tmpa, cF, cG;
xue@1:     //found atoms
xue@1:     double *as=new double[Fr*2], *phs=&as[Fr];
xue@1:     for (int i=P-1; i>=0; i--)
xue@1:     {
xue@1:       int lp=ps[i];
xue@1:       double lf=fs[i];
xue@1:       if (lp==0 || lf==0) continue;
xue@1: 
xue@1:       if (!pclass[lp])
xue@1:       {
xue@1:         //Get LSE estimation of atoms
xue@1:         double startlf=lf;
xue@1:         LSESinusoidMP(lf, lf-1, lf+1, x, Fr, N, 3, M, c, iH2, as, phs, epf);
xue@1:         //LSESinusoidMPC(lf, lf-1, lf+1, x, Fr, N, offst, 3, M, c, iH2, as, phs, epf);
xue@1:         if (fabs(lf-startlf)>0.6)
xue@1:         {
xue@1:           lf=startlf;
xue@1:           for (int fr=0; fr<Fr; fr++)
xue@1:           {
xue@1:             cdouble r=IPWindowC(lf, x[fr], N, M, c, iH2, lf-settings->hB, lf+settings->hB);
xue@1:             as[fr]=abs(r);
xue@1:             phs[fr]=arg(r);
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:       else //frequencies of local anchor atoms are not re-estimated
xue@1:       {
xue@1:         for (int fr=0; fr<Fr; fr++)
xue@1:         {
xue@1:           cdouble r=IPWindowC(lf, x[fr], N, M, c, iH2, lf-settings->hB, lf+settings->hB);
xue@1:           as[fr]=abs(r);
xue@1:           phs[fr]=arg(r);
xue@1:         }
xue@1:       }
xue@1: 
xue@1:         //LSESinusoid(lf, f1-delm, f2+delm, x, N, 3, M, c, iH2, la, lph, epf);
xue@1:         //fp[lp-1]=lf; vfp[lp-1]=la; pfp[lp-1]=lph;
xue@1:       for (int fr=0; fr<Fr; fr++) results[fr].fp[lp-1]=lf, results[fr].vfp[lp-1]=as[fr], results[fr].pfp[lp-1]=phs[fr];
xue@1:       if (psb[lp]==1 || (psb[lp]==2 && settings->pin0asanchor)) for (int fr=0; fr<Fr; fr++) results[fr].ptype[lp-1]=atAnchor; //0 for anchor points
xue@1:       else for (int fr=0; fr<Fr; fr++) results[fr].ptype[lp-1]=atPeak; //1 for local maximum
xue@1:       //update R using found partails with amplitude>1
xue@1:       if (Rret->N==0)
xue@1:       {
xue@1:       //temporary treatment: +0.5 should be +rsr or something similar
xue@1:         InitializeR(Rret, lp, lf, delm+0.5, maxB);
xue@1:         areaandcentroid(tmpa, cF, cG, Rret->N, Rret->X, Rret->Y); //minimaxstiff(cF, cG, P, ps, fs, norm, R->N, R->X, R->Y);
xue@1:       }
xue@1:       else //if (la>1)
xue@1:       {
xue@1:         CutR(Rret, lp, lf, delm+0.5, true);
xue@1:         areaandcentroid(tmpa, cF, cG, Rret->N, Rret->X, Rret->Y); //minimaxstiff(cF, cG, P, ps, fs, norm, R->N, R->X, R->Y);
xue@1:       }
xue@1:     }
xue@1:     //estimate f0 and B
xue@1:     double norm[1024]; for (int i=0; i<1024; i++) norm[i]=1;
xue@1:     areaandcentroid(tmpa, cF, cG, Rret->N, Rret->X, Rret->Y); //minimaxstiff(cF, cG, P, ps, fs, norm, R->N, R->X, R->Y);
xue@1:     testnn(cF); f0=sqrt(cF); B=cG/cF;
xue@1: 
xue@1:     //Get LSE estimates for unfound partials
xue@1:     for (int i=0; i<numsam; i++)
xue@1:     {
xue@1:       if (results[0].fp[i]==0) //no peak is found for this partial in lcand
xue@1:       {
xue@1:         int m=i+1;
xue@1:         double tmp=cF+(m*m-1)*cG; testnn(tmp);
xue@1:         double lf=m*sqrt(tmp);
xue@1:         if (lf<N/2.1)
xue@1:         {
xue@1:           for (int fr=0; fr<Fr; fr++)
xue@1:           {
xue@1:             results[fr].fp[i]=lf, results[fr].ptype[i]=atInfered; //2 for non peak
xue@1:             int k1=ceil(lf-hB); if (k1<0) k1=0;
xue@1:             int k2=floor(lf+hB); if (k2>=N/2) k2=N/2-1;
xue@1:             cdouble r=IPWindowC(lf, x[fr], N, M, c, iH2, k1, k2);
xue@1:             results[fr].vfp[i]=abs(r);
xue@1:             results[fr].pfp[i]=arg(r);
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     delete[] as;
xue@1:     if (f0>0) for (int fr=0; fr<Fr; fr++) for (int i=0; i<numsam; i++) if (results[fr].fp[i]>0) result+=results[fr].vfp[i]*results[fr].vfp[i];
xue@1:     result/=Fr;
xue@1:   }
xue@1:   delete[] pclass;
xue@1:   return result;
xue@1: }//GetResultsMultiFr
xue@1: 
Chris@5: /**
xue@1:   function InitailizeR: initializes a F-G polygon with a fundamental frequency range and stiffness coefficient bound
xue@1: 
xue@1:   In: af, ef: centre and half width of the fundamental frequency range
xue@1:       maxB: maximal value of stiffness coefficient (the minimal is set to 0)
xue@1:   Out: R: the initialized F-G polygon.
xue@1: 
xue@1:   No reutrn value.
xue@1: */
xue@1: void InitializeR(TPolygon* R, double af, double ef, double maxB)
xue@1: {
xue@1:   R->N=4;
xue@1:   double *X=R->X, *Y=R->Y;
xue@1:   double g1=af-ef, g2=af+ef;
xue@1:   if (g1<0) g1=0;
xue@1:   g1=g1*g1, g2=g2*g2;
xue@1:   X[0]=X[3]=g1, X[1]=X[2]=g2;
xue@1:   Y[0]=X[0]*maxB, Y[1]=X[1]*maxB, Y[2]=Y[3]=0;
xue@1: }//InitializeR
xue@1: 
Chris@5: /**
xue@1:   function InitialzeR: initializes a F-G polygon with a frequency range for a given partial and
xue@1:   stiffness coefficient bound
xue@1: 
xue@1:   In: apind: partial index
xue@1:       af, ef: centre and half width of the frequency range of the apind-th partial
xue@1:       maxB; maximal value of stiffness coefficient (the minimal is set to 0)
xue@1:   Out: R: the initialized F-G polygon.
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void InitializeR(TPolygon* R, int apind, double af, double ef, double maxB)
xue@1: {
xue@1:   R->N=4;
xue@1:   double *X=R->X, *Y=R->Y;
xue@1:   double k=apind*apind-1;
xue@1:   double g1=(af-ef)/apind, g2=(af+ef)/apind;
xue@1:   if (g1<0) g1=0;
xue@1:   g1=g1*g1, g2=g2*g2;
xue@1:   double kb1=1+k*maxB;
xue@1:   X[0]=g1/kb1, X[1]=g2/kb1, X[2]=g2, X[3]=g1;
xue@1:   Y[0]=X[0]*maxB, Y[1]=X[1]*maxB, Y[2]=Y[3]=0;
xue@1: }//InitializeR
xue@1: 
Chris@5: /**
xue@1:   function maximalminimum: finds the point within a polygon that maximizes its minimal distance to the
xue@1:   sides.
xue@1: 
xue@1:   In: sx[N], sy[N]: x- and y-coordinates of vertices of a polygon
xue@1:   Out: (x, y): point within the polygon with maximal minimal distance to the sides
xue@1: 
xue@1:   Returns the maximial minimal distance. A circle centred at (x, y) with the return value as the radius
xue@1:   is the maximum inscribed circle of the polygon.
xue@1: */
xue@1: double maximalminimum(double& x, double& y, int N, double* sx, double* sy)
xue@1: {
xue@1:   //at the beginning let (x, y) be (sx[0], sy[0]),  then the mininum distance d is 0.
xue@1:   double dm=0;
xue@1:   x=sx[0], y=sy[0];
xue@1:   double *A=new double[N*3];
xue@1:   double *B=&A[N], *C=&A[N*2];
xue@1:   //calcualte equations of all sides A[k]x+B[k]y+C[k]=0, with signs adjusted so that for
xue@1:   //  any (x, y) within the polygon, A[k]x+B[k]y+C[k]>0. A[k]^2+B[k]^2=1.
xue@1:   for (int n=0; n<N; n++)
xue@1:   {
xue@1:     double x1=sx[n], y1=sy[n], x2, y2, AA, BB, CC;
xue@1:     if (n+1!=N) x2=sx[n+1], y2=sy[n+1];
xue@1:     else x2=sx[0], y2=sy[0];
xue@1:     double dx=x1-x2, dy=y1-y2;
xue@1:     double ds=sqrt(dx*dx+dy*dy);
xue@1:     AA=dy/ds, BB=-dx/ds;
xue@1:     CC=-AA*x1-BB*y1;
xue@1:     //adjust signs
xue@1:     if (n+2<N) x2=sx[n+2], y2=sy[n+2];
xue@1:     else x2=sx[n+2-N], y2=sy[n+2-N];
xue@1:     if (AA*x2+BB*y2+CC<0) A[n]=-AA, B[n]=-BB, C[n]=-CC;
xue@1:     else A[n]=AA, B[n]=BB, C[n]=CC;
xue@1:   }
xue@1: 
xue@1:   //during the whole process (x, y) is always equal-distance to at least two sides,
xue@1:   //  namely l1--(l1+1) and l2--(l2+1). there equations are A1x+B1y+C1=0 and A2x+B2y+C2=0,
xue@1:   //  where A^2+B^2=1.
xue@1:   int l1=0, l2=N-1;
xue@1: 
xue@1:   double a, b;
xue@1:     b=A[l1]-A[l2], a=B[l2]-B[l1];
xue@1:     double ds=sqrt(a*a+b*b);
xue@1:     a/=ds, b/=ds;
xue@1:     //the line (x+at, y+bt) passes (x, y), and points on this line are equal-distance to l1 and l2.
xue@1:     //along this line at point (x, y), we have dx=ads, dy=bds
xue@1:     //now find the signs so that dm increases as ds>0
xue@1:     double ddmds=A[l1]*a+B[l1]*b;
xue@1:     if (ddmds<0) a=-a, b=-b, ddmds=-ddmds;
xue@1: 
xue@1:   while (true)
xue@1:   {
xue@1:     //now the vector starting from (x, y) pointing in (a, b) is equi-distance-to-l1-and-l2 and
xue@1:     //  dm-increasing. actually at s from (x,y), d=dm+ddmds*s.
xue@1: 
xue@1:     //it is now guaranteed that the distance of (x, y) to l1 (=l2) is smaller than to any other
xue@1:     //  sides. along direction (A, B) the distance of (x, y) to l1 (=l2) is increasing and
xue@1:     //  the distance to at least one other sides is increasing, so that at some value of s the
xue@1:     //  distances equal. we find the smallest s>0 where this happens.
xue@1:     int l3=-1;
xue@1:     double s=-1;
xue@1:     for (int n=0; n<N; n++)
xue@1:     {
xue@1:       if (n==l1 || n==l2) continue;
xue@1:       //distance of (x,y) to the side
xue@1:       double ldm=A[n]*x+B[n]*y+C[n]; //ldm>dm
xue@1:       double dldmds=A[n]*a+B[n]*b;
xue@1:       //so ld=ldm+lddmds*s, the equality is dm+ddmds*s=ldm+lddmds*s
xue@1:       if (ddmds-dldmds>0)
xue@1:       {
xue@1:         ds=(ldm-dm)/(ddmds-dldmds);
xue@1:         if (l3==-1) l3=n, s=ds;
xue@1:         else if (ds<s) l3=n, s=ds;
xue@1:       }
xue@1:     }
xue@1:     if (ddmds==0) s/=2;
xue@1:     x=x+a*s, y=y+b*s;
xue@1:     dm=A[l1]*x+B[l1]*y+C[l1];
xue@1: //    Form1->Canvas->Ellipse(x-dm, y-dm, x+dm, y+dm);
xue@1:     //(x, y) is equal-distance to l1, l2 and l3
xue@1:     //try use l3 to substitute l2
xue@1:     b=A[l1]-A[l3], a=B[l3]-B[l1];
xue@1:     ds=sqrt(a*a+b*b);
xue@1:     a/=ds, b/=ds;
xue@1:     ddmds=A[l1]*a+B[l1]*b;
xue@1:     if (ddmds<0) a=-a, b=-b, ddmds=-ddmds;
xue@1:     if (ddmds==0 || A[l2]*a+B[l2]*b>0)
xue@1:     {
xue@1:       l2=l3;
xue@1:     }
xue@1:     else  //l1<-l3 fails, then try use l3 to substitute l2
xue@1:     {
xue@1:       b=A[l3]-A[l2], a=B[l2]-B[l3];
xue@1:       ds=sqrt(a*a+b*b);
xue@1:       a/=ds, b/=ds;
xue@1:       ddmds=A[l3]*a+B[l3]*b;
xue@1:       if (ddmds<0) a=-a, b=-b, ddmds=-ddmds;
xue@1:       if (ddmds==0 || A[l1]*a+B[l1]*b>0)
xue@1:       {
xue@1:         l1=l3;
xue@1:       }
xue@1:       else break;
xue@1:     }
xue@1:   }
xue@1: 
xue@1:   delete[] A;
xue@1:   return dm;
xue@1: }//maximalminimum
xue@1: 
Chris@5: /**
xue@1:   function minimaxstiff: finds the point in polygon (N; F, G) that minimizes the maximal value of the
xue@1:   function
xue@1: 
xue@1:         | m[l]sqrt(F+(m[l]^2-1)*G)-f[l] |
xue@1:   e_l = | ----------------------------- | regarding l=0, ..., L-1
xue@1:         |            norm[l]            |
xue@1: 
xue@1:   In: _m[L], _f[L], norm[L]: coefficients in the above
xue@1:       F[N], G[N]: vertices of a F-G polygon
xue@1:   Out: (F1, G1): the mini-maximum.
xue@1: 
xue@1:   Returns the minimized maximum value.
xue@1: 
xue@1:   Further reading: Wen X,  "Estimating model parameters", Ch.3.2.6 from "Harmonic sinusoid modeling of
xue@1:   tonal music events", PhD thesis, University of London, 2007.
xue@1: */
xue@1: double minimaxstiff(double& F1, double& G1, int L, int* _m, double* _f, double* norm, int N, double* F, double* G)
xue@1: {
xue@1:   if (L==0 || N<=2)
xue@1:   {
xue@1:     F1=F[0], G1=G[0];
xue@1:     return 0;
xue@1:   }
xue@1:   //normalizing
xue@1:   double* m=(double*)malloc(sizeof(double)*L*6);//new double[L*6];
xue@1:   double* f=&m[L*2];
xue@1:   int* k=(int*)&m[L*4];
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     k[2*l]=_m[l]*_m[l]-1;
xue@1:     k[2*l+1]=k[2*l];
xue@1:     m[2*l]=_m[l]/norm[l];
xue@1:     m[2*l+1]=-m[2*l];
xue@1:     f[2*l]=_f[l]/norm[l];
xue@1:     f[2*l+1]=-f[2*l];
xue@1:   }
xue@1:   //From this point on the L distance functions with absolute value is replace by 2L distance functions
xue@1:   L*=2;
xue@1:   double* vmnl=new double[N*2];
xue@1:   int* mnl=(int*)&vmnl[N];
xue@1: start:
xue@1:   //Initialize (F0, G0) to be the polygon vertex that has the minimal max_l(e_l)
xue@1:   //  maxn: the vertex index
xue@1:   //  maxl: the l that maximizes e_l at that vertex
xue@1:   //  maxsg: the sign of e_l before taking the abs. value
xue@1:   int nc=-1, nd;
xue@1:   int l1=-1, l2=0, l3=0;
xue@1:   double vmax=0;
xue@1: 
xue@1:   for (int n=0; n<N; n++)
xue@1:   {
xue@1:     int lmax=-1;
xue@1:     double lvmax=0;
xue@1:     for (int l=0; l<L; l++)
xue@1:     {
xue@1:       double tmp=F[n]+k[l]*G[n]; testnn(tmp);
xue@1:       double e=m[l]*sqrt(tmp)-f[l];
xue@1:       if (e>lvmax) lvmax=e, lmax=l;
xue@1:     }
xue@1:     mnl[n]=lmax, vmnl[n]=lvmax;
xue@1:     if (n==0)
xue@1:     {
xue@1:       vmax=lvmax, nc=n, l1=lmax;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       if (lvmax<vmax) vmax=lvmax, nc=n, l1=lmax;
xue@1:     }
xue@1:   }
xue@1:   double F0=F[nc], G0=G[nc];
xue@1: 
xue@1: 
xue@1: //    start searching the the minimal maximum from (F0, G0)
xue@1: //
xue@1: //    Each searching step starts from (F0, G0), ends at (F1, G1)
xue@1: //
xue@1: //    Starting conditions of one step:
xue@1: //
xue@1: //      (F0, G0) can be
xue@1: //        (1)inside polygon R;
xue@1: //        (2)on one side of R (nc:(nc+1) gives the side)
xue@1: //        (3)a vertex of R  (nc being the vertex index)
xue@1: //
xue@1: //      The maximum at (F0, G0) can be
xue@1: //        (1)vmax=e1=e2=e3>...; (l1, l2, l3)
xue@1: //        (2)vmax=e1=e2>...; (l1, l2)
xue@1: //        (3)vmax=e1>....  (l1)
xue@1: //
xue@1: //      More complication arise if we have more than 3 equal maxima, i.e.
xue@1: //        vmax=e1=e2=e3=e4>=.... CURRENTLY WE ASSUME THIS NEVER HAPPENS.
xue@1: //
xue@1: //      These are also the ending conditions of one step.
xue@1: //
xue@1: //    There are  types of basic searching steps, i.e.
xue@1: //
xue@1: //      (1) e1=e2 search: starting with e1=e2, search down the decreasing direction
xue@1: //          of e1=e2 until at point (F1, G1) there is another l3 so that e1(F1, G1)
xue@1: //          =e2(F1, G1)=e3(F1, G1), or until at point (F1, G1) the search is about
xue@1: //          to go out of R.
xue@1: //          Arguments: l1, l2, (F0, G0, vmax)
xue@1: //      (2) e1!=e2 search: starting with e1=e2 and (F0, G0) being on one side, search
xue@1: //          down the side in the decreasing direction of both e1 and e1 until at point
xue@1: //          (F1, G1) there is a l3 so that e3(F1, G1)=max(e1, e2)(F1, G1), or
xue@1: //          at a the search reaches a vertex of R.
xue@1: //          Arguments: l1, l2, dF, dG, (F0, G0, vmax)
xue@1: //      (3) e1 free search: starting with e1 being the only maximum, search down the decreasing
xue@1: //          direction of e1 until at point (F1, G1) there is another l2 so that
xue@1: //          e1(F1, G1)=e2(F1, G1), or until at point (F1, G1) the search is about
xue@1: //          to go out of R.
xue@1: //          Arguments: l1, dF, dG, (F0, G0, vmax)
xue@1: //      (4) e1 side search: starting with e1 being the only maximum, search down the
xue@1: //          a side of R in the decreasing direction of e1 until at point (F1, G1) there
xue@1: //          is another l2 so that e1=e2, or until point (F1, G1) the search reaches
xue@1: //          a vertex.
xue@1: //          Arguments: l1, destimation vertex nd, (F0, G0, vmax)
xue@1: //
xue@1: //    At the beginning of each searching step we check the conditions to see which
xue@1: //    basic step to perform, and provide the starting conditions. At the very start of
xue@1: //    the search, (F0, G0) is a vertex of R, e_l1 being the maximum at this point.
xue@1: //
xue@1: 
xue@1:   int condpos=3; //inside, on side, vertex
xue@1:   int condmax=3; //triple max, duo max, solo max
xue@1:   int searchmode;
xue@1: 
xue@1:   bool minimax=false; //set to true when the minimal maximum is met
xue@1: 
xue@1:   int iter=L*2;
xue@1:   while (!minimax && iter>0)
xue@1:   {
xue@1:     iter--;
xue@1:     double m1=m[l1], m2=m[l2], m3=m[l3], k1=k[l1], k2=k[l2], k3=k[l3];
xue@1:     double tmp, tmp1, tmp2, tmp3;
xue@1: 
xue@1:     switch (condmax)
xue@1:     {
xue@1:       case 1:
xue@1:         tmp=F0+k3*G0; testnn(tmp);
xue@1:         tmp3=sqrt(tmp);
xue@1:       case 2:
xue@1:         tmp=F0+k2*G0; testnn(tmp)
xue@1:         tmp2=sqrt(tmp);
xue@1:       case 3:
xue@1:         tmp=F0+k1*G0; testnn(tmp);
xue@1:         tmp1=sqrt(tmp);
xue@1:     }
xue@1:     double dF, dG;
xue@1:     int n0=(nc==0)?(N-1):(nc-1), n1=(nc==N-1)?0:(nc+1);
xue@1:     double x0, y0, x1, y1;
xue@1:     if (n0>=0) x0=F[nc]-F[n0], y0=G[nc]-G[n0];
xue@1:     if (nc>=0) x1=F[n1]-F[nc], y1=G[n1]-G[nc];
xue@1: 
xue@1:     if (condpos==1) //(F0, G0) being inside polygon
xue@1:     {
xue@1:       if (condmax==1) //e1=e2=e3 being the maximum
xue@1:       {
xue@1:         //vmax holds the maximum
xue@1:         //l1, l2, l3
xue@1: 
xue@1:         //now choose a searching direction, either e1=e2 or e1=e3 or e2=e3.
xue@1:         //  choose e1=e2 if (e3-e1) decreases along the decreasing direction of e1=e2
xue@1:         //  choose e1=e3 if (e2-e1) decreases along the decreasing direction of e1=e3
xue@1:         //  choose e2=e3 if (e1-e2) decreases along the decreasing directino of e2=e3
xue@1:         //  if no condition is satisfied, then (F0, G0) is the minimal maximum.
xue@1:         //calculate the decreasing direction of e1=e3 as (dF, dG)
xue@1:         double den=m1*m3*(k1-k3)/2;
xue@1:         dF=-(k1*m1*tmp3-k3*m3*tmp1)/den,
xue@1:         dG=(m1*tmp3-m3*tmp1)/den;
xue@1:         //the negative gradient of e2-e1 is calculated as (gF, gG)
xue@1:         double gF=-(m2/tmp2-m1/tmp1)/2,
xue@1:                gG=-(k2*m2/tmp2-k1*m1/tmp1)/2;
xue@1:         if (dF*gF+dG*gG>0) //so that e2-e1 decreases in the decreasing direction of e1=e3
xue@1:         {
xue@1:           l2=l3;
xue@1:           searchmode=1;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           //calculate the decreasing direction of e2=e3 as (dF, dG)
xue@1:           den=m2*m3*(k2-k3)/2;
xue@1:           dF=-(k2*m2*tmp3-k3*m3*tmp2)/den,
xue@1:           dG=(m2*tmp3-m3*tmp2)/den;
xue@1:           //calculate the negative gradient of e1-e2 as (gF, gG)
xue@1:           gF=-gF, gG=-gG;
xue@1:           if (dF*gF+dG*gG>0) //so that e1-e2 decreases in the decreasing direction of e2=e3
xue@1:           {
xue@1:             l1=l3;
xue@1:             searchmode=1;
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             //calculate the decreasing direction of e1=e2 as (dF, dG)
xue@1:             den=m1*m2*(k1-k2)/2;
xue@1:             dF=-(k1*m1*tmp2-k2*m2*tmp1)/den,
xue@1:             dG=(m1*tmp2-m2*tmp1)/den;
xue@1:             //calculate the negative gradient of (e3-e1) as (gF, gG)
xue@1:             gF=-(m3/tmp3-m1/tmp1)/2,
xue@1:             gG=-(k3*m3/tmp3-k1*m1/tmp1)/2;
xue@1:             if (dF*gF+dG*gG>0) //so that e3-e1 decreases in the decreasing direction of e1=e2
xue@1:             {
xue@1:               searchmode=1;
xue@1:             }
xue@1:             else
xue@1:             {
xue@1:               F1=F0, G1=G0;
xue@1:               searchmode=0; //no search
xue@1:               minimax=true; //quit loop
xue@1:             }
xue@1:           }
xue@1:         }
xue@1:       } //
xue@1:       else if (condmax==2) //e1=e2 being the maximum
xue@1:       {
xue@1:         //vmax holds the maximum
xue@1:         //l1, l2
xue@1:         searchmode=1;
xue@1:       }
xue@1:       else if (condmax==3) //e1 being the maximum
xue@1:       {
xue@1:         //the negative gradient of e1
xue@1:         dF=-0.5*m1/tmp1;
xue@1:         dG=k1*dF;
xue@1:         searchmode=3;
xue@1:       }
xue@1:     }
xue@1:     else if (condpos==2) //(F0, G0) being on side nc:(nc+1)
xue@1:     {
xue@1:       //the vector nc->(nc+1) as (x1, y1)
xue@1:       if (condmax==1) //e1=e2=e3 being the maximum
xue@1:       {
xue@1:         //This case rarely happens.
xue@1: 
xue@1:         //First see if a e1=e2 search is possible
xue@1:         //calculate the decreasing direction of e1=e3 as (dF, dG)
xue@1:         double den=m1*m3*(k1-k3)/2;
xue@1:         double dF=-(k1*m1*tmp3-k3*m3*tmp1)/den, dG=(m1*tmp3-m3*tmp1)/den;
xue@1:         //the negative gradient of e2-e1 is calculated as (gF, gG)
xue@1:         double gF=-(m2/tmp2-m1/tmp1)/2, gG=-(k2*m2/tmp2-k1*m1/tmp1)/2;
xue@1:         if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0) //so that e2-e1 decreases in the decreasing direction of e1=e3
xue@1:         {                  //~~~~~~~~~~~~~and this direction points inward
xue@1:           l2=l3;
xue@1:           searchmode=1;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           //calculate the decreasing direction of e2=e3 as (dF, dG)
xue@1:           den=m2*m3*(k2-k3)/2;
xue@1:           dF=-(k2*m2*tmp3-k3*m3*tmp2)/den,
xue@1:           dG=(m2*tmp3-m3*tmp2)/den;
xue@1:           //calculate the negative gradient of e1-e2 as (gF, gG)
xue@1:           gF=-gF, gG=-gG;
xue@1:           if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0) //so that e1-e2 decreases in the decreasing direction of e2=e3
xue@1:           {
xue@1:             l1=l3;
xue@1:             searchmode=1;
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             //calculate the decreasing direction of e1=e2 as (dF, dG)
xue@1:             den=m1*m2*(k1-k2)/2;
xue@1:             dF=-(k1*m1*tmp2-k2*m2*tmp1)/den,
xue@1:             dG=(m1*tmp2-m2*tmp1)/den;
xue@1:             //calculate the negative gradient of (e3-e1) as (gF, gG)
xue@1:             gF=-(m3/tmp3-m1/tmp1)/2,
xue@1:             gG=-(k3*m3/tmp3-k1*m1/tmp1)/2;
xue@1:             if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0) //so that e3-e1 decreases in the decreasing direction of e1=e2
xue@1:             {
xue@1:               searchmode=1;
xue@1:             }
xue@1:             else
xue@1:             {
xue@1:               //see the possibility of a e1!=e2 search
xue@1:               //calcualte the dot product of the gradients and (x1, y1)
xue@1:               double d1=m1/2/tmp1*(x1+k1*y1),
xue@1:                      d2=m2/2/tmp2*(x1+k2*y1),
xue@1:                      d3=m3/2/tmp3*(x1+k3*y1);
xue@1:               //we can prove that if there is a direction pointing inward R in which
xue@1:               //  e1, e2, e2 decrease, and another direction pointing outside R in
xue@1:               //  which e1, e2, e3 decrease, then on one direction along the side
xue@1:               //  all the three decrease. (Even more, this direction must be inside
xue@1:               //  the <180 angle formed by the two directions.)
xue@1:               //
xue@1:               //  On the contrary, if there is a direction
xue@1:               //  in which all the three decrease, with two equal, it has to point
xue@1:               //  outward for the program to get here. Then if along neither direction
xue@1:               //  of side R can the three all descend, then there doesn't exist any
xue@1:               //  direction inward R in which the three descend. In that case we
xue@1:               //  have a minimal maximum at (F0, G0).
xue@1:               if (d1*d2<=0 || d1*d3<=0 || d2*d3<=0) //so that the three don't decrease in the same direction
xue@1:               {
xue@1:                 F1=F0, G1=G0;
xue@1:                 searchmode=0; //no search
xue@1:                 minimax=true; //quit loop
xue@1:               }
xue@1:               else
xue@1:               {
xue@1:                 if (d1>0) //so that d2>0, d3>0, all three decrease in the direction -(x1, y1) towards nc
xue@1:                 {
xue@1:                   if (d1>d2 && d1>d3) //e1 decreases fastest
xue@1:                   {
xue@1:                     l1=l3; //keep e2, e3
xue@1:                   }
xue@1:                   else if (d2>=d1 && d2>d3) //e2 decreases fastest
xue@1:                   {
xue@1:                     l2=l3;  //keep e1, e3
xue@1:                   }
xue@1:                   else //d3>=d1 && d3>=d2, e3 decreases fastest
xue@1:                   {
xue@1:                     //keep e1, e2
xue@1:                   }
xue@1:                   nd=nc;
xue@1:                 }
xue@1:                 else //d1<0, d2<0, d3<0, all three decrease in the direction (x1, y1)
xue@1:                 {
xue@1:                   if (d1<d2 && d1<d3) //e1 decreases fastest
xue@1:                   {
xue@1:                     l1=l3;
xue@1:                   }
xue@1:                   else if (d2<=d1 && d2<d3) //e2 decreases fastest
xue@1:                   {
xue@1:                     l2=l3;
xue@1:                   }
xue@1:                   else //d3<=d1 && d3<=d2
xue@1:                   {
xue@1:                     //keep e1, e2
xue@1:                   }
xue@1:                   nd=n1;
xue@1:                 }
xue@1:                 searchmode=2;
xue@1:               }
xue@1:             }
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:       else if (condmax==2)  //e1=e2 being the maximum
xue@1:       {
xue@1:         //first see if the decreasing direction of e1=e2 points to the inside
xue@1:         //calculate the decreasing direction of e1=e2 as (dF, dG)
xue@1:         double den=m1*m2*(k1-k2)/2;
xue@1:         dF=-(k1*m1*tmp2-k2*m2*tmp1)/den,
xue@1:         dG=(m1*tmp2-m2*tmp1)/den;
xue@1: 
xue@1:         if (x1*dG-y1*dF<0) //so that (dF, dG) points inward R
xue@1:         {
xue@1:           searchmode=1;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           //calcualte the dot product of the gradients and (x1, y1)
xue@1:           double d1=m1/2/tmp1*(x1+k1*y1),
xue@1:                  d2=m2/2/tmp2*(x1+k2*y1);
xue@1:           if (d1*d2<=0) //so that along the side e1, e2 descend in opposite directions
xue@1:           {
xue@1:             F1=F0, G1=G0;
xue@1:             searchmode=0;
xue@1:             minimax=true;
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             if (d1>0) //so that both decrease in direction -(x1, y1)
xue@1:               nd=nc;
xue@1:             else
xue@1:               nd=n1;
xue@1:             searchmode=2;
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:       else if (condmax==3)  //e1 being the maximum
xue@1:       {
xue@1:         //calculate the negative gradient of e1 as (dF, dG)
xue@1:         dF=-0.5*m1/tmp1;
xue@1:         dG=k1*dF;
xue@1: 
xue@1:         if (x1*dG-y1*dF<0) //so the gradient points inward R
xue@1:           searchmode=3;
xue@1:         else
xue@1:         {
xue@1:           //calculate the dot product of the gradient and (x1, y1)
xue@1:           double d1=m1/2/tmp1*(x1+k1*y1);
xue@1:           if (d1>0) //so that e1 decreases in direction -(x1, y1)
xue@1:           {
xue@1:             nd=nc;
xue@1:             searchmode=4;
xue@1:           }
xue@1:           else if (d1<0)
xue@1:           {
xue@1:             nd=n1;
xue@1:             searchmode=4;
xue@1:           }
xue@1:           else //so that e1 does not change along side nc:(nc+1)
xue@1:           {
xue@1:             F1=F0, G1=G0;
xue@1:             searchmode=0;
xue@1:             minimax=true;
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     else //condpos==3, (F0, G0) being vertex nc
xue@1:     {
xue@1:       //the vector nc->(nc+1) as (x1, y1)
xue@1:       //the vector (nc-1)->nc as (x0, y0)
xue@1: 
xue@1:       if (condmax==1) //e1=e2=e3 being the maximum
xue@1:       {
xue@1:         //This case rarely happens.
xue@1: 
xue@1:         //First see if a e1=e2 search is possible
xue@1:         //calculate the decreasing direction of e1=e3 as (dF, dG)
xue@1:         double den=m1*m3*(k1-k3)/2;
xue@1:         double dF=-(k1*m1*tmp3-k3*m3*tmp1)/den, dG=(m1*tmp3-m3*tmp1)/den;
xue@1:         //the negative gradient of e2-e1 is calculated as (gF, gG)
xue@1:         double gF=-(m2/tmp2-m1/tmp1)/2, gG=-(k2*m2/tmp2-k1*m1/tmp1)/2;
xue@1:         if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0 && x0*dG-y0*dF<0) //so that e2-e1 decreases in the decreasing direction of e1=e3
xue@1:         {                  //~~~~~~~~~~~~~and this direction points inward
xue@1:           l2=l3;
xue@1:           searchmode=1;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           //calculate the decreasing direction of e2=e3 as (dF, dG)
xue@1:           den=m2*m3*(k2-k3)/2;
xue@1:           dF=-(k2*m2*tmp3-k3*m3*tmp2)/den,
xue@1:           dG=(m2*tmp3-m3*tmp2)/den;
xue@1:           //calculate the negative gradient of e1-e2 as (gF, gG)
xue@1:           gF=-gF, gG=-gG;
xue@1:           if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0 && x0*dG-y0*dF<0) //so that e1-e2 decreases in the decreasing direction of e2=e3
xue@1:           {
xue@1:             l1=l3;
xue@1:             searchmode=1;
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             //calculate the decreasing direction of e1=e2 as (dF, dG)
xue@1:             den=m1*m2*(k1-k2)/2;
xue@1:             dF=-(k1*m1*tmp2-k2*m2*tmp1)/den,
xue@1:             dG=(m1*tmp2-m2*tmp1)/den;
xue@1:             //calculate the negative gradient of (e3-e1) as (gF, gG)
xue@1:             gF=-(m3/tmp3-m1/tmp1)/2,
xue@1:             gG=-(k3*m3/tmp3-k1*m1/tmp1)/2;
xue@1:             if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0 && x0*dG-y0*dF<0) //so that e3-e1 decreases in the decreasing direction of e1=e2
xue@1:             {
xue@1:               searchmode=1;
xue@1:             }
xue@1:             else
xue@1:             {
xue@1:               //see the possibility of a e1!=e2 search
xue@1:               //calcualte the dot product of the gradients and (x1, y1)
xue@1:               double d1=m1/2/tmp1*(x1+k1*y1),
xue@1:                      d2=m2/2/tmp2*(x1+k2*y1),
xue@1:                      d3=m3/2/tmp3*(x1+k3*y1);
xue@1:               //we can also prove that if there is a direction pointing inward R in which
xue@1:               //  e1, e2, e2 decrease, and another direction pointing outside R in
xue@1:               //  which e1, e2, e3 decrease, all the three decrease either on nc->(nc+1)
xue@1:               //  direction along side nc:(nc+1), or on nc->(nc-1) direction along side
xue@1:               //  nc:(nc-1).
xue@1:               //
xue@1:               //  On the contrary, if there is a direction
xue@1:               //  in which all the three decrease, with two equal, it has to point
xue@1:               //  outward for the program to get here. Then if along neither direction
xue@1:               //  of side R can the three all descend, then there doesn't exist any
xue@1:               //  direction inward R in which the three descend. In that case we
xue@1:               //  have a minimal maximum at (F0, G0).
xue@1:               if (d1<0 && d2<0 && d3<0) //so that all the three decrease in the direction (x1, y1)
xue@1:               {
xue@1:                 if (d1<d2 && d1<d3) //e1 decreases fastest
xue@1:                 {
xue@1:                   l1=l3;
xue@1:                 }
xue@1:                 else if (d2<=d1 && d2<d3) //e2 decreases fastest
xue@1:                 {
xue@1:                   l2=l3;
xue@1:                 }
xue@1:                 else //d3<=d1 && d3<=d2
xue@1:                 {
xue@1:                   //keep e1, e2
xue@1:                 }
xue@1:                 nd=n1;
xue@1:                 searchmode=2;
xue@1:               }
xue@1:               else
xue@1:               {
xue@1:                 d1=m1/2/tmp1*(x0+k1*y0),
xue@1:                 d2=m2/2/tmp2*(x0+k2*y0),
xue@1:                 d3=m3/2/tmp3*(x0+k3*y0);
xue@1:                 if (d1>0 && d2>0 && d3>0) //so that all the three decrease in the direction -(x0, y0)
xue@1:                 {
xue@1:                   if (d1>d2 && d1>d3) //e1 decreases fastest
xue@1:                   {
xue@1:                     l1=l3; //keep e2, e3
xue@1:                   }
xue@1:                   else if (d2>=d1 && d2>d3) //e2 decreases fastest
xue@1:                   {
xue@1:                     l2=l3;  //keep e1, e3
xue@1:                   }
xue@1:                   else //d3>=d1 && d3>=d2, e3 decreases fastest
xue@1:                   {
xue@1:                     //keep e1, e2
xue@1:                   }
xue@1:                   nd=n0;
xue@1:                   searchmode=2;
xue@1:                 }
xue@1:                 else
xue@1:                 {
xue@1:                   F1=F0, G1=G0;
xue@1:                   searchmode=0;
xue@1:                   minimax=true;
xue@1:                 }
xue@1:               }
xue@1:             }
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:       else if (condmax==2) //e1=e2 being the maximum
xue@1:       {
xue@1:         //first see if the decreasing direction of e1=e2 points to the inside
xue@1:         //calculate the decreasing direction of e1=e2 as (dF, dG)
xue@1:         double den=m1*m2*(k1-k2)/2;
xue@1:         dF=-(k1*m1*tmp2-k2*m2*tmp1)/den,
xue@1:         dG=(m1*tmp2-m2*tmp1)/den;
xue@1: 
xue@1:         if (x1*dG-y1*dF<0 && x0*dG-y1*dF<0) //so that (dF, dG) points inward R
xue@1:         {
xue@1:           searchmode=1;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           //calcualte the dot product of the gradients and (x1, y1)
xue@1:           double d1=m1/2/tmp1*(x1+k1*y1),
xue@1:                  d2=m2/2/tmp2*(x1+k2*y1);
xue@1:           if (d1<0 && d2<0) //so that along side nc:(nc+1) e1, e2 descend in direction (x1, y1)
xue@1:           {
xue@1:             nd=n1;
xue@1:             searchmode=2;
xue@1:           }
xue@1:           else
xue@1:           {
xue@1:             d1=m1/2/tmp1*(x0+k1*y0),
xue@1:             d2=m2/2/tmp2*(x0+k2*y0);
xue@1:             if (d1>0 && d2>0) //so that slong the side (nc-1):nc e1, e2 decend in direction -(x0, y0)
xue@1:             {
xue@1:               nd=n0;
xue@1:               searchmode=2;
xue@1:             }
xue@1:             else
xue@1:             {
xue@1:               F1=F0, G1=G0;
xue@1:               searchmode=0;
xue@1:               minimax=true;
xue@1:             }
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:       else //condmax==3, e1 being the solo maximum
xue@1:       {
xue@1:         //calculate the negative gradient of e1 as (dF, dG)
xue@1:         dF=-0.5*m1/tmp1;
xue@1:         dG=k1*dF;
xue@1: 
xue@1:         if (x1*dG-y1*dF<0 && x0*dG-y0*dF<0) //so the gradient points inward R
xue@1:           searchmode=3;
xue@1:         else
xue@1:         {
xue@1:           //calculate the dot product of the gradient and (x1, y1)
xue@1:           double d1=m1/2/tmp1*(x1+k1*y1),
xue@1:                  d0=-m1/2/tmp1*(x0+k1*y0);
xue@1:           if (d1<d0) //so that e1 decreases in direction (x1, y1) faster than in -(x0, y0)
xue@1:           {
xue@1:             if (d1<0)
xue@1:             {
xue@1:               nd=n1;
xue@1:               searchmode=4;
xue@1:             }
xue@1:             else
xue@1:             {
xue@1:               F1=F0, G1=G0;
xue@1:               searchmode=0;
xue@1:               minimax=true;
xue@1:             }
xue@1:           }
xue@1:           else //so that e1 decreases in direction -(x0, y0) faster than in (x1, y1)
xue@1:           {
xue@1:             if (d0<0)
xue@1:             {
xue@1:               nd=n0;
xue@1:               searchmode=4;
xue@1:             }
xue@1:             else
xue@1:             {
xue@1:               F1=F0, G1=G0;
xue@1:               searchmode=0;
xue@1:               minimax=true;
xue@1:             }
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     //  the conditioning stage ends here
xue@1:     //  the searching step starts here
xue@1:     if (searchmode==0)
xue@1:     {}
xue@1:     else if (searchmode==1)
xue@1:     {
xue@1:       //Search mode 1: e1=e2 search
xue@1:       //  starting with e1=e2, search down the decreasing direction of
xue@1:       //  e1=e2 until at point (F1, G1) there is another l3 so that e1(F1, G1)
xue@1:       //  =e2(F1, G1)=e3(F1, G1), or until at point (F1, G1) the search is about
xue@1:       //  to go out of R.
xue@1:       //Arguments: l1, l2, (F0, G0, vmax)
xue@1:       double tmp=F0+k[l1]*G0; testnn(tmp);
xue@1:       double e1=m[l1]*sqrt(tmp)-f[l1];
xue@1:       tmp=F0+k[l2]*G0; testnn(tmp);
xue@1:       double e2=m[l2]*sqrt(tmp)-f[l2];
xue@1:       if (fabs(e1-e2)>1e-4)
xue@1:       {
xue@1: //        int kk=1;
xue@1:         goto start;
xue@1:       }
xue@1: 
xue@1:       //find intersections of e1=e2 with other ek's
xue@1:       l3=-1;
xue@1:     loopl3:
xue@1:       double e=-1;
xue@1:       for (int l=0; l<L; l++)
xue@1:       {
xue@1:         if (l==l1 || l==l2) continue;
xue@1:         double lF1[2], lG1[2], lE[2];
xue@1:         double lm[3]={m[l1], m[l2], m[l]};
xue@1:         double lf[3]={f[l1], f[l2], f[l]};
xue@1:         int lk[3]={k[l1], k[l2], k[l]};
xue@1:         int r=FGFrom3(lF1, lG1, lE, lm, lf, lk);
xue@1:         for (int i=0; i<r; i++)
xue@1:         {
xue@1:           if (lE[i]>e && lE[i]<vmax) l3=l, e=lE[i], F1=lF1[i], G1=lG1[i];
xue@1:         }
xue@1:       }
xue@1:       //l3 shall be >=0 at this point
xue@1:       //find intersections of e1=e2 with polygon sides
xue@1:       if (l3<0)
xue@1:       {
xue@1:         ///int kkk=1;
xue@1:         goto loopl3;
xue@1:       }
xue@1:       nc=-1;
xue@1:       double F2, G2;
xue@1:       for (int n=0; n<N; n++)
xue@1:       {
xue@1:         int nn=(n==N-1)?0:(n+1);
xue@1:         double xn1=F[nn]-F[n], yn1=G[nn]-G[n], xn2=F1-F[n], yn2=G1-G[n];
xue@1:         if (xn1*yn2-xn2*yn1>=0) //so that (F1, G1) is on or out of side n:(n+1)
xue@1:         {
xue@1:           nc=n;
xue@1:           break;
xue@1:         }
xue@1:       }
xue@1: 
xue@1:       if (nc<0) //(F1, G1) being inside R
xue@1:       {
xue@1:         vmax=e;
xue@1:         condpos=1;
xue@1:         condmax=1;
xue@1:         //l3 shall be >=0 at this point, so l1, l2, l3 are all ok
xue@1:       }
xue@1:       else //(F1, G1) being outside nc:(nc+1) //(F2, G2) being on side nc:(nc+1)
xue@1:       {
xue@1:         //find where e1=e2 crosses a side of R between (F0, G0) and (F1, G1)
xue@1:         double lF2[2], lG2[2], lmd[2];
xue@1:         double lm[2]={m[l1], m[l2]};
xue@1:         double lf[2]={f[l1], f[l2]};
xue@1:         int lk[2]={k[l1], k[l2]};
xue@1: 
xue@1:         int ncc=-1;
xue@1:         while (ncc<0)
xue@1:         {
xue@1:           n1=(nc==N-1)?0:(nc+1);
xue@1:           double xn1=F[n1]-F[nc], yn1=G[n1]-G[nc];
xue@1: 
xue@1:           int r=FGFrom2(lF2, lG2, lmd, F[nc], xn1, G[nc], yn1, lm, lf, lk);
xue@1: 
xue@1:           bool once=false;
xue@1:           for (int i=0; i<r; i++)
xue@1:           {
xue@1:             double tmp=lF2[i]+k[l1]*lG2[i]; testnn(tmp);
xue@1:             double le=m[l1]*sqrt(tmp)-f[l1];
xue@1:             if (le<=vmax) //this shall be satisfied once
xue@1:             {
xue@1:               once=true;
xue@1:               if (lmd[i]>=0 && lmd[i]<=1) //so that curve e1=e2 does cross the boundry within it
xue@1:                 ncc=nc, F2=lF2[i], G2=lG2[i], e=le;
xue@1:               else if (lmd[i]>1) //so that the crossing point is out of vertex n1
xue@1:                 nc=n1;
xue@1:               else //lmd[i]<0, so that the crossing point is out of vertex nc-1
xue@1:                 nc=(nc==0)?(N-1):(nc-1);
xue@1:             }
xue@1:           }
xue@1:           if (!once)
xue@1:           {
xue@1:             //int kk=0;
xue@1:             goto start;
xue@1:           }
xue@1:         }
xue@1:         nc=ncc;
xue@1: 
xue@1:         vmax=e;
xue@1:         condpos=2;
xue@1:         if (F1==F2 && G1==G2)
xue@1:           condmax=1;
xue@1:         else
xue@1:           condmax=2;
xue@1:         F1=F2, G1=G2;
xue@1:       }
xue@1:     }
xue@1:     else if (searchmode==2)
xue@1:     {
xue@1:       //  Search mode 2 (e1!=e2 search): starting with e1=e2, and a linear equation
xue@1:       //  constraint, search down the decreasing direction until at point (F1, G1)
xue@1:       //  there is a l3 so that e3(F1, G1)=max(e1, e2)(F1, G1), or at point (F1, G1)
xue@1:       //  the search is about to go out of R.
xue@1:       //  Arguments: l1, l2, dF, dG, (F0, G0, vmax)
xue@1: 
xue@1:       //The searching direction (dF, dG) is always along some polygon side
xue@1:       //  If conpos==2, it is along nc:(nc+1)
xue@1:       //  If conpos==3, it is along nc:(nc+1) or (nc-1):nc
xue@1: 
xue@1:       //
xue@1: 
xue@1:       //first check whether e1>e2 or e2>e1 down (dF, dG)
xue@1:       dF=F[nd]-F0, dG=G[nd]-G0;
xue@1:       //
xue@1:       //the negative gradient of e2-e1 is calculated as (gF, gG)
xue@1:       double gF=-(m2/tmp2-m1/tmp1)/2,
xue@1:              gG=-(k2*m2/tmp2-k1*m1/tmp1)/2;
xue@1:       if (gF*dF+gG*dG>0) //e2-e1 decrease down direction (dF, dG), so that e1>e2
xue@1:       {}
xue@1:       else //e1<e2 down (dF, dG)
xue@1:       {
xue@1:         int ll=l2; l2=l1; l1=ll;
xue@1:         m1=m[l1], m2=m[l2], k1=k[l1], k2=k[l2];
xue@1:         tmp=F0+k1*G0; testnn(tmp); tmp1=sqrt(tmp);
xue@1:         tmp=F0+k2*G0; testnn(tmp); tmp2=sqrt(tmp);
xue@1:       }
xue@1:       //now e1>e2 down (dF, dG) from (F0, G0)
xue@1:       tmp=F[nd]+k1*G[nd]; testnn(tmp);
xue@1:       double e1=m1*sqrt(tmp)-f[l1];
xue@1:       tmp=F[nd]+k2*G[nd]; testnn(tmp);
xue@1:       double e2=m2*sqrt(tmp)-f[l2], FEx, GEx, eEx;
xue@1:       int maxlmd=1;
xue@1:       if (e1>=e2)
xue@1:       {
xue@1:         // then e1 is larger than e2 for the whole searching interval
xue@1:         FEx=F[nd], GEx=G[nd];
xue@1:         eEx=e1;
xue@1:       }
xue@1:       else // the they swap somewhere in between, now find it and make it maxlmd
xue@1:       {
xue@1:         double lF1[2], lG1[2], lmd[2];
xue@1:         double lm[2]={m[l1], m[l2]};
xue@1:         double lf[2]={f[l1], f[l2]};
xue@1:         int lk[2]={k[l1], k[l2]};
xue@1:         int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
xue@1:         //process the results
xue@1:         if (r==2) //must be so
xue@1:         {
xue@1:           if (lmd[0]<lmd[1]) lmd[0]=lmd[1], FEx=lF1[1], GEx=lG1[1];
xue@1:           else FEx=lF1[0], GEx=lG1[0];
xue@1:         }
xue@1:         if (lmd[0]>0 && lmd[0]<1) //must be so
xue@1:           maxlmd=lmd[0];
xue@1:         tmp=FEx+k1*GEx; testnn(tmp);
xue@1:         eEx=m1*sqrt(tmp)-f[l1];
xue@1:       }
xue@1: 
xue@1:       l3=-1;
xue@1: //      int l4;
xue@1:       double e, llmd=maxlmd;
xue@1:       int *tpl=new int[L], ctpl=0;
xue@1:       for (int l=0; l<L; l++)
xue@1:       {
xue@1:         if (l==l1 || l==l2) continue;
xue@1:         tmp=FEx+k[l]*GEx; testnn(tmp);
xue@1:         double le=m[l]*sqrt(tmp)-f[l];
xue@1:         if (le<eEx)
xue@1:         {
xue@1:           tpl[ctpl]=l;
xue@1:           ctpl++;
xue@1:           continue;
xue@1:         }
xue@1:         //solve for the equation system e1(F1, G1)=el(F1, G1), with (F1, G1) on nc:n1
xue@1:         double lF1[2], lG1[2], lmd[2];
xue@1:         double lm[2]={m[l1], m[l]};
xue@1:         double lf[2]={f[l1], f[l]};
xue@1:         int lk[2]={k[l1], k[l]};
xue@1:         int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
xue@1:         //process the results
xue@1:         for (int i=0; i<r; i++)
xue@1:         {
xue@1:           if (lmd[i]<0 || lmd[i]>maxlmd) continue; //the solution is in the wrong direction
xue@1:           if (lmd[i]<llmd)
xue@1:             l3=l, F1=lF1[i], G1=lG1[i], llmd=lmd[i];
xue@1:         }
xue@1:       }
xue@1:       if (ctpl==L-2) //so that at (FEx, GEx) e1 is maximal, equivalent to "l3<0"
xue@1:       {}
xue@1:       else
xue@1:       {
xue@1:         //at this point F1 and G1 must have valid values already
xue@1:         tmp=F1+k[l1]*G1; testnn(tmp);
xue@1:         e=m[l1]*sqrt(tmp)-f[l1];
xue@1:         //test if e1 is maximal at (F1, G1)
xue@1:         bool lmax=true;
xue@1:         for (int tp=0; tp<ctpl; tp++)
xue@1:         {
xue@1:           tmp=F1+k[tpl[tp]]*G1; testnn(tmp);
xue@1:           double le=m[tpl[tp]]*sqrt(tmp)-f[tpl[tp]];
xue@1:           if (le>e) {lmax=false; break;}
xue@1:         }
xue@1:         if (!lmax)
xue@1:         {
xue@1:           for (int tp=0; tp<ctpl; tp++)
xue@1:           {
xue@1:             double lF1[2], lG1[2], lmd[2];
xue@1:             double lm[2]={m[l1], m[tpl[tp]]};
xue@1:             double lf[2]={f[l1], f[tpl[tp]]};
xue@1:             int lk[2]={k[l1], k[tpl[tp]]};
xue@1:             int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
xue@1:             //process the results
xue@1:             for (int i=0; i<r; i++)
xue@1:             {
xue@1:               if (lmd[i]<0 || lmd[i]>maxlmd) continue; //the solution is in the wrong direction
xue@1:               if (lmd[i]<=llmd)
xue@1:                 l3=tpl[tp], F1=lF1[i], G1=lG1[i], llmd=lmd[i];
xue@1:             }
xue@1:           }
xue@1:           tmp=F1+k[l1]*G1; testnn(tmp);
xue@1:           e=m[l1]*sqrt(tmp)-f[l1];
xue@1:         }
xue@1:       }
xue@1:       delete[] tpl;
xue@1:       if (l3>=0) //the solution is found in the right direction
xue@1:       {
xue@1:         vmax=e;
xue@1:         if (llmd==1) //(F1, G1) is a vertex of R
xue@1:         {
xue@1:           nc=nd;
xue@1:           condpos=3;
xue@1:           condmax=2;
xue@1:           l2=l3;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           if (condpos==3 && nd==n0) nc=n0;
xue@1:           condpos=2;
xue@1:           condmax=2;
xue@1:           l2=l3;
xue@1:         }
xue@1:       }
xue@1:       else if (maxlmd<1) //so that e1=e2 remains maximal at maxlmd
xue@1:       {
xue@1:         if (condpos==3 && nd==n0) nc=n0;
xue@1:         condpos=2;
xue@1:         condmax=2;
xue@1:         //l1, l2 remain unchanged
xue@1:         F1=FEx, G1=GEx;
xue@1:         vmax=eEx;
xue@1:       }
xue@1:       else //so that e1 remains maximal at vertex nd
xue@1:       {
xue@1:         condpos=3;
xue@1:         condmax=3;
xue@1:         nc=nd;
xue@1:         F1=FEx, G1=GEx; //this shall equal to F0+dF, G0+dG
xue@1:         vmax=eEx;
xue@1:       }
xue@1:     }
xue@1:     else if (searchmode==3)
xue@1:     {
xue@1:       //Search mode 3 (e1 search): starting with e1 being the only maximum, search down
xue@1:       //  the decreasing direction of e1 until at point (F1, G1) there is another l2 so
xue@1:       //  that e1(F1, G1)=e2(F1, G1), or until at point (F1, G1) the search is about
xue@1:       //  to go out of R.
xue@1:       //
xue@1:       //  Arguments: l1, dF, dG, (F0, G0, vmax)
xue@1:       //
xue@1: 
xue@1:       //first calculate the value of lmd from (F0, G0) to get out of R
xue@1:       double maxlmd=-1, FEx, GEx;
xue@1:       double fdggdf=-F0*dG+G0*dF;
xue@1:       nd=-1;
xue@1:       for (int n=0; n<N; n++)
xue@1:       {
xue@1:         if (condpos==2 && n==nc) continue;
xue@1:         if ((condpos==3 && n==nc) || n==n0) continue;
xue@1:         int nn=(n==N-1)?0:n+1;
xue@1:         double xn1=F[n], xn2=F[nn], yn1=G[n], yn2=G[nn];
xue@1:         double test1=xn1*dG-yn1*dF+fdggdf,
xue@1:                test2=xn2*dG-yn2*dF+fdggdf;
xue@1:         if (test1*test2<=0)
xue@1:         {
xue@1:           //the two following are equivalent. we use the larger denominator.
xue@1:           FEx=(xn1*test2-xn2*test1)/(test2-test1);
xue@1:           GEx=(yn1*test2-yn2*test1)/(test2-test1);
xue@1:           if (fabs(dF)>fabs(dG)) maxlmd=(FEx-F0)/dF;
xue@1:           else maxlmd=(GEx-G0)/dG;
xue@1:           nd=n;
xue@1:         }
xue@1:       }
xue@1: 
xue@1:       //maxlmd must be >0 at this point.
xue@1:       l2=-1;
xue@1:       tmp=FEx+k[l1]*GEx; testnn(tmp);
xue@1:       double e, eEx=m[l1]*sqrt(tmp)-f[l1], llmd=maxlmd;
xue@1:       int *tpl=new int[L], ctpl=0;
xue@1:       for (int l=0; l<L; l++)
xue@1:       {
xue@1:         if (l==l1) continue;
xue@1: 
xue@1:         //if el does not catch up with e1 at (FEx, GEx), then there is no hope this
xue@1:         //  happens in between (F0, G0) and (FEx, GEx)
xue@1:         tmp=FEx+k[l]*GEx; testnn(tmp);
xue@1:         double le=m[l]*sqrt(tmp)-f[l];
xue@1:         if (le<eEx)
xue@1:         {
xue@1:           tpl[ctpl]=l;
xue@1:           ctpl++;
xue@1:           continue;
xue@1:         }
xue@1:         //now the existence of (F1, G1) is guaranteed
xue@1:         double lF1[2], lG1[2], lmd[2];
xue@1:         double lm[2]={m[l1], m[l]};
xue@1:         double lf[2]={f[l1], f[l]};
xue@1:         int lk[2]={k[l1], k[l]};
xue@1:         int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
xue@1:         for (int i=0; i<r; i++)
xue@1:         {
xue@1:           if (lmd[i]<0 || lmd[i]>maxlmd) continue;
xue@1:           if (lmd[i]<=llmd) llmd=lmd[i], l2=l, F1=lF1[i], G1=lG1[i];
xue@1:         }
xue@1:       }
xue@1:       if (ctpl==L-1) //e1 is maximal at eEx, equivalent to "l2<0"
xue@1:       {}//l2 must equal -1 at this point
xue@1:       else
xue@1:       {
xue@1:         tmp=F1+k[l1]*G1; testnn(tmp);
xue@1:         e=m[l1]*sqrt(tmp)-f[l1];
xue@1:         //test if e1 is maximal at (F1, G1)
xue@1:         //e1 is already maximal of those l's not in tpl[ctpl]
xue@1:         bool lmax=true;
xue@1:         for (int tp=0; tp<ctpl; tp++)
xue@1:         {
xue@1:           tmp=F1+k[tpl[tp]]*G1; testnn(tmp);
xue@1:           double le=m[tpl[tp]]*sqrt(tmp)-f[tpl[tp]];
xue@1:           if (le>e) {lmax=false; break;}
xue@1:         }
xue@1:         if (!lmax)
xue@1:         {
xue@1:           for (int tp=0; tp<ctpl; tp++)
xue@1:           {
xue@1:             double lF1[2], lG1[2], lmd[2];
xue@1:             double lm[2]={m[l1], m[tpl[tp]]};
xue@1:             double lf[2]={f[l1], f[tpl[tp]]};
xue@1:             int lk[2]={k[l1], k[tpl[tp]]};
xue@1:             int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
xue@1:             for (int i=0; i<r; i++)
xue@1:             {
xue@1:               if (lmd[i]<0 || lmd[i]>maxlmd) continue;
xue@1:               if (llmd>=lmd[i]) llmd=lmd[i], l2=tpl[tp], F1=lF1[i], G1=lG1[i];
xue@1:             }
xue@1:           }
xue@1:           tmp=F1+k[l1]*G1; testnn(tmp);
xue@1:           e=m[l1]*sqrt(tmp)-f[l1];
xue@1:         }
xue@1:       }
xue@1:       delete[] tpl;
xue@1: 
xue@1:       if (l2>=0) //so that e1 meets some other ek during the search
xue@1:       {
xue@1:         vmax=e; //this has to equal m[l2]*sqrt(F1+k[l2]*G1)-f[l2]
xue@1:         if (vmax==eEx)
xue@1:         {
xue@1:           condpos=2;
xue@1:           condmax=2;
xue@1:           nc=nd;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           condpos=1;
xue@1:           condmax=2;
xue@1:         }
xue@1:       }
xue@1:       else //l2==-1
xue@1:       {
xue@1:         vmax=eEx;
xue@1:         F1=FEx, G1=GEx;
xue@1:         condpos=2;
xue@1:         condmax=3;
xue@1:         nc=nd;
xue@1:       }
xue@1:     }
xue@1:     else //searchmode==4
xue@1:     {
xue@1:       //Search mode 4: a special case of search mode 3 in which the boundry constraint
xue@1:       //  is simply 0<=lmd<=1.
xue@1:       dF=F[nd]-F0, dG=G[nd]-G0;
xue@1:       l2=-1;
xue@1:       int *tpl=new int[L], ctpl=0;
xue@1:       tmp=F[nd]+k[l1]*G[nd]; testnn(tmp);
xue@1:       double e, eEx=m[l1]*sqrt(tmp)-f[l1], llmd=1;
xue@1:       for (int l=0; l<L; l++)
xue@1:       {
xue@1:         if (l==l1) continue;
xue@1:         //if el does not catch up with e1 at (F[nd], G[nd]), then there is no hope this
xue@1:         //  happens in between (F0, G0) and vertex nd.
xue@1:         tmp=F[nd]+k[l]*G[nd]; testnn(tmp);
xue@1:         double le=m[l]*sqrt(tmp)-f[l];
xue@1:         if (le<eEx)
xue@1:         {
xue@1:           tpl[ctpl]=l;
xue@1:           ctpl++;
xue@1:           continue;
xue@1:         }
xue@1:         //now the existence of (F1, G1) is guaranteed
xue@1:         double lF1[2], lG1[2], lmd[2];
xue@1:         double lm[2]={m[l1], m[l]};
xue@1:         double lf[2]={f[l1], f[l]};
xue@1:         int lk[2]={k[l1], k[l]};
xue@1:         loop1:
xue@1:         int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
xue@1:         for (int i=0; i<r; i++)
xue@1:         {
xue@1:           if (lmd[i]<0 || lmd[i]>1) continue;
xue@1:           if (llmd>=lmd[i])
xue@1:           {
xue@1:             tmp=lF1[i]+k[l1]*lG1[i]; testnn(tmp);
xue@1:             double e1=m[l1]*sqrt(tmp)-f[l1];
xue@1:             tmp=lF1[i]+k[l]*lG1[i]; testnn(tmp);
xue@1:             double e2=m[l]*sqrt(tmp)-f[l];
xue@1:             if (fabs(e1-e2)>1e-4)
xue@1:             {
xue@1:               //int kk=0;
xue@1:               goto loop1;
xue@1:             }
xue@1:             llmd=lmd[i], l2=l, F1=lF1[i], G1=lG1[i];
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:       if (ctpl==N-1) //so e1 is maximal at (F[nd], G[nd])
xue@1:       {}
xue@1:       else
xue@1:       {
xue@1:         tmp=F1+k[l1]*G1; testnn(tmp);
xue@1:         e=m[l1]*sqrt(tmp)-f[l1];
xue@1:         //test if e1 is maximal at (F1, G1)
xue@1:         bool lmax=true;
xue@1:         for (int tp=0; tp<ctpl; tp++)
xue@1:         {
xue@1:           tmp=F1+k[tpl[tp]]*G1; testnn(tmp);
xue@1:           double le=m[tpl[tp]]*sqrt(tmp)-f[tpl[tp]];
xue@1:           if (le>e) {lmax=false; break;}
xue@1:         }
xue@1:         if (!lmax)
xue@1:         {
xue@1:           for (int tp=0; tp<ctpl; tp++)
xue@1:           {
xue@1:             double lF1[2], lG1[2], lmd[2];
xue@1:             double lm[2]={m[l1], m[tpl[tp]]};
xue@1:             double lf[2]={f[l1], f[tpl[tp]]};
xue@1:             int lk[2]={k[l1], k[tpl[tp]]};
xue@1:             int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
xue@1:             for (int i=0; i<r; i++)
xue@1:             {
xue@1:               if (lmd[i]<0 || lmd[i]>1) continue;
xue@1:               if (llmd>=lmd[i]) llmd=lmd[i], l2=tpl[tp], F1=lF1[i], G1=lG1[i];
xue@1:             }
xue@1:           }
xue@1: //          e=m[l1]*sqrt(F1+k[l1]*G1)-f[l1];
xue@1:         }
xue@1:       }
xue@1:       tmp=F1+k[l1]*G1; testnn(tmp);
xue@1:       e=m[l1]*sqrt(tmp)-f[l1];
xue@1:       delete[] tpl;
xue@1:       if (l2>=0)
xue@1:       {
xue@1:         vmax=e;
xue@1:         if (vmax==eEx)
xue@1:         {
xue@1:           condpos=3;
xue@1:           condmax=2;
xue@1:           nc=nd;
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           condpos=2;
xue@1:           condmax=2;
xue@1:           //(F1, G1) lies on side n0:nc (nd=n0) or nc:n1 (nd=nc or nd=n1)
xue@1:           if (nd==n0) nc=n0;
xue@1:           //else nc=nc;
xue@1:         }
xue@1:       }
xue@1:       else //l2==-1,
xue@1:       {
xue@1:         vmax=eEx;
xue@1:         condpos=3;
xue@1:         condmax=3;
xue@1:         F1=F[nd], G1=G[nd];
xue@1:         nc=nd;  //
xue@1:       }
xue@1:     }
xue@1:     F0=F1, G0=G1;
xue@1: 
xue@1:     if (condmax==1 && ((l1^l2)==1 || (l1^l3)==1 || (l2^l3)==1))
xue@1:       minimax=true;
xue@1:     else if (condmax==2 && ((l1^l2)==1))
xue@1:       minimax=true;
xue@1:     else if (vmax<1e-6)
xue@1:       minimax=true;
xue@1:   }
xue@1: 
xue@1:   free(m);//delete[] m;
xue@1:   delete[] vmnl;
xue@1: 
xue@1:   return vmax;
xue@1: }//minimaxstiff*/
xue@1: 
Chris@5: /**
xue@1:   function NMResultToAtoms: converts the note-match result hosted in a NMResults structure, i.e.
xue@1:   parameters of a harmonic atom, to a list of atoms. The process retrieves atom parameters from low
xue@1:   partials until a required number of atoms have been retrieved or a non-positive frequency is
xue@1:   encountered (non-positive frequencies are returned by note-match to indicate high partials for which
xue@1:   no informative estimates can be obtained).
xue@1: 
xue@1:   In: results: the NMResults structure hosting the harmonic atom
xue@1:       M: number of atoms to request from &results
xue@1:       t: time of this harmonic atom
xue@1:       wid: scale of this harmonic atom with which the note-match was performed
xue@1:   Out: HP[return value]: list of atoms retrieved from $result.
xue@1: 
xue@1:   Returns the number of atoms retrieved.
xue@1: */
xue@1: int NMResultToAtoms(int M, atom* HP, int t, int wid, NMResults results)
xue@1: {
xue@1:   double *fpp=results.fp, *vfpp=results.vfp, *pfpp=results.pfp;
xue@1:   atomtype* ptype=results.ptype;
xue@1:   for (int i=0; i<M; i++)
xue@1:   {
xue@1:     if (fpp[i]<=0) {memset(&HP[i], 0, sizeof(atom)*(M-i)); return i;}
xue@1:     HP[i].t=t, HP[i].s=wid, HP[i].f=fpp[i]/wid, HP[i].a=vfpp[i];
xue@1:     HP[i].p=pfpp[i]; HP[i].type=ptype[i]; HP[i].pin=i+1;
xue@1:   }
xue@1:   return M;
xue@1: }//NMResultToAtoms
xue@1: 
Chris@5: /**
xue@1:   function NMResultToPartials: reads atoms of a harmonic atom in a NMResult structure into HS partials.
xue@1: 
xue@1:   In: results: the NMResults structure hosting the harmonic atom
xue@1:       M: number of atoms to request from &results
xue@1:       fr: frame index of this harmonic atom
xue@1:       t: time of this harmonic atom
xue@1:       wid: scale of this harmonic atom with which the note-match was performed
xue@1:   Out: Partials[M][]: HS partials whose fr-th frame is updated to the harmonic partial read from $results
xue@1: 
xue@1:   Returns the number of partials retrieved.
xue@1: */
xue@1: int NMResultToPartials(int M, int fr, atom** Partials, int t, int wid, NMResults results)
xue@1: {
xue@1:   double *fpp=results.fp, *vfpp=results.vfp, *pfpp=results.pfp;
xue@1:   atomtype* ptype=results.ptype;
xue@1:   for (int i=0; i<M; i++)
xue@1:   {
xue@1:     atom* part=&Partials[i][fr];
xue@1:     if (fpp[i]<=0) {for (int j=i; j<M; j++) memset(&Partials[j][fr], 0, sizeof(atom)); return i;}
xue@1:     part->t=t, part->s=wid, part->f=fpp[i]/wid, part->a=vfpp[i];
xue@1:     part->p=pfpp[i]; part->type=ptype[i]; part->pin=i+1;
xue@1:   }
xue@1:   return M;
xue@1: }//NMResultToPartials
xue@1: 
Chris@5: /**
xue@1:   function NoteMatchStiff3: finds harmonic atom from spectrum if Fr=1, or constant-pitch harmonic
xue@1:   sinusoid from spectrogram if Fr>1.
xue@1: 
xue@1:   In: x[Fr][N/2+1]: spectrogram
xue@1:       fps[pc], vps[pc]: primitive (rough) peak frequencies and amplitudes
xue@1:       N, offst: atom scale and hop size
xue@1:       R: initial F-G polygon constraint, optional
xue@1:       settings: note match settings
xue@1:       computes: pointer to a function that computes HA score, must be ds0 or ds1
xue@1:       lastvfp[lastp]: amplitude of the previous harmonic atom
xue@1:       forceinputlocalfr: specifies if partial settings->pin0 is taken for granted ("pinned")
xue@1:   Out: results: note match results
xue@1:        f0, B: fundamental frequency and stiffness coefficient
xue@1:        R: F-G polygon of harmonic atom
xue@1: 
xue@1:   Returns the total energy of HA or constant-pitch HS.
xue@1: */
xue@1: double NoteMatchStiff3(TPolygon* R, double &f0, double& B, int pc, double* fps, double* vps,
xue@1:   int Fr, cdouble** x, int N, int offst, NMSettings* settings, NMResults* results, int lastp,
xue@1:   double* lastvfp, double (*computes)(double a, void* params), int forceinputlocalfr)
xue@1: {
xue@1:   double result=0;
xue@1: 
xue@1:   double maxB=settings->maxB, minf0=settings->minf0, maxf0=settings->maxf0,
xue@1:          delm=settings->delm, delp=settings->delp, *c=settings->c, iH2=settings->iH2,
xue@1:          *pinf=settings->pinf, hB=settings->hB, epf0=settings->epf0;// epf=settings->epf,
xue@1:   int maxp=settings->maxp, *pin=settings->pin, pin0=settings->pin0, *pinfr=settings->pinfr,
xue@1:       pcount=settings->pcount, M=settings->M;
xue@1: 
xue@1: //	int *ptype=results->ptype;
xue@1: 
xue@1:   //calculate the energy of the last frame (hp)
xue@1:   double lastene=0; for (int i=0; i<lastp; i++) lastene+=lastvfp[i]*lastvfp[i];
xue@1:   //fill frequency and amplitude buffers with 0
xue@1:   //now determine numsam (the maximal number of partials)
xue@1:   bool inputpartial=(f0>0 && settings->pin0>0);
xue@1:   if (!inputpartial)
xue@1:   {
xue@1:     if (pcount>0) f0=pinf[0], pin0=pin[0];
xue@1:     else f0=sqrt(R->X[0]), pin0=1;
xue@1:   }
xue@1:   int numsam=N*pin0/2.1/f0;
xue@1:   if (numsam>maxp) numsam=maxp;
xue@1:   //allocate buffer for candidate atoms
xue@1:   TTempAtom*** Allocate2(TTempAtom*, numsam+1, MAX_CAND, cands);
xue@1:   //pcs[p] records the number of candidates for partial index p
xue@1:   //psb[p] = 1: anchor, 2: user input, 0: normal
xue@1:   int *psb=new int[(numsam+1)*2], *pcs=&psb[numsam+1];
xue@1:   memset(psb, 0, sizeof(int)*(numsam+1)*2);
xue@1:   //if R is not specified, initialize a trivial candidate with maxB
xue@1:   if (R->N==0) cands[0][0]=new TTempAtom(1, (minf0+maxf0)*0.5, (maxf0-minf0)*0.5, maxB);
xue@1:   //if R is, initialize a trivial candidate by extending R by delp
xue@1:   else cands[0][0]=new TTempAtom(R, delp, delp, minf0);
xue@1: 
xue@1:   int pm=0, pM=0;
xue@1:   int ind=1; pcs[0]=1;
xue@1:   //anchor partials: highest priority atoms, must have spectral peaks
xue@1:   for (int i=0; i<pcount; i++)
xue@1:   {
xue@1:     //skip out-of-scope atoms
xue@1:     if (pin[i]>=numsam) continue;
xue@1:     //skip user input atom
xue@1:     if (inputpartial && pin[i]==pin0) continue;
xue@1: 
xue@1:     void* dsparams;
xue@1:     if (computes==ds0) dsparams=&cands[ind-1][0]->s;
xue@1:     else
xue@1:     {
xue@1:       dsparams1 params={cands[ind-1][0]->s, lastene, cands[ind-1][0]->acce, pin[i], lastp, lastvfp};
xue@1:       dsparams=&params;
xue@1:     }
xue@1: 
xue@1:     if (pinfr[i]>0) //local anchor
xue@1:     {
xue@1:       double ls=0;
xue@1:       for (int fr=0; fr<Fr; fr++)
xue@1:       {
xue@1:         cdouble r=IPWindowC(pinf[i], x[fr], N, M, c, iH2, pinf[i]-hB, pinf[i]+hB);
xue@1:         ls+=~r;
xue@1:       }
xue@1:       ls=sqrt(ls/Fr);
xue@1:       cands[ind][0]=new TTempAtom(cands[ind-1][0], pin[i], pinf[i], delm, ls, computes(ls, dsparams));
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       //find the nearest peak to pinf[i]
xue@1:       while (pm<pc-1 && fps[pm]<pinf[i]) pm++; while (pm>0 && fps[pm]>=pinf[i]) pm--;
xue@1:       if (pm<pc-1 && pinf[i]-fps[pm]>fps[pm+1]-pinf[i]) pm++;
xue@1:       cands[ind][0]=new TTempAtom(cands[ind-1][0], pin[i], fps[pm], delm, vps[pm], computes(vps[pm], dsparams));
xue@1:     }
xue@1:     delete cands[ind-1][0]->R; cands[ind-1][0]->R=0; psb[pin[i]]=1; pcs[ind]=1; ind++;
xue@1:   }
xue@1:   //harmonic atom grouping fails if anchors conflict
xue@1:   if (cands[ind-1][0]->R->N==0) {f0=0; goto cleanup;}
xue@1: 
xue@1:   //user input partial: must have spectral peak
xue@1:   if (inputpartial)
xue@1:   {
xue@1:     //harmonic atom grouping fails if user partial is out of scope
xue@1:     if (pin0>numsam){f0=0; goto cleanup;}
xue@1: 
xue@1:     TTempAtom* lcand=cands[ind-1][0];
xue@1:     //feasible frequency range for partial pin0
xue@1:     double f1, f2; ExFmStiff(f1, f2, pin0, lcand->R->N, lcand->R->X, lcand->R->Y);
xue@1:     f1-=delm, f2+=delm;
xue@1:     if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1;
xue@1:     //forcepin0 being true means this partial have to be the peak nearest to f0
xue@1:     bool inputfound=false;
xue@1:     if (forceinputlocalfr>=0)
xue@1:     {
xue@1:       int k1=floor(f0-epf0-1), k2=ceil(f0+epf0+1), k12=k2-k1+1, pk=-1;
xue@1: 
xue@1:       cdouble *lx=x[forceinputlocalfr], *lr=new cdouble[k12];
xue@1:       for (int k=0; k<k12; k++) lr[k]=IPWindowC(k1+k, lx, N, M, c, iH2, k1+k-hB, k1+k+hB);
xue@1:       for (int k=1; k<k12-1; k++)
xue@1:         if (~lr[k]>~lr[k-1] && ~lr[k]>~lr[k+1])
xue@1:         {
xue@1:           if (pk==-1 || fabs(k1+pk-f0)>fabs(k1+k-f0)) pk=k;
xue@1:         }
xue@1:       if (pk>0)
xue@1:       {
xue@1:         double lf=k1+pk, la, lph, oldlf=lf;
xue@1:         LSESinusoid(lf, lf-1, lf+1, lx, N, hB, M, c, iH2, la, lph, settings->epf);
xue@1:         if (fabs(lf-oldlf)>0.6)
xue@1:         {
xue@1:           lf=oldlf;
xue@1:           cdouble r=IPWindowC(lf, lx, N, M, c, iH2, lf-hB, lf+hB);
xue@1:           la=abs(r); lph=arg(r);
xue@1:         }
xue@1:         if (lf>f1 && lf<f2)
xue@1:         {
xue@1:           void* dsparams;
xue@1:           if (computes==ds0) dsparams=&lcand->s;
xue@1:           else
xue@1:           {
xue@1:             dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
xue@1:             dsparams=&params;
xue@1:           }
xue@1:           cands[ind][0]=new TTempAtom(lcand, pin0, lf, delm, la, computes(la, dsparams));
xue@1:           pcs[ind]=1;
xue@1:           inputfound=true;
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     if (!inputfound && settings->forcepin0)
xue@1:     { //find the nearest peak to f0
xue@1:       while (pm<pc-1 && fps[pm]<f0) pm++; while (pm>0 && fps[pm]>=f0) pm--;
xue@1:       if (pm<pc-1 && f0-fps[pm]>fps[pm+1]-f0) pm++;
xue@1:       if (fps[pm]>f1 && fps[pm]<f2)
xue@1:       {
xue@1:         void* dsparams;
xue@1:         if (computes==ds0) dsparams=&lcand->s;
xue@1:         else
xue@1:         {
xue@1:           dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
xue@1:           dsparams=&params;
xue@1:         }
xue@1:         cands[ind][0]=new TTempAtom(lcand, pin0, fps[pm], delm, vps[pm], computes(vps[pm], dsparams));
xue@1:         pcs[ind]=1;
xue@1:       }
xue@1:     }
xue@1:     else if (!inputfound)
xue@1:     {
xue@1:       double epf0=settings->epf0;
xue@1:       //lpcs records number of candidates found for partial pin0
xue@1:       int lpcs=0;
xue@1:       //lcoate peaks with the feasible frequency range, specified by f0 and epf0
xue@1:       while (pm>0 && fps[pm]>=f0-epf0) pm--; while (pm<pc-1 && fps[pm]<f0-epf0) pm++;
xue@1:       while (pM<pc-1 && fps[pM]<=f0+epf0) pM++; while (pM>0 && fps[pM]>f0+epf0) pM--;
xue@1:       //add peaks as candidates
xue@1:       for (int j=pm; j<=pM; j++) if (fps[j]>f1 && fps[j]<f2)
xue@1:       {
xue@1:         void* dsparams;
xue@1:         if (computes==ds0) dsparams=&lcand->s;
xue@1:         else
xue@1:         {
xue@1:           dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
xue@1:           dsparams=&params;
xue@1:         }
xue@1:         cands[ind][lpcs]=new TTempAtom(lcand, pin0, fps[j], delm, vps[j], computes(vps[j], dsparams));
xue@1:         lpcs++;
xue@1:       }
xue@1:       pcs[ind]=lpcs;
xue@1:     }
xue@1:     //harmonic atom grouping fails if user partial is not found
xue@1:     if (pcs[ind]==0){f0=0; goto cleanup;}
xue@1:     else {delete lcand->R; lcand->R=0;}
xue@1:     psb[pin0]=2; ind++;
xue@1:   }
xue@1:   //normal partials (non-anchor, non-user)
xue@1:   //p: partial index
xue@1:   {
xue@1:   int p=0;
xue@1:   while (ind<=numsam)
xue@1:   {
xue@1:     p++;
xue@1:     //skip anchor or user input partials
xue@1:     if (psb[p]) continue;
xue@1:     //loop over all candidates
xue@1:     for (int i=0; i<pcs[ind-1]; i++)
xue@1:     {
xue@1:       //the ith candidate of last partial
xue@1:       TTempAtom* lcand=cands[ind-1][i];
xue@1:       //lpcs records number of candidates found for partial p
xue@1:       int lpcs=0;
xue@1:       //the feasible frequency range for partial p
xue@1:       double f1, f2; ExFmStiff(f1, f2, p, lcand->R->N, lcand->R->X, lcand->R->Y); //calculate the frequency range to search for peak
xue@1:       f1-=delm, f2+=delm; //allow a frequency error for the new partial
xue@1:       if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1;
xue@1:       //locate peaks within this range
xue@1:       while (pm>0 && fps[pm]>=f1) pm--; while (pm<pc-1 && fps[pm]<f1) pm++;
xue@1:       while (pM<pc-1 && fps[pM]<=f2) pM++; while (pM>0 && fps[pM]>f2) pM--; //an index range for peaks
xue@1:       //add peaks as candidates
xue@1:       for (int j=pm; j<=pM; j++)
xue@1:       {
xue@1:         if (fps[j]>f1 && fps[j]<f2) //this peak frequency falls in the frequency range
xue@1:         {
xue@1:           void* dsparams;
xue@1:           if (computes==ds0) dsparams=&lcand->s;
xue@1:           else
xue@1:           {
xue@1:             dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
xue@1:             dsparams=&params;
xue@1:           }
xue@1:           cands[ind][pcs[ind]+lpcs]=new TTempAtom(lcand, p, fps[j], delm, vps[j], computes(vps[j], dsparams));
xue@1:           lpcs++;
xue@1:           if (pcs[ind]+lpcs>=MAX_CAND) {int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom)*lpcs); pcs[ind]=newpcs;}
xue@1:         }
xue@1:       }
xue@1:       //if there is no peak found for partial p
xue@1:       if (lpcs==0)
xue@1:       {
xue@1:         cands[ind][pcs[ind]+lpcs]=lcand; lpcs++;
xue@1:         cands[ind-1][i]=0;
xue@1:         if (pcs[ind]+lpcs>=MAX_CAND){int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;}
xue@1:       }
xue@1:       else{delete lcand->R; lcand->R=0;}
xue@1: 
xue@1:       pcs[ind]+=lpcs;
xue@1:     }
xue@1:     ind++;
xue@1:   }
xue@1: 
xue@1:   //select the candidate with maximal s
xue@1:   int maxs=0; double vmax=cands[ind-1][0]->s;
xue@1:   for (int i=1; i<pcs[ind-1]; i++) if (vmax<cands[ind-1][i]->s) maxs=i, vmax=cands[ind-1][i]->s;
xue@1:   TTempAtom* lcand=cands[ind-1][maxs];
xue@1: 
xue@1:   //get results
xue@1:   //read frequencies of found partials
xue@1:   int P=0; int* ps=new int[maxp]; double* fs=new double[maxp]; //these are used for evaluating f0 and B
xue@1:   while (lcand){if (lcand->pind) {ps[P]=lcand->pind; fs[P]=lcand->f; P++;} lcand=lcand->Prev;}
xue@1:   //read R of candidate with maximal s as R0
xue@1:   TPolygon* R0=cands[ind-1][maxs]->R;
xue@1: 
xue@1:   if (Fr==1) result=GetResultsSingleFr(f0, B, R, R0, P, ps, fs, 0, x[0], N, psb, numsam, settings, results);
xue@1:   else result=GetResultsMultiFr(f0, B, R, R0, P, ps, fs, 0, Fr, x, N, offst, psb, numsam, settings, results, forceinputlocalfr);
xue@1: 
xue@1:   delete[] ps; delete[] fs;
xue@1:   }
xue@1: cleanup:
xue@1:   for (int i=0; i<=numsam; i++)
xue@1:     for (int j=0; j<pcs[i]; j++)
xue@1:       delete cands[i][j];
xue@1:   DeAlloc2(cands);
xue@1:   delete[] psb;
xue@1: 
xue@1:   return result;
xue@1: }//NoteMatchStiff3
xue@1: 
Chris@5: /**
xue@1:   function NoteMatchStiff3: finds harmonic atom from spectrum if Fr=1, or constant-pitch harmonic
xue@1:   sinusoid from spectrogram if Fr>1. This version uses residue-sinusoid ratio ("rsr") that measures how
xue@1:   "good" a spectral peak is in terms of its shape. peaks with large rsr[] is likely to be contaminated
xue@1:   and their frequency estimates are regarded unreliable.
xue@1: 
xue@1:   In: x[Fr][N/2+1]: spectrogram
xue@1:       N, offst: atom scale and hop size
xue@1:       fps[pc], vps[pc], rsr[pc]: primitive (rough) peak frequencies, amplitudes and shape factor
xue@1:       R: initial F-G polygon constraint, optional
xue@1:       settings: note match settings
xue@1:       computes: pointer to a function that computes HA score, must be ds0 or ds1
xue@1:       lastvfp[lastp]: amplitude of the previous harmonic atom
xue@1:       forceinputlocalfr: specifies if partial settings->pin0 is taken for granted ("pinned")
xue@1:   Out: results: note match results
xue@1:        f0, B: fundamental frequency and stiffness coefficient
xue@1:        R: F-G polygon of harmonic atom
xue@1: 
xue@1:   Returns the total energy of HA or constant-pitch HS.
xue@1: */
xue@1: double NoteMatchStiff3(TPolygon* R, double &f0, double& B, int pc, double* fps, double* vps,
xue@1:   double* rsr, int Fr, cdouble** x, int N, int offst, NMSettings* settings, NMResults* results,
xue@1:   int lastp, double* lastvfp, double (*computes)(double a, void* params), int forceinputlocalfr)
xue@1: {
xue@1:   double result=0;
xue@1: 
xue@1:   double maxB=settings->maxB, minf0=settings->minf0, maxf0=settings->maxf0,
xue@1:          delm=settings->delm, delp=settings->delp, *c=settings->c, iH2=settings->iH2,
xue@1:          *pinf=settings->pinf, hB=settings->hB, epf0=settings->epf0;// epf=settings->epf,
xue@1:   int maxp=settings->maxp, *pin=settings->pin, pin0=settings->pin0, *pinfr=settings->pinfr,
xue@1:       pcount=settings->pcount, M=settings->M;
xue@1: 
xue@1: //	int *ptype=results->ptype;
xue@1: 
xue@1:   //calculate the energy of the last frame (hp)
xue@1:   double lastene=0; for (int i=0; i<lastp; i++) lastene+=lastvfp[i]*lastvfp[i];
xue@1:   //fill frequency and amplitude buffers with 0
xue@1:   //now determine numsam (the maximal number of partials)
xue@1:   bool inputpartial=(f0>0 && settings->pin0>0);
xue@1:   if (!inputpartial)
xue@1:   {
xue@1:     if (pcount>0) f0=pinf[0], pin0=pin[0];
xue@1:     else f0=sqrt(R->X[0]), pin0=1;
xue@1:   }
xue@1:   int numsam=N*pin0/2.1/f0;
xue@1:   if (numsam>maxp) numsam=maxp;
xue@1:   //allocate buffer for candidate atoms
xue@1:   TTempAtom*** Allocate2(TTempAtom*, numsam+1, MAX_CAND, cands);
xue@1:   //pcs[p] records the number of candidates for partial index p
xue@1:   //psb[p] = 1: anchor, 2: user input, 0: normal
xue@1:   int *psb=new int[(numsam+1)*2], *pcs=&psb[numsam+1];
xue@1:   memset(psb, 0, sizeof(int)*(numsam+1)*2);
xue@1:   //if R is not specified, initialize a trivial candidate with maxB
xue@1:   if (R->N==0) cands[0][0]=new TTempAtom(1, (minf0+maxf0)*0.5, (maxf0-minf0)*0.5, maxB);
xue@1:   //if R is, initialize a trivial candidate by extending R by delp
xue@1:   else cands[0][0]=new TTempAtom(R, delp, delp, minf0);
xue@1: 
xue@1:   int pm=0, pM=0;
xue@1:   int ind=1; pcs[0]=1;
xue@1:   //anchor partials: highest priority atoms, must have spectral peaks
xue@1:   for (int i=0; i<pcount; i++)
xue@1:   {
xue@1:     //skip out-of-scope atoms
xue@1:     if (pin[i]>=numsam) continue;
xue@1:     //skip user input atom
xue@1:     if (inputpartial && pin[i]==pin0) continue;
xue@1: 
xue@1:     void* dsparams;
xue@1:     if (computes==ds0) dsparams=&cands[ind-1][0]->s;
xue@1:     else
xue@1:     {
xue@1:       dsparams1 params={cands[ind-1][0]->s, lastene, cands[ind-1][0]->acce, pin[i], lastp, lastvfp};
xue@1:       dsparams=&params;
xue@1:     }
xue@1: 
xue@1:     if (pinfr[i]>0) //local anchor
xue@1:     {
xue@1:       double ls=0, lrsr=PeakShapeC(pinf[i], 1, N, &x[pinfr[i]], hB*2, M, c, iH2);
xue@1:       for (int fr=0; fr<Fr; fr++)
xue@1:       {
xue@1:         cdouble r=IPWindowC(pinf[i], x[fr], N, M, c, iH2, pinf[i]-hB, pinf[i]+hB);
xue@1:         ls+=~r;
xue@1:       }
xue@1:       ls=sqrt(ls/Fr);
xue@1:       cands[ind][0]=new TTempAtom(cands[ind-1][0], pin[i], pinf[i], delm+lrsr, ls, computes(ls, dsparams)); cands[ind][0]->rsr=lrsr;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       //find the nearest peak to pinf[i]
xue@1:       while (pm<pc-1 && fps[pm]<pinf[i]) pm++; while (pm>0 && fps[pm]>=pinf[i]) pm--;
xue@1:       if (pm<pc-1 && pinf[i]-fps[pm]>fps[pm+1]-pinf[i]) pm++;
xue@1:       cands[ind][0]=new TTempAtom(cands[ind-1][0], pin[i], fps[pm], delm+rsr[pm], vps[pm], computes(vps[pm], dsparams)); cands[ind][0]->rsr=rsr[pm];
xue@1:     }
xue@1:     delete cands[ind-1][0]->R; cands[ind-1][0]->R=0; psb[pin[i]]=1; pcs[ind]=1; ind++;
xue@1:   }
xue@1:   //harmonic atom grouping fails if anchors conflict
xue@1:   if (cands[ind-1][0]->R->N==0) {f0=0; goto cleanup;}
xue@1: 
xue@1:   //user input partial: must have spectral peak
xue@1:   if (inputpartial)
xue@1:   {
xue@1:     //harmonic atom grouping fails if user partial is out of scope
xue@1:     if (pin0>numsam){f0=0; goto cleanup;}
xue@1: 
xue@1:     TTempAtom* lcand=cands[ind-1][0];
xue@1:     //feasible frequency range for partial pin0
xue@1:     double f1, f2; ExFmStiff(f1, f2, pin0, lcand->R->N, lcand->R->X, lcand->R->Y);
xue@1:     f1-=delm, f2+=delm;
xue@1:     if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1;
xue@1:     bool inputfound=false;
xue@1:     if (forceinputlocalfr>=0)
xue@1:     {
xue@1:       int k1=floor(f0-epf0-1), k2=ceil(f0+epf0+1), k12=k2-k1+1, pk=-1;
xue@1: 
xue@1:       cdouble *lx=x[forceinputlocalfr], *lr=new cdouble[k12];
xue@1:       for (int k=0; k<k12; k++) lr[k]=IPWindowC(k1+k, lx, N, M, c, iH2, k1+k-hB, k1+k+hB);
xue@1:       for (int k=1; k<k12-1; k++)
xue@1:         if (~lr[k]>~lr[k-1] && ~lr[k]>~lr[k+1])
xue@1:         {
xue@1:           if (pk==-1 || fabs(k1+pk-f0)>fabs(k1+k-f0)) pk=k;
xue@1:         }
xue@1:       if (pk>0)
xue@1:       {
xue@1:         double lf=k1+pk, la, lph, oldlf=lf;
xue@1:         LSESinusoid(lf, lf-1, lf+1, lx, N, hB, M, c, iH2, la, lph, settings->epf);
xue@1:         if (fabs(lf-oldlf)>0.6) //by which we judge that the LS result is biased by interference
xue@1:         {
xue@1:           lf=oldlf;
xue@1:           cdouble r=IPWindowC(lf, lx, N, M, c, iH2, lf-hB, lf+hB);
xue@1:           la=abs(r); lph=arg(r);
xue@1:         }
xue@1:         if (lf>f1 && lf<f2)
xue@1:         {
xue@1:           void* dsparams;
xue@1:           if (computes==ds0) dsparams=&lcand->s;
xue@1:           else
xue@1:           {
xue@1:             dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
xue@1:             dsparams=&params;
xue@1:           }
xue@1:           double lrsr=PeakShapeC(lf, 1, N, &x[forceinputlocalfr], hB*2, M, c, iH2);
xue@1:           cands[ind][0]=new TTempAtom(lcand, pin0, lf, delm+lrsr, la, computes(la, dsparams)); cands[ind][0]->rsr=lrsr;
xue@1:           pcs[ind]=1;
xue@1:           inputfound=true;
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     //forcepin0 being true means this partial have to be the peak nearest to f0
xue@1:     if (!inputfound && settings->forcepin0)
xue@1:     { //find the nearest peak to f0
xue@1:       while (pm<pc-1 && fps[pm]<f0) pm++; while (pm>0 && fps[pm]>=f0) pm--;
xue@1:       if (pm<pc-1 && f0-fps[pm]>fps[pm+1]-f0) pm++;
xue@1:       if (fps[pm]>f1 && fps[pm]<f2)
xue@1:       {
xue@1:         void* dsparams;
xue@1:         if (computes==ds0) dsparams=&lcand->s;
xue@1:         else
xue@1:         {
xue@1:           dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
xue@1:           dsparams=&params;
xue@1:         }
xue@1:         cands[ind][0]=new TTempAtom(lcand, pin0, fps[pm], delm+rsr[pm], vps[pm], computes(vps[pm], dsparams)); cands[ind][0]->rsr=rsr[pm];
xue@1:         pcs[ind]=1;
xue@1:       }
xue@1:     }
xue@1:     else if (!inputfound)
xue@1:     {
xue@1:       double epf0=settings->epf0;
xue@1:       //lpcs records number of candidates found for partial pin0
xue@1:       int lpcs=0;
xue@1:       //lcoate peaks with the feasible frequency range, specified by f0 and epf0
xue@1:       while (pm>0 && fps[pm]>=f0-epf0) pm--; while (pm<pc-1 && fps[pm]<f0-epf0) pm++;
xue@1:       while (pM<pc-1 && fps[pM]<=f0+epf0) pM++; while (pM>0 && fps[pM]>f0+epf0) pM--;
xue@1:       //add peaks as candidates
xue@1:       for (int j=pm; j<=pM; j++) if (fps[j]>f1 && fps[j]<f2)
xue@1:       {
xue@1:         void* dsparams;
xue@1:         if (computes==ds0) dsparams=&lcand->s;
xue@1:         else
xue@1:         {
xue@1:           dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
xue@1:           dsparams=&params;
xue@1:         }
xue@1:         cands[ind][lpcs]=new TTempAtom(lcand, pin0, fps[j], delm+rsr[j], vps[j], computes(vps[j], dsparams)); cands[ind][lpcs]->rsr=rsr[j];
xue@1:         lpcs++;
xue@1:       }
xue@1:       pcs[ind]=lpcs;
xue@1:     }
xue@1:     //harmonic atom grouping fails if user partial is not found
xue@1:     if (pcs[ind]==0){f0=0; goto cleanup;}
xue@1:     else {delete lcand->R; lcand->R=0;}
xue@1:     psb[pin0]=2; ind++;
xue@1:   }
xue@1:   //normal partials (non-anchor, non-user)
xue@1:   //p: partial index
xue@1:   {
xue@1:   int p=0;
xue@1:   while (ind<=numsam)
xue@1:   {
xue@1:     p++;
xue@1:     //skip anchor or user input partials
xue@1:     if (psb[p]) continue;
xue@1:     //loop over all candidates
xue@1:     for (int i=0; i<pcs[ind-1]; i++)
xue@1:     {
xue@1:       int tightpks=0;
xue@1:       //the ith candidate of last partial
xue@1:       TTempAtom* lcand=cands[ind-1][i];
xue@1:       //lpcs records number of candidates found for partial p
xue@1:       int lpcs=0;
xue@1:       //the feasible frequency range for partial p
xue@1:       double tightf1, tightf2; ExFmStiff(tightf1, tightf2, p, lcand->R->N, lcand->R->X, lcand->R->Y); //calculate the frequency range to search for peak
xue@1:       double tightf=(tightf1+tightf2)*0.5, f1=tightf1-delm, f2=tightf2+delm; //allow a frequency error for the new partial
xue@1:       if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1;
xue@1:       //locate peaks within this range
xue@1:       while (pm>0 && fps[pm]>=f1) pm--; while (pm<pc-1 && fps[pm]<f1) pm++;
xue@1:       while (pM<pc-1 && fps[pM]<=f2) pM++; while (pM>0 && fps[pM]>f2) pM--; //an index range for peaks
xue@1:       //add peaks as candidates
xue@1:       for (int j=pm; j<=pM; j++)
xue@1:       {
xue@1:         if (fps[j]>f1 && fps[j]<f2) //this peak frequency falls in the frequency range
xue@1:         {
xue@1:           if ((fps[j]>tightf1 && fps[j]<tightf2) || fabs(fps[j]-tightf)<0.5) tightpks++; //indicates there are "tight" peaks found for this partial
xue@1:           void* dsparams;
xue@1:           if (computes==ds0) dsparams=&lcand->s;
xue@1:           else
xue@1:           {
xue@1:             dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
xue@1:             dsparams=&params;
xue@1:           }
xue@1:           cands[ind][pcs[ind]+lpcs]=new TTempAtom(lcand, p, fps[j], delm+rsr[j], vps[j], computes(vps[j], dsparams)); cands[ind][pcs[ind]+lpcs]->rsr=rsr[j];
xue@1:           lpcs++;
xue@1:           if (pcs[ind]+lpcs>=MAX_CAND) {int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;}
xue@1:         }
xue@1:       }
xue@1:       //if there is no peak found for partial p.
xue@1:       if (!lpcs)
xue@1:       {
xue@1:         //use the lcand directly
xue@1:         //BUT UPDATE accumulative energy (acce) and s using induced partial
xue@1:         if (tightf<N/2-hB)
xue@1:         {
xue@1:           double la=0; for (int fr=0; fr<Fr; fr++) la+=~IPWindowC(tightf, x[fr], N, M, c, iH2, tightf-hB, tightf+hB); la=sqrt(la/Fr);
xue@1:           void* dsparams;
xue@1:           if (computes==ds0) dsparams=&lcand->s;
xue@1:           else
xue@1:           {
xue@1:             dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
xue@1:             dsparams=&params;
xue@1:           }
xue@1:           double ls=computes(la, dsparams);
xue@1: 
xue@1:           lcand->acce+=la*la;
xue@1:           lcand->s=ls;
xue@1:         }
xue@1:         cands[ind][pcs[ind]+lpcs]=lcand;
xue@1:         lpcs++;
xue@1:         cands[ind-1][i]=0;
xue@1:         if (pcs[ind]+lpcs>=MAX_CAND){int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;}
xue@1:       }
xue@1:       //if there are peaks but no tight peak found for partial p
xue@1:       else if (!tightpks)
xue@1:       {
xue@1:         if (tightf<N/2-hB)
xue@1:         {
xue@1:           double la=0; for (int fr=0; fr<Fr; fr++) la+=~IPWindowC(tightf, x[fr], N, M, c, iH2, tightf-hB, tightf+hB); la=sqrt(la/Fr);
xue@1:           void* dsparams;
xue@1:           if (computes==ds0) dsparams=&lcand->s;
xue@1:           else
xue@1:           {
xue@1:             dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
xue@1:             dsparams=&params;
xue@1:           }
xue@1:           double ls=computes(la, dsparams);
xue@1:           TTempAtom* lnewcand=new TTempAtom(lcand, true);
xue@1:           lnewcand->f=0; lnewcand->acce+=la*la; lnewcand->s=ls;
xue@1:           cands[ind][pcs[ind]+lpcs]=lnewcand;
xue@1:           lpcs++;
xue@1:           if (pcs[ind]+lpcs>=MAX_CAND)
xue@1:           {int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;}
xue@1:         }
xue@1:         delete lcand->R; lcand->R=0;
xue@1:       }
xue@1:       else{delete lcand->R; lcand->R=0;}
xue@1: 
xue@1:       pcs[ind]+=lpcs;
xue@1:     }
xue@1:     ind++;
xue@1:   }
xue@1: 
xue@1:   //select the candidate with maximal s
xue@1:   int maxs=0; double vmax=cands[ind-1][0]->s;
xue@1:   for (int i=1; i<pcs[ind-1]; i++) if (vmax<cands[ind-1][i]->s) maxs=i, vmax=cands[ind-1][i]->s;
xue@1:   TTempAtom* lcand=cands[ind-1][maxs];
xue@1: 
xue@1:   //get results
xue@1:   //read frequencies of found partials
xue@1:   int P=0; int* ps=new int[maxp]; double* fs=new double[maxp*2], *rsrs=&fs[maxp]; //these are used for evaluating f0 and B
xue@1:   while (lcand){if (lcand->pind && lcand->f) {ps[P]=lcand->pind; fs[P]=lcand->f; rsrs[P]=lcand->rsr; P++;} lcand=lcand->Prev;}
xue@1:   //read R of candidate with maximal s as R0
xue@1:   TPolygon* R0=cands[ind-1][maxs]->R;
xue@1: 
xue@1:   if (Fr==1) result=GetResultsSingleFr(f0, B, R, R0, P, ps, fs, rsrs, x[0], N, psb, numsam, settings, results);
xue@1:   else result=GetResultsMultiFr(f0, B, R, R0, P, ps, fs, rsrs, Fr, x, N, offst, psb, numsam, settings, results, forceinputlocalfr);
xue@1: 
xue@1:   delete[] ps; delete[] fs;
xue@1:   }
xue@1: cleanup:
xue@1:   for (int i=0; i<=numsam; i++)
xue@1:     for (int j=0; j<pcs[i]; j++)
xue@1:       delete cands[i][j];
xue@1:   DeAlloc2(cands);
xue@1:   delete[] psb;
xue@1: 
xue@1:   return result;
xue@1: }//NoteMatchStiff3
xue@1: 
Chris@5: /**
xue@1:   function NoteMatchStiff3: wrapper function of the above that does peak picking and HA grouping (or
xue@1:   constant-pitch HS finding) in a single call.
xue@1: 
xue@1:   In: x[Fr][N/2+1]: spectrogram
xue@1:       N, offst: atom scale and hop size
xue@1:       R: initial F-G polygon constraint, optional
xue@1:       startfr, validfrrange: centre and half width, in frames, of the interval used for peak picking if Fr>1
xue@1:       settings: note match settings
xue@1:       computes: pointer to a function that computes HA score, must be ds0 or ds1
xue@1:       lastvfp[lastp]: amplitude of the previous harmonic atom
xue@1:       forceinputlocalfr: specifies if partial settings->pin0 is taken for granted ("pinned")
xue@1:   Out: results: note match results
xue@1:        f0, B: fundamental frequency and stiffness coefficient
xue@1:        R: F-G polygon of harmonic atom
xue@1: 
xue@1:   Returns the total energy of HA or constant-pitch HS.
xue@1: */
xue@1: double NoteMatchStiff3(TPolygon* R, double &f0, double& B, int Fr, cdouble** x, int N, int offst, NMSettings* settings,
xue@1:          NMResults* results, int lastp, double* lastvfp, double (*computes)(double a, void* params),
xue@1:          bool forceinputlocalfr, int startfr, int validfrrange)
xue@1: {
xue@1:   //find spectral peaks
xue@1:   double *fps=(double*)malloc8(sizeof(double)*N*2*Fr), *vps=&fps[N/2*Fr], *rsr=&fps[N*Fr];
xue@1:   int pc;
xue@1:   if (Fr>1) pc=QuickPeaks(fps, vps, Fr, N, x, startfr, validfrrange, settings->M, settings->c, settings->iH2, 0.0005, -1, -1, settings->hB*2, rsr);
xue@1:   else pc=QuickPeaks(fps, vps, N, x[0], settings->M, settings->c, settings->iH2, 0.0005, -1, -1, settings->hB*2, rsr);
xue@1:   //if no peaks are present, return 0, indicating empty hp
xue@1:   if (pc==0){free8(fps); return 0;}
xue@1: 
xue@1:   double result=NoteMatchStiff3(R, f0, B, pc, fps, vps, rsr, Fr, x, N, offst, settings, results, lastp, lastvfp, computes, forceinputlocalfr?startfr:-1);
xue@1:   free8(fps);
xue@1:   return result;
xue@1: }//NoteMatchStiff3
xue@1: 
Chris@5: /**
xue@1:   function NoteMatchStiff3: finds harmonic atom from spectrum given the pitch as a (partial index,
xue@1:   frequency) pair. This is used internally by NoteMatch3FB().
xue@1: 
xue@1:   In: x[N/2+1]: spectrum
xue@1:       fps[pc], vps[pc]: primitive (rough) peak frequencies and amplitudes
xue@1:       R: initial F-G polygon constraint, optional
xue@1:       pin0: partial index in the pitch specifier pair
xue@1:       peak0: index to frequency in fps[] in the pitch specifier pair
xue@1:       settings: note match settings
xue@1:   Out: vfp[]: partial amplitudes
xue@1:        R: F-G polygon of harmonic atom
xue@1: 
xue@1:   Returns the total energy of HA.
xue@1: */
xue@1: double NoteMatchStiff3(TPolygon* R, int peak0, int pin0, cdouble* x, int pc, double* fps, double* vps, int N,
xue@1:          NMSettings* settings, double* vfp, int** pitchind, int newpc)
xue@1: {
xue@1:   double maxB=settings->maxB, minf0=settings->minf0, maxf0=settings->maxf0,
xue@1:          delm=settings->delm, delp=settings->delp, *c=settings->c, iH2=settings->iH2,
xue@1:          hB=settings->hB;
xue@1:   int maxp=settings->maxp, M=settings->M; // pcount=settings->pcount;
xue@1: 
xue@1:   double f0=fps[peak0];
xue@1: 
xue@1:   //determine numsam
xue@1:   int numsam=N*pin0/2.1/f0; if (numsam>maxp) numsam=maxp;
xue@1:   if (pin0>=numsam) return 0;
xue@1:   //pcs[p] contains the number of candidates for partial index p
xue@1:   TTempAtom*** Allocate2(TTempAtom*, numsam+1, MAX_CAND, cands);
xue@1:   int *pcs=new int[numsam+1]; memset(pcs, 0, sizeof(int)*(numsam+1));
xue@1:   if (R->N==0) cands[0][0]=new TTempAtom(1, (minf0+maxf0)*0.5, (maxf0-minf0)*0.5, maxB); //initialization: trivial candidate with maxB
xue@1:   else cands[0][0]=new TTempAtom(R, delp, delp, minf0); //initialization: trivial candidate by expanding R
xue@1: 
xue@1:   cands[1][0]=new TTempAtom(cands[0][0], pin0, fps[peak0], delm, vps[peak0], vps[peak0]); cands[1][0]->tag[0]=peak0;
xue@1:   delete cands[0][0]->R; cands[0][0]->R=0;
xue@1: 
xue@1:   int pm=0, pM=0, ind=2, p=0; pcs[0]=pcs[1]=1;
xue@1: 
xue@1:   while (ind<=numsam)
xue@1:   {
xue@1:     p++;
xue@1:     if (p==pin0) continue;
xue@1:     for (int i=0; i<pcs[ind-1]; i++)
xue@1:     {
xue@1:       TTempAtom* lcand=cands[ind-1][i]; //the the ith candidate of last partial
xue@1:       int lpcs=0;
xue@1:       {
xue@1:         double f1, f2; ExFmStiff(f1, f2, p, lcand->R->N, lcand->R->X, lcand->R->Y); //calculate the frequency range to search for peak
xue@1:         f1-=delm, f2+=delm; //allow a frequency error for the new partial
xue@1:         if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1;
xue@1:         while (pm>0 && fps[pm]>=f1) pm--; while (pm<pc-1 && fps[pm]<f1) pm++;
xue@1:         while (pM<pc-1 && fps[pM]<=f2) pM++; while (pM>0 && fps[pM]>f2) pM--; //an index range for peaks
xue@1:         for (int j=pm; j<=pM; j++)
xue@1:         {
xue@1:           if (fps[j]>f1 && fps[j]<f2 && (p>pin0 || pitchind[j][p]<0))
xue@1:           {
xue@1:             //this peak frequency falls in the frequency range
xue@1:             cands[ind][pcs[ind]+lpcs]=new TTempAtom(lcand, p, fps[j], delm, vps[j], lcand->s+vps[j]); cands[ind][pcs[ind]+lpcs]->tag[0]=j; lpcs++;
xue@1:             if (pcs[ind]+lpcs>=MAX_CAND){int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom)*lpcs); pcs[ind]=newpcs;}
xue@1:           }
xue@1:         }
xue@1:         if (lpcs==0) //there is no peak found for the partial i, the last level
xue@1:         {
xue@1:           cands[ind][pcs[ind]+lpcs]=lcand; lpcs++; //new TTempAtom(lcand, false);
xue@1:           cands[ind-1][i]=0;
xue@1:           if (pcs[ind]+lpcs>=MAX_CAND){int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;}
xue@1:         }
xue@1:         else
xue@1:         {
xue@1:           delete lcand->R; lcand->R=0;
xue@1:         }
xue@1:       }
xue@1:       pcs[ind]+=lpcs;
xue@1:     }
xue@1:     ind++;
xue@1:   }
xue@1: 
xue@1:   //select the candidate with maximal s
xue@1:   int maxs=0; double vmax=cands[ind-1][0]->s;
xue@1:   for (int i=1; i<pcs[ind-1]; i++) if (vmax<cands[ind-1][i]->s) maxs=i, vmax=cands[ind-1][i]->s;
xue@1:   TTempAtom* lcand=cands[ind-1][maxs];
xue@1: 
xue@1:   //get fp, vfp, pfp, ptype
xue@1:   memset(vfp, 0, sizeof(double)*maxp);
xue@1: 
xue@1:   int P=0; int *ps=new int[maxp], *peaks=new int[maxp]; double* fs=new double[maxp]; //these are used for evaluating f0 and B
xue@1: 
xue@1:   while (lcand){if (lcand->pind) {ps[P]=lcand->pind; fs[P]=lcand->f; peaks[P]=lcand->tag[0]; P++;} lcand=lcand->Prev;}
xue@1:   R->N=0;
xue@1: 
xue@1:   if (P>0)
xue@1:   {
xue@1:     for (int i=P-1; i>=0; i--)
xue@1:     {
xue@1:       int lp=ps[i];
xue@1:       double lf=fs[i];
xue@1:       vfp[lp-1]=vps[peaks[i]];
xue@1:       if (R->N==0) InitializeR(R, lp, lf, delm, maxB);
xue@1:       else if (vfp[lp-1]>1) CutR(R, lp, lf, delm, true);
xue@1:       if (pitchind[peaks[i]][lp]>=0) {R->N=0; goto cleanup;}
xue@1:       else pitchind[peaks[i]][lp]=newpc;
xue@1:     }
xue@1: 
xue@1:     //estimate f0 and B
xue@1:     double tmpa, cF, cG;
xue@1: //    double norm[1024]; for (int i=0; i<1024; i++) norm[i]=1;
xue@1:     areaandcentroid(tmpa, cF, cG, R->N, R->X, R->Y);
xue@1:     testnn(cF); f0=sqrt(cF); //B=cG/cF;
xue@1: 
xue@1: 
xue@1:     for (int i=0; i<numsam; i++)
xue@1:     {
xue@1:       if (vfp[i]==0) //no peak is found for this partial in lcand
xue@1:       {
xue@1:         int m=i+1;
xue@1:         double tmp=cF+(m*m-1)*cG; testnn(tmp);
xue@1:         double lf=m*sqrt(tmp);
xue@1:         if (lf<N/2.1)
xue@1:         {
xue@1:           cdouble r=IPWindowC(lf, x, N, M, c, iH2, lf-hB, lf+hB);
xue@1:           vfp[i]=-sqrt(r.x*r.x+r.y*r.y);
xue@1: //          vfp[i]=-IPWindowC(lf, xr, xi, N, M, c, iH2, lf-hB, lf+hB, 0);
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:   }
xue@1: 
xue@1: cleanup:
xue@1:   delete[] ps; delete[] fs; delete[] peaks;
xue@1:   for (int i=0; i<=numsam; i++) for (int j=0; j<pcs[i]; j++) delete cands[i][j];
xue@1:   DeAlloc2(cands); delete[] pcs;
xue@1: 
xue@1:   double result=0;
xue@1:   if (f0>0) for (int i=0; i<numsam; i++) result+=vfp[i]*vfp[i];
xue@1:   return result;
xue@1: }//NoteMatchStiff3*/
xue@1: 
Chris@5: /**
xue@1:   function NoteMatchStiff3FB: does one dynamic programming step in forward-background pitch tracking of
xue@1:   harmonic sinusoids. This is used internally by FindNoteFB().
xue@1: 
xue@1:   In: R[pitchcount]: initial F-G polygons associated with pitch candidates at last frame
xue@1:       vfp[pitchcount][maxp]: amplitude vectors associated with pitch candidates at last frame
xue@1:       sc[pitchcount]: accumulated scores associated with pitch candidates at last frame
xue@1:       fps[pc], vps[pc]: primitive (rough) peak frequencies and amplitudes at this frame
xue@1:       maxpitch: maximal number of pitch candidates
xue@1:       x[wid/2+1]: spectrum
xue@1:       settings: note match settings
xue@1:   Out: pitchcount: number of pitch candidiate at this frame
xue@1:        newpitches[pitchcount]: pitch candidates at this frame, whose lower word is peak index, higher word is partial index
xue@1:        prev[pitchcount]: indices to predecessors of the pitch candidates at this frame
xue@1:        R[pitchcount]: F-G polygons associated with pitch candidates at this frame
xue@1:        vfp[pitchcount][maxp]: amplitude vectors associated with pitch candidates at this frame
xue@1:        sc[pitchcount]: accumulated scores associated with pitch candidates at this frame
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void NoteMatchStiff3FB(int& pitchcount, TPolygon**& R, double**& vfp, double*& sc, int* newpitches, int* prev, int pc, double* fps, double* vps, cdouble* x, int wid, int maxpitch, NMSettings* settings)
xue@1: {
xue@1:   int maxpin0=6; //this specifies the maximal value that a pitch candidate may have as partial index
xue@1:   double minf0=settings->minf0, delm=settings->delm, delp=settings->delp;
xue@1:   int maxp=settings->maxp;
xue@1: 
xue@1:   //pc is the number of peaks at this frame, maxp is the maximal number of partials in the model
xue@1:   //pitchind is initialized to a sequence of -1
xue@1:   int** Allocate2(int, pc, maxp+1, pitchind); memset(pitchind[0], 0xff, sizeof(int)*pc*(maxp+1));
xue@1: 
xue@1:   //extend F-G polygons to allow pitch variation
xue@1:   for (int i=0; i<pitchcount; i++) ExtendR(R[i], delp, delp, minf0);
xue@1: 
xue@1:   double *f1=new double[pitchcount], *f2=new double[pitchcount];
xue@1:   int pm=0, newpc=0;
xue@1:   TPolygon** newR=new TPolygon*[maxpitch]; memset(newR, 0, sizeof(TPolygon*)*maxpitch);
xue@1:   double* newsc=new double[maxpitch]; memset(newsc, 0, sizeof(double)*maxpitch);
xue@1:   double** Allocate2(double, maxpitch, maxp, newvfp);
xue@1: 
xue@1:   for (int pin0=1; pin0<=maxpin0; pin0++) //pin0: the partial index of a candidate pitch
xue@1:   {
xue@1:     //find out the range [pm, pM) of indices into fps[], so that for a * in this range (pin0, fps[*]) may fall
xue@1:     //in the feasible pitch range succeeding one of the candidate pitches of the previous frame
xue@1:     for (int i=0; i<pitchcount; i++) {ExFmStiff(f1[i], f2[i], pin0, R[i]->N, R[i]->X, R[i]->Y); f1[i]-=delm, f2[i]+=delm;}
xue@1:     double f1a=f1[0], f2a=f2[0]; for (int i=1; i<pitchcount; i++) {if (f1a>f1[i]) f1a=f1[i]; if (f2a<f2[i]) f2a=f2[i];}
xue@1:     while (pm<pc-1 && fps[pm]<f1a) pm++;
xue@1:     int pM=pm; while (pM<pc-1 && fps[pM]<f2a) pM++;
xue@1: 
xue@1:     for (int p=pm; p<pM; p++) //loop through all peaks in this range
xue@1:     {
xue@1:       if (pitchind[p][pin0]>=0) continue; //skip this peak if it is already ...
xue@1:       int max; double maxs; TPolygon* lnewR=0;
xue@1: 
xue@1:       for (int i=0; i<pitchcount; i++) //loop through candidate pitches of the previous frame
xue@1:       {
xue@1:         if (fps[p]>f1[i] && fps[p]<f2[i]) //so that this peak is a feasible successor of the i-th candidate pitch of the previous frame
xue@1:         {
xue@1:           if (pitchind[p][pin0]<0) //if this peak has not been registered as a candidate pitch with pin0 being the partial index
xue@1:           {
xue@1:             lnewR=new TPolygon(maxp*2+4, R[i]); //create a F-G polygon for (this peak, pin0) pair
xue@1:             if (newpc==maxpitch)  //maximal number of candidate pitches is reached
xue@1:             {
xue@1:               //delete the candidate with the lowest score without checking if this lowest score is below the score
xue@1:               //of the current pitch candidate (which is not yet computed). of course there is a risk that the new
xue@1:               //score is even lower so that $maxpitch buffered scores may not be the highest $maxpitch scores, but
xue@1:               //the (maxpitch-1) highest of the $maxpitch buffered scores should be the highest (maxpitch01) scores.
xue@1:               int minsc=0; for (int j=0; j<maxpitch; j++) if (newsc[j]<newsc[minsc]) minsc=j;
xue@1:               delete newR[minsc];
xue@1:               if (minsc!=newpc-1) {newR[minsc]=newR[newpc-1]; memcpy(newvfp[minsc], newvfp[newpc-1], sizeof(double)*maxp); newsc[minsc]=newsc[newpc-1]; prev[minsc]=prev[newpc-1]; newpitches[minsc]=newpitches[newpc-1];}
xue@1:             }
xue@1:             else newpc++;
xue@1:             //try to find harmonic atom with this candidate pitch
xue@1:             if (NoteMatchStiff3(lnewR, p, pin0, x, pc, fps, vps, wid, settings, newvfp[newpc-1], pitchind, newpc-1)>0)
xue@1:             {//if successful, buffer its F-G polygon and save its score
xue@1:               newR[newpc-1]=lnewR;
xue@1:               max=i; maxs=sc[i]+conta(maxp, newvfp[newpc-1], vfp[i]);
xue@1:             }
xue@1:             else
xue@1:             {//if not, discard this candidate pitch
xue@1:               delete lnewR; lnewR=0; newpc--;
xue@1:             }
xue@1:           }
xue@1:           else  //if this peak has already been registered as a candidate pitch with pin0 being the partial index
xue@1:           {
xue@1:             //compute it score as successor to the i-th candidate of the previous frame
xue@1:             double ls=sc[i]+conta(maxp, newvfp[newpc-1], vfp[i]);
xue@1:             //if the score is higher than mark the i-th candidate of previous frame as its predecessor
xue@1:             if (ls>maxs) maxs=ls, max=i;
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:       if (lnewR) //i.e. a HA is found for this pitch candidate
xue@1:       {
xue@1:         ((__int16*)&newpitches[newpc-1])[0]=p; ((__int16*)&newpitches[newpc-1])[1]=pin0;
xue@1:         newsc[newpc-1]=maxs; //take note of its score
xue@1:         prev[newpc-1]=max;  //take note of its predecessor
xue@1:       }
xue@1:     }
xue@1:   }
xue@1:   DeAlloc2(pitchind);
xue@1:   delete[] f1; delete[] f2;
xue@1: 
xue@1:   for (int i=0; i<pitchcount; i++) delete R[i]; delete[] R; R=newR;
xue@1:   for (int i=0; i<newpc; i++) for (int j=0; j<maxp; j++) if (newvfp[i][j]<0) newvfp[i][j]=-newvfp[i][j];
xue@1:   DeAlloc2(vfp); vfp=newvfp;
xue@1:   delete[] sc; sc=newsc;
xue@1:   pitchcount=newpc;
xue@1: }//NoteMatchStiff3FB
xue@1: 
Chris@5: /**
xue@1:   function PeakShapeC: residue-sinusoid-ratio for a given (hypothesis) sinusoid frequency
xue@1: 
xue@1:   In: x[Fr][N/2+1]: spectrogram
xue@1:       M, c[], iH2: cosine-family window specifiers
xue@1:       f: reference frequency, in bins
xue@1:       B: spectral truncation width
xue@1: 
xue@1:   Returns the residue-sinusoid-ratio.
xue@1: */
xue@1: double PeakShapeC(double f, int Fr, int N, cdouble** x, int B, int M, double* c, double iH2)
xue@1: {
xue@1:   cdouble* w=new cdouble[B];
xue@1:   int fst=floor(f+1-B/2.0);
xue@1:   if (fst<0) fst=0;
xue@1:   if (fst+B>N/2) fst=N/2-B;
xue@1:   Window(w, f, N, M, c, fst, fst+B-1);
xue@1:   cdouble xx=0, ww=Inner(B, w, w);
xue@1:   double xw2=0;
xue@1:   for (int fr=0; fr<Fr; fr++)
xue@1:     xw2+=~Inner(B, &x[fr][fst], w), xx+=Inner(B, &x[fr][fst], &x[fr][fst]);
xue@1:   delete[] w;
xue@1:   if (xx.x==0) return 1;
xue@1:   return 1-xw2/(xx.x*ww.x);
xue@1: }//PeakShapeC
xue@1: //version using cmplx<float> as spectrogram data type
xue@1: double PeakShapeC(double f, int Fr, int N, cfloat** x, int B, int M, double* c, double iH2)
xue@1: {
xue@1:   cdouble* w=new cdouble[B];
xue@1:   int fst=floor(f+1-B/2.0);
xue@1:   if (fst<0) fst=0;
xue@1:   if (fst+B>N/2) fst=N/2-B;
xue@1:   Window(w, f, N, M, c, fst, fst+B-1);
xue@1:   cdouble xx=0, ww=Inner(B, w, w);
xue@1:   double xw2=0;
xue@1:   for (int fr=0; fr<Fr; fr++)
xue@1:     xw2+=~Inner(B, &x[fr][fst], w), xx+=Inner(B, &x[fr][fst], &x[fr][fst]);
xue@1:   delete[] w;
xue@1:   if (xx.x==0) return 1;
xue@1:   return 1-xw2/(xx.x*ww.x);
xue@1: }//PeakShapeC
xue@1: 
Chris@5: /**
xue@1:   function QuickPeaks: finds rough peaks in the spectrum (peak picking)
xue@1: 
xue@1:   In: x[N/2+1]: spectrum
xue@1:       M, c[], iH2: cosine-family window function specifiers
xue@1:       mina: minimal amplitude to spot a spectral peak
xue@1:       [binst, binen): frequency range, in bins, to look for peaks
xue@1:       B: spectral truncation width
xue@1:   Out; f[return value], a[return value]: frequencies (in bins) and amplitudes of found peaks
xue@1:        rsr[return value]: residue-sinusoid-ratio of found peaks, optional
xue@1: 
xue@1:   Returns the number of peaks found. f[] and a[] must be allocated enough space before calling.
xue@1: */
xue@1: int QuickPeaks(double* f, double* a, int N, cdouble* x, int M, double* c, double iH2, double mina, int binst, int binen, int B, double* rsr)
xue@1: {
xue@1:   double hB=B*0.5;
xue@1:   if (binst<2) binst=2;
xue@1:   if (binst<hB) binst=hB;
xue@1:   if (binen<0) binen=N/2-1;
xue@1:   if (binen<0 || binen+hB>N/2-1) binen=N/2-1-hB;
xue@1:   double a0=~x[binst-1], a1=~x[binst], a2;
xue@1:   double minA=mina*mina*2/iH2;
xue@1:   int p=0, n=binst;
xue@1:   cdouble* w=new cdouble[B];
xue@1:   while (n<binen)
xue@1:   {
xue@1:     a2=~x[n+1];
xue@1:     if (a1>0 && a1>=a0 && a1>=a2)
xue@1:     {
xue@1:       if (a1>minA)
xue@1:       {
xue@1:         double A0=sqrt(a0), A1=sqrt(a1), A2=sqrt(a2);
xue@1:         f[p]=n+(A0-A2)/2/(A0+A2-2*A1);
xue@1:         int fst=floor(f[p]+1-hB);
xue@1:         Window(w, f[p], N, M, c, fst, fst+B-1);
xue@1:         double xw2=~Inner(B, &x[fst], w);
xue@1:         a[p]=sqrt(xw2)*iH2;
xue@1:         if (rsr)
xue@1:         {
xue@1:           cdouble xx=Inner(B, &x[fst], &x[fst]);
xue@1:           if (xx.x==0) rsr[p]=1;
xue@1:           else rsr[p]=1-xw2/xx.x*iH2;
xue@1:         }
xue@1:         p++;
xue@1:       }
xue@1:     }
xue@1:     a0=a1;
xue@1:     a1=a2;
xue@1:     n++;
xue@1:   }
xue@1:   delete[] w;
xue@1:   return p;
xue@1: }//QuickPeaks
xue@1: 
Chris@5: /**
xue@1:   function QuickPeaks: finds rough peaks in the spectrogram (peak picking) for constant-frequency
xue@1:   sinusoids
xue@1: 
xue@1:   In: x[Fr][N/2+1]: spectrogram
xue@1:       fr0, r0: centre and half width of interval (in frames) to use for peak picking
xue@1:       M, c[], iH2: cosine-family window function specifiers
xue@1:       mina: minimal amplitude to spot a spectral peak
xue@1:       [binst, binen): frequency range, in bins, to look for peaks
xue@1:       B: spectral truncation width
xue@1:   Out; f[return value], a[return value]: frequencies (in bins) and summary amplitudes of found peaks
xue@1:        rsr[return value]: residue-sinusoid-ratio of found peaks, optional
xue@1: 
xue@1:   Returns the number of peaks found. f[] and a[] must be allocated enough space before calling.
xue@1: */
xue@1: int QuickPeaks(double* f, double* a, int Fr, int N, cdouble** x, int fr0, int r0, int M, double* c, double iH2, double mina, int binst, int binen, int B, double* rsr)
xue@1: {
xue@1:   double hB=B*0.5;
xue@1:   int hN=N/2;
xue@1:   if (binst<hB) binst=hB;
xue@1:   if (binen<0 || binen>hN-hB) binen=hN-hB;
xue@1:   double minA=mina*mina*2/iH2;
xue@1: 
xue@1:   double *a0s=new double[hN*3*r0], *a1s=&a0s[hN*r0], *a2s=&a0s[hN*2*r0];
xue@1:   int *idx=new int[hN*r0], *frc=new int[hN*r0];
xue@1:   int** Allocate2(int, (hN*r0), (r0*2+1), frs);
xue@1: 
xue@1:   int pc=0;
xue@1: 
xue@1:   int lr=0;
xue@1:   while (lr<=r0)
xue@1:   {
xue@1:     int fr[2]; //indices to frames to process for this lr
xue@1:     if (lr==0) fr[0]=fr0, fr[1]=-1;
xue@1:     else
xue@1:     {
xue@1:       fr[0]=fr0-lr, fr[1]=fr0+lr;
xue@1:       if (fr[1]>=Fr) fr[1]=-1;
xue@1:       if (fr[0]<0) {fr[0]=fr[1]; fr[1]=-1;}
xue@1:     }
xue@1:     for (int i=0; i<2; i++)
xue@1:     {
xue@1:       if (fr[i]>=0)
xue@1:       {
xue@1:         cdouble* xfr=x[fr[i]];
xue@1:         for (int k=binst; k<binen; k++)
xue@1:         {
xue@1:           double a0=~xfr[k-1], a1=~xfr[k], a2=~xfr[k+1];
xue@1:           if (a1>=a0 && a1>=a2)
xue@1:           {
xue@1:             if (a1>minA)
xue@1:             {
xue@1:               double A1=sqrt(a1), A2=sqrt(a2), A0=sqrt(a0);
xue@1:               double lf=k+(A0-A2)/2/(A0+A2-2*A1);
xue@1:               if (lr==0) //starting frame
xue@1:               {
xue@1:                 f[pc]=lf;
xue@1:                 a0s[pc]=a0, a1s[pc]=a1, a2s[pc]=a2;
xue@1:                 idx[pc]=pc; frs[pc][0]=fr[i]; frc[pc]=1;
xue@1:                 pc++;
xue@1:               }
xue@1:               else
xue@1:               {
xue@1:                 int closep, indexp=InsertInc(lf, f, pc, false); //find the closest peak ever found in previous (i.e. closer to fr0) frames
xue@1:                 if (indexp==-1) indexp=pc, closep=pc-1;
xue@1:                 else if (indexp-1>=0 && fabs(lf-f[indexp-1])<fabs(lf-f[indexp])) closep=indexp-1;
xue@1:                 else closep=indexp;
xue@1: 
xue@1:                 if (fabs(lf-f[closep])<0.2) //i.e. lf is very close to previous peak at fs[closep]
xue@1:                 {
xue@1:                   int idxp=idx[closep];
xue@1:                   a0s[idxp]+=a0, a1s[idxp]+=a1, a2s[idxp]+=a2;
xue@1:                   frs[idxp][frc[idxp]]=fr[i]; frc[idxp]++;
xue@1:                   double A0=sqrt(a0s[idxp]), A1=sqrt(a1s[idxp]), A2=sqrt(a2s[idxp]);
xue@1:                   f[closep]=k+(A0-A2)/2/(A0+A2-2*A1);
xue@1:                 }
xue@1:                 else
xue@1:                 {
xue@1:                   memmove(&f[indexp+1], &f[indexp], sizeof(double)*(pc-indexp)); f[indexp]=lf;
xue@1:                   memmove(&idx[indexp+1], &idx[indexp], sizeof(int)*(pc-indexp)); idx[indexp]=pc;
xue@1:                   a0s[pc]=a0, a1s[pc]=a1, a2s[pc]=a2, frs[pc][0]=fr[i], frc[pc]=1;
xue@1:                   pc++;
xue@1:                 }
xue@1:               }
xue@1:             }
xue@1:           }
xue@1:         }
xue@1:       }
xue@1:     }
xue@1:     lr++;
xue@1:   }
xue@1: 
xue@1:   cdouble* w=new cdouble[B];
xue@1:   int* frused=new int[Fr];
xue@1:   for (int p=0; p<pc; p++)
xue@1:   {
xue@1:     int fst=floor(f[p]+1-hB);
xue@1:     memset(frused, 0, sizeof(int)*Fr);
xue@1:     int idxp=idx[p];
xue@1: 
xue@1:     Window(w, f[p], N, M, c, fst, fst+B-1);
xue@1:     double xw2=0, xw2r=0, xxr=0;
xue@1: 
xue@1:     for (int ifr=0; ifr<frc[idxp]; ifr++)
xue@1:     {
xue@1:       int fr=frs[idxp][ifr];
xue@1:       frused[fr]=1;
xue@1:       xw2r+=~Inner(B, &x[fr][fst], w);
xue@1:       xxr+=Inner(B, &x[fr][fst], &x[fr][fst]).x;
xue@1:     }
xue@1:     xw2=xw2r;
xue@1:     for (int fr=0; fr<Fr; fr++)
xue@1:       if (!frused[fr]) xw2+=~Inner(B, &x[fr][fst], w);
xue@1:     a[p]=sqrt(xw2/Fr)*iH2;
xue@1:     if (xxr==0) rsr[p]=1;
xue@1:     else rsr[p]=1-xw2r/xxr*iH2;
xue@1:   }
xue@1:   delete[] w;
xue@1:   delete[] frused;
xue@1:   delete[] a0s;
xue@1:   delete[] idx;
xue@1:   delete[] frc;
xue@1:   DeAlloc2(frs);
xue@1:   return pc;
xue@1: }//QuickPeaks
xue@1: 
Chris@5: /**
xue@1:   function ReEstHS1: reestimates a harmonic sinusoid by one multiplicative reestimation using phasor
xue@1:   multiplier
xue@1: 
xue@1:   In: partials[M][Fr]: HS partials
xue@1:       [frst, fren): frame (measurement point) range on which to do reestimation
xue@1:       Data16[]: waveform data
xue@1:   Out: partials[M][Fr]: updated HS partials
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void ReEstHS1(int M, int Fr, int frst, int fren, atom** partials, __int16* Data16)
xue@1: {
xue@1:   double* fs=new double[Fr*3], *as=&fs[Fr], *phs=&fs[Fr*2];
xue@1:   int wid=partials[0][0].s, offst=partials[0][1].t-partials[0][0].t;
xue@1:   int ldatastart=partials[0][0].t-wid/2;
xue@1:   __int16* ldata16=&Data16[ldatastart];
xue@1:   for (int m=0; m<M; m++)
xue@1:   {
xue@1:     for (int fr=0; fr<Fr; fr++) {fs[fr]=partials[m][fr].f; if (fs[fr]<=0){delete[] fs; return;} as[fr]=partials[m][fr].a, phs[fr]=partials[m][fr].p;}
xue@1:     MultiplicativeUpdateF(fs, as, phs, ldata16, Fr, frst, fren, wid, offst);
xue@1:     for (int fr=0; fr<Fr; fr++) partials[m][fr].f=fs[fr], partials[m][fr].a=as[fr], partials[m][fr].p=phs[fr];
xue@1:   }
xue@1:   delete[] fs;
xue@1: }//ReEstHS1
xue@1: 
Chris@5: /**
xue@1:   function ReEstHS1: wrapper function.
xue@1: 
xue@1:   In: HS: a harmonic sinusoid
xue@1:       Data16: its waveform data
xue@1:   Out: HS: updated harmonic sinusoid
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void ReEstHS1(THS* HS, __int16* Data16)
xue@1: {
xue@1:   ReEstHS1(HS->M, HS->Fr, 0, HS->Fr, HS->Partials, Data16);
xue@1: }//ReEstHS1
xue@1: 
Chris@5: /**
xue@1:   function SortCandid: inserts a candid object into a listed of candid objects sorted first by f, then
xue@1:   (for identical f's) by df.
xue@1: 
xue@1:   In: cand: the candid object to insert to the list
xue@1:       cands[newN]: the sorted list of candid objects
xue@1:   Out: cands[newN+1]: the sorted list after the insertion
xue@1: 
xue@1:   Returns the index of $cand in the new list.
xue@1: */
xue@1: int SortCandid(candid cand, candid* cands, int newN)
xue@1: {
xue@1:   int lnN=newN-1;
xue@1:   while (lnN>=0 && cands[lnN].f>cand.f) lnN--;
xue@1:   while (lnN>=0 && cands[lnN].f==cand.f && cands[lnN].df>cand.df) lnN--;
xue@1:   lnN++; //now insert cantmp as cands[lnN]
xue@1:   memmove(&cands[lnN+1], &cands[lnN], sizeof(candid)*(newN-lnN));
xue@1:   cands[lnN]=cand;
xue@1:   return lnN;
xue@1: }//SortCandid
xue@1: 
Chris@5: /**
xue@1:   function SynthesisHS: synthesizes a harmonic sinusoid without aligning the phases
xue@1: 
xue@1:   In: partials[M][Fr]: HS partials
xue@1:       terminatetag: external termination flag. Function SynthesisHS() polls *terminatetag and exits with
xue@1:       0 when it is set.
xue@1:   Out: [dst, den): time interval synthesized
xue@1:        xrec[den-dst]: resynthesized harmonic sinusoid
xue@1: 
xue@1:   Returns pointer to xrec on normal finish, or 0 on external termination by setting $terminatetag. In
xue@1:   the first case xrec is created anew with malloc8() and must be freed by caller using free8().
xue@1: */
xue@1: double* SynthesisHS(int M, int Fr, atom** partials, int& dst, int& den, bool* terminatetag)
xue@1: {
xue@1:   int wid=partials[0][0].s, hwid=wid/2;
xue@1:   double *as=(double*)malloc8(sizeof(double)*Fr*13);
xue@1:   double *fs=&as[Fr], *f3=&as[Fr*2], *f2=&as[Fr*3], *f1=&as[Fr*4], *f0=&as[Fr*5],
xue@1:          *a3=&as[Fr*6], *a2=&as[Fr*7], *a1=&as[Fr*8], *a0=&as[Fr*9], *xs=&as[Fr*11];
xue@1:   int* ixs=(int*)&as[Fr*12];
xue@1: 
xue@1:   dst=partials[0][0].t-hwid, den=partials[0][Fr-1].t+hwid;
xue@1:   double* xrec=(double*)malloc8(sizeof(double)*(den-dst));
xue@1:   memset(xrec, 0, sizeof(double)*(den-dst));
xue@1: 
xue@1:   for (int m=0; m<M; m++)
xue@1:   {
xue@1:     atom* part=partials[m]; bool fzero=false;
xue@1:     for (int fr=0; fr<Fr; fr++)
xue@1:     {
xue@1:       if (part[fr].f<=0){fzero=true; break;}
xue@1:       ixs[fr]=part[fr].t;
xue@1:       xs[fr]=part[fr].t;
xue@1:       as[fr]=part[fr].a*2;
xue@1:       fs[fr]=part[fr].f;
xue@1:       if (part[fr].type==atMuted) as[fr]=0;
xue@1: 		}
xue@1:     if (fzero) break;
xue@1:     if (terminatetag && *terminatetag) {free8(xrec); free8(as); return 0;}
xue@1: 
xue@1:     CubicSpline(Fr-1, f3, f2, f1, f0, xs, fs, 1, 1);
xue@1:     CubicSpline(Fr-1, a3, a2, a1, a0, xs, as, 1, 1);
xue@1:     double ph=0, ph0=0;
xue@1:     for (int fr=0; fr<Fr-1; fr++)
xue@1:     {
xue@1:       part[fr].p=ph;
xue@1:       ALIGN8(Sinusoid(&xrec[ixs[fr]-dst], 0, ixs[fr+1]-ixs[fr], a3[fr], a2[fr], a1[fr], a0[fr], f3[fr], f2[fr], f1[fr], f0[fr], ph, true);)
xue@1:       if (terminatetag && *terminatetag) {free8(xrec); free8(as); return 0;}
xue@1:     }
xue@1:     part[Fr-1].p=ph;
xue@1:     ALIGN8(Sinusoid(&xrec[ixs[Fr-2]-dst], ixs[Fr-1]-ixs[Fr-2], den-ixs[Fr-2], a3[Fr-2], a2[Fr-2], a1[Fr-2], a0[Fr-2], f3[Fr-2], f2[Fr-2], f1[Fr-2], f0[Fr-2], ph, true);
xue@1:            Sinusoid(&xrec[ixs[0]-dst], dst-ixs[0], 0, a3[0], a2[0], a1[0], a0[0], f3[0], f2[0], f1[0], f0[0], ph0, true);)
xue@1:   }
xue@1:   free8(as);
xue@1:   return xrec;
xue@1: }//SynthesisHS
xue@1: 
Chris@5: /**
xue@1:   function SynthesisHS: synthesizes a perfectly harmonic sinusoid without aligning the phases.
xue@1:   Frequencies of partials above the fundamental are not used in this synthesis process.
xue@1: 
xue@1:   In: partials[M][Fr]: HS partials
xue@1:       terminatetag: external termination flag. This function polls *terminatetag and exits with 0 when
xue@1:       it is set.
xue@1:   Out: [dst, den) time interval synthesized
xue@1:        xrec[den-dst]: resynthesized harmonic sinusoid
xue@1: 
xue@1:   Returns pointer to xrec on normal finish, or 0 on external termination by setting $terminatetag. In
xue@1:   the first case xrec is created anew with malloc8() and must be freed by caller using free8().
xue@1: */
xue@1: double* SynthesisHS2(int M, int Fr, atom** partials, int& dst, int& den, bool* terminatetag)
xue@1: {
xue@1:   int wid=partials[0][0].s, hwid=wid/2;
xue@1:   double *as=(double*)malloc8(sizeof(double)*Fr*7); //*as=new double[Fr*8],
xue@1:   double *xs=&as[Fr], *f3=&as[Fr*2], *f2=&as[Fr*3], *f1=&as[Fr*4], *f0=&as[Fr*5];
xue@1: 	int *ixs=(int*)&as[Fr*6];
xue@1: 	double** a0=new double*[M*4], **a1=&a0[M], **a2=&a0[M*2], **a3=&a0[M*3];
xue@1: 	a0[0]=(double*)malloc8(sizeof(double)*M*Fr*4); //a0[0]=new double[M*Fr*4];
xue@1: 	for (int m=0; m<M; m++) a0[m]=&a0[0][m*Fr*4], a1[m]=&a0[m][Fr], a2[m]=&a0[m][Fr*2], a3[m]=&a0[m][Fr*3];
xue@1: 
xue@1:   dst=partials[0][0].t-hwid, den=partials[0][Fr-1].t+hwid;
xue@1:   double* xrec=(double*)malloc8(sizeof(double)*(den-dst));
xue@1: 	memset(xrec, 0, sizeof(double)*(den-dst));
xue@1:   atom* part=partials[0];
xue@1: 
xue@1:   for (int fr=0; fr<Fr; fr++)
xue@1:   {
xue@1:     xs[fr]=ixs[fr]=part[fr].t;
xue@1:     as[fr]=part[fr].f;
xue@1:   }
xue@1:   CubicSpline(Fr-1, f3, f2, f1, f0, xs, as, 1, 1);
xue@1: 
xue@1:   for (int m=0; m<M; m++)
xue@1:   {
xue@1:     part=partials[m]; 
xue@1:     for (int fr=0; fr<Fr; fr++)
xue@1:     {
xue@1:       if (part[fr].f<=0){M=m; break;}
xue@1:       as[fr]=part[fr].a*2;
xue@1:     }
xue@1:     if (terminatetag && *terminatetag) {free8(xrec); free8(as); free8(a0[0]); delete[] a0; return 0;}
xue@1:     if (m>=M) break;
xue@1: 
xue@1: 		CubicSpline(Fr-1, a3[m], a2[m], a1[m], a0[m], xs, as, 1, 1);
xue@1:   }
xue@1: 
xue@1:   double *ph=(double*)malloc8(sizeof(double)*M*2), *ph0=&ph[M]; memset(ph, 0, sizeof(double)*M*2);
xue@1:   for (int fr=0; fr<Fr-1; fr++)
xue@1:   {
xue@1: //    double *la0=new double[M*4], *la1=&la0[M], *la2=&la0[M*2], *la3=&la0[M*3]; for (int m=0; m<M; m++) la0[m]=a0[m][fr], la1[m]=a1[m][fr], la2[m]=a2[m][fr], la3[m]=a3[m][fr], part[fr].p=ph[m]; Sinusoids(M, &xrec[ixs[fr]-dst], 0, ixs[fr+1]-ixs[fr], la3, la2, la1, la0, f3[fr], f2[fr], f1[fr], f0[fr], ph, true); delete[] la0;
xue@1:     for (int m=0; m<M; m++) ALIGN8(Sinusoid(&xrec[ixs[fr]-dst], 0, ixs[fr+1]-ixs[fr], a3[m][fr], a2[m][fr], a1[m][fr], a0[m][fr], (m+1)*f3[fr], (m+1)*f2[fr], (m+1)*f1[fr], (m+1)*f0[fr], ph[m], true);)
xue@1:   }
xue@1: 	for (int m=0; m<M; m++)
xue@1:   {
xue@1:     part[Fr-1].p=ph[m];
xue@1:     ALIGN8(Sinusoid(&xrec[ixs[Fr-2]-dst], ixs[Fr-1]-ixs[Fr-2], den-ixs[Fr-2], a3[m][Fr-2], a2[m][Fr-2], a1[m][Fr-2], a0[m][Fr-2], (m+1)*f3[Fr-2], (m+1)*f2[Fr-2], (m+1)*f1[Fr-2], (m+1)*f0[Fr-2], ph[m], true);
xue@1: 					 Sinusoid(&xrec[ixs[0]-dst], dst-ixs[0], 0, a3[m][0], a2[m][0], a1[m][0], a0[m][0], (m+1)*f3[0], (m+1)*f2[0], (m+1)*f1[0], (m+1)*f0[0], ph0[m], true);)
xue@1:     if (terminatetag && *terminatetag) {free8(xrec); free8(as); free8(a0[0]); delete[] a0; free8(ph); return 0;}
xue@1:   }
xue@1:   free8(as); free8(ph); free8(a0[0]); delete[] a0;
xue@1:   return xrec;
xue@1: }//SynthesisHS2
xue@1: 
Chris@5: /**
xue@1:   function SynthesisHSp: synthesizes a harmonic sinusoid with phase alignment
xue@1: 
xue@1:   In: partials[pm][pfr]: HS partials
xue@1:       startamp[pm][st_count]: onset amplifiers, optional
xue@1:       st_start, st_offst: start of and interval between onset amplifying points, optional
xue@1:   Out: [dst, den): time interval synthesized
xue@1:        xrec[den-dst]: resynthesized harmonic sinusoid
xue@1: 
xue@1:   Returns pointer to xrec. xrec is created anew with malloc8() and must be freed by caller with free8().
xue@1: */
xue@1: double* SynthesisHSp(int pm, int pfr, atom** partials, int& dst, int& den, double** startamp, int st_start, int st_offst, int st_count)
xue@1: {
xue@1:   int wid=partials[0][0].s, hwid=wid/2, offst=partials[0][1].t-partials[0][0].t;
xue@1:   double *a1=new double[pfr*13];
xue@1:   double *f1=&a1[pfr], *fa=&a1[pfr*2], *fb=&a1[pfr*3], *fc=&a1[pfr*4], *fd=&a1[pfr*5],
xue@1:          *aa=&a1[pfr*6], *ab=&a1[pfr*7], *ac=&a1[pfr*8], *ad=&a1[pfr*9], *p1=&a1[pfr*10], *xs=&a1[pfr*11];
xue@1:   int* ixs=(int*)&a1[pfr*12];
xue@1: 
xue@1:   dst=partials[0][0].t-hwid, den=partials[0][pfr-1].t+hwid;
xue@1: 	double *xrec=(double*)malloc8(sizeof(double)*(den-dst)*2), *xrecm=&xrec[den-dst];
xue@1:   memset(xrec, 0, sizeof(double)*(den-dst));
xue@1: 
xue@1:   for (int p=0; p<pm; p++)
xue@1:   {
xue@1:     atom* part=partials[p];
xue@1:     bool fzero=false;
xue@1:     for (int fr=0; fr<pfr; fr++)
xue@1:     {
xue@1:       if (part[fr].f<=0)
xue@1:       {
xue@1:         fzero=true;
xue@1:         break;
xue@1:       }
xue@1:       ixs[fr]=part[fr].t;
xue@1:       xs[fr]=part[fr].t;
xue@1:       a1[fr]=part[fr].a*2;
xue@1:       f1[fr]=part[fr].f;
xue@1:       p1[fr]=part[fr].p;
xue@1:       if (part[fr].type==atMuted) a1[fr]=0;
xue@1:     }
xue@1:     if (fzero) break;
xue@1: 
xue@1: 		CubicSpline(pfr-1, fa, fb, fc, fd, xs, f1, 1, 1);
xue@1:     CubicSpline(pfr-1, aa, ab, ac, ad, xs, a1, 1, 1);
xue@1: 
xue@1:     for (int fr=0; fr<pfr-1; fr++) Sinusoid(&xrecm[ixs[fr]-dst], 0, offst, aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], p1[fr], p1[fr+1], false);
xue@1: //    Sinusoid(&xrecm[ixs[0]-dst], -hwid, 0, aa[0], ab[0], ac[0], ad[0], fa[0], fb[0], fc[0], fd[0], p1[0], p1[1], false);
xue@1: //    Sinusoid(&xrecm[ixs[pfr-2]-dst], offst, offst+hwid, aa[pfr-2], ab[pfr-2], ac[pfr-2], ad[pfr-2], fa[pfr-2], fb[pfr-2], fc[pfr-2], fd[pfr-2], p1[pfr-2], p1[pfr-1], false);
xue@1:     Sinusoid(&xrecm[ixs[0]-dst], -hwid, 0, 0, 0, 0, ad[0], fa[0], fb[0], fc[0], fd[0], p1[0], p1[1], false);
xue@1:     Sinusoid(&xrecm[ixs[pfr-2]-dst], offst, offst+hwid, 0, 0, 0, ad[pfr-2], fa[pfr-2], fb[pfr-2], fc[pfr-2], fd[pfr-2], p1[pfr-2], p1[pfr-1], false);
xue@1: 
xue@1:     if (st_count)
xue@1:     {
xue@1:       double* amp=startamp[p];
xue@1:       for (int c=0; c<st_count; c++)
xue@1:       {
xue@1:         double a1=amp[c], a2=(c+1<st_count)?amp[c+1]:1, da=(a2-a1)/st_offst;
xue@1:         int lst=ixs[0]-dst+st_start+c*st_offst;
xue@1:         double *lxrecm=&xrecm[lst];
xue@1:         if (lst>0) for (int i=0; i<st_offst; i++) lxrecm[i]*=(a1+da*i);
xue@1:         else for (int i=-lst; i<st_offst; i++) lxrecm[i]*=(a1+da*i);
xue@1:       }
xue@1:       for (int i=0; i<ixs[0]-dst+st_start; i++) xrecm[i]*=amp[0];
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       /*
xue@1:       for (int i=0; i<=hwid; i++)
xue@1:       {
xue@1:         double tmp=0.5+0.5*cos(M_PI*i/hwid);
xue@1:         xrecm[ixs[0]-dst-i]*=tmp;
xue@1:         xrecm[ixs[pfr-2]-dst+offst+i]*=tmp;
xue@1:       } */
xue@1:     }
xue@1: 
xue@1:     for (int n=0; n<den-dst; n++) xrec[n]+=xrecm[n];
xue@1:   }
xue@1: 
xue@1:   delete[] a1;
xue@1:   return xrec;
xue@1: }//SynthesisHSp
xue@1: 
Chris@5: /**
xue@1:   function SynthesisHSp: wrapper function.
xue@1: 
xue@1:   In: HS: a harmonic sinusoid.
xue@1:   Out: [dst, den): time interval synthesized
xue@1:        xrec[den-dst]: resynthesized harmonic sinusoid
xue@1: 
xue@1:   Returns pointer to xrec, which is created anew with malloc8() and must be freed by caller using free8().
xue@1: */
xue@1: double* SynthesisHSp(THS* HS, int& dst, int& den)
xue@1: {
xue@1:   return SynthesisHSp(HS->M, HS->Fr, HS->Partials, dst, den, HS->startamp, HS->st_start, HS->st_offst, HS->st_count);
xue@1: }//SynthesisHSp
xue@1: