mirror of
https://github.com/cookiengineer/audacity
synced 2025-07-31 16:09:28 +02:00
1960 lines
47 KiB
C++
1960 lines
47 KiB
C++
#include "real.h"
|
|
#include "utils.h"
|
|
#include <math.h>
|
|
#include "sms.h"
|
|
#include "buffer.h"
|
|
#include "defs.h"
|
|
#include "limits.h"
|
|
|
|
using namespace std;
|
|
namespace _sbsms_ {
|
|
|
|
#define SMS_BUF_SIZE 4096
|
|
|
|
sms :: sms(int N, unsigned short M, unsigned short M_MAX, int res, int latency, real p, real q, real pad, int Nlo, int channels, TrackAllocator *ta, PeakAllocator *pa) {
|
|
this->N = N;
|
|
this->M = M;
|
|
Nover2 = N/2;
|
|
this->p = 1.0/p;
|
|
this->q = q;
|
|
this->res = res;
|
|
this->m = (real)M/(real)M_MAX;
|
|
this->channels = channels;
|
|
this->ta = ta;
|
|
this->pa = pa;
|
|
terminal = (M == M_MAX);
|
|
|
|
datasize = sizeof(char) + 6*sizeof(unsigned short);
|
|
pData = (char*)calloc(datasize,sizeof(char));
|
|
|
|
// Phase lock channels
|
|
bPhaseLock = 0;
|
|
// Stitch bands together
|
|
bStitch = 1;
|
|
// Threshold (relative to maximum magnitude) for finding peaks
|
|
peakThresh = .000005;
|
|
// Threshold (in trackpoint units) for starting a track
|
|
startThresh = 8.0*sqrt(pad/(real)N) * peakThresh;
|
|
// Maximum figure of merit for continuing a track (see merit)
|
|
maxMerit2 = square(.11);
|
|
// Maximum differency in frequency for continuing a track
|
|
maxDF2 = square(.08);
|
|
// Coefficient for dB magnitude changes in figure of merit for continuing a track
|
|
dMCoeff = 0.01;
|
|
// Maximum increase in dB for continuing a track
|
|
maxdBIncr = (12.0 - 6.0*m);
|
|
// Maximum difference in dB for stitching two subband tracks together
|
|
maxdBIncrStitch = (12.0 - 6.0*m);
|
|
// Maximum figure of merit for matching tracks for phase locking
|
|
maxMerit2PhaseLock = square(.01);
|
|
// Maximum differene in frequency for matching tracks for phase locking
|
|
maxDF2PhaseLock = square(.005);
|
|
// Coefficient for dB magnitude changes in figure of merit for phase locking
|
|
dMCoeffPhaseLock = 0.0005;
|
|
// Maximum figure of merit for matching peaks for band stitching
|
|
maxMerit2Match = square(.08);
|
|
// Maximum difference in frequency for matching peaks for band stitching
|
|
maxDF2Match = square(.04);
|
|
// Coefficient for dB magnitude changes in figure of merit for band stitching
|
|
dMCoeffMatch = 0.02;
|
|
// Minimum number of trackpoints in a track for synthesis
|
|
minNpts = 4;
|
|
// Maximum width of a peak for determining spectral energy
|
|
peakWidth = 2*((round2int(pad*(real)N/96.0)+1)/2);
|
|
// Beginning and end of band
|
|
if(terminal) kStart = 1;
|
|
else kStart = N/4-(int)(0.5*round2int((real)peakWidth*(real)N/(real)Nlo)+6);
|
|
if(M==1) kEnd = N/2;
|
|
else kEnd = N/2-peakWidth;
|
|
// Maximum frequency for matching peaks for band stitching
|
|
maxFMatch = (0.5*kEnd+8.0)*TWOPI/(real)N;
|
|
// Minimum frequency for matching peaks for band stitching
|
|
minFMatch = (2.0*kStart-16.0)*TWOPI/(real)N;
|
|
// Minimum ratio of peak "top" to total peak energy
|
|
minPeakTopRatio = 0.05;
|
|
// Minimum ratio of low frequency resolution/high frequency resolution peaks for determining peaks
|
|
// Ratio for favoring continuing a track in local band over stitching to another band
|
|
localFavorRatio = 1.1;
|
|
// Maximum ratio of (peak energy given to other peaks) to (peak energy)
|
|
maxDecRatio = 1.5;
|
|
|
|
// Normalization from spectrum energy to peak amplitude
|
|
if(SBSMS_WINDOW == SBSMS_HANN)
|
|
mNorm = 1.0/6.0 * pad * square(8.0 / (real)(N));
|
|
else if(SBSMS_WINDOW == SBSMS_HAMMING)
|
|
mNorm = 0.18141196440283 * pad * square(8.0 / (real)(N));
|
|
else
|
|
abort();
|
|
|
|
// maximum magnitude (for determining peaks)
|
|
magmax = 0;
|
|
|
|
assigntime[0] = 0;
|
|
assigntime[1] = 0;
|
|
marktime[0] = 0;
|
|
marktime[1] = 0;
|
|
currtime = 0;
|
|
synthtime = 0;
|
|
|
|
trax = (list<track*>**)calloc(2,sizeof(list<track*>*));
|
|
trax[0] = new list<track*>;
|
|
trax[1] = new list<track*>;
|
|
peak1 = (real*)calloc(2*N,sizeof(real));
|
|
dec = (real*)calloc(N,sizeof(real));
|
|
mag0[0] = (real*)calloc(N,sizeof(real));
|
|
mag0[1] = (real*)calloc(N,sizeof(real));
|
|
mag1[0] = (real*)calloc(N,sizeof(real));
|
|
mag1[1] = (real*)calloc(N,sizeof(real));
|
|
mag2[0] = (real*)calloc(N,sizeof(real));
|
|
mag2[1] = (real*)calloc(N,sizeof(real));
|
|
trackPointListBuffer[0] = new TrackPointListBuffer();
|
|
trackPointListBuffer[1] = new TrackPointListBuffer();
|
|
|
|
h2cum = 0;
|
|
sines2 = new SampleBuf(0);
|
|
outMixer = sines2;
|
|
|
|
x0[0] = grain :: create(N,1,SBSMS_WINDOW);
|
|
x0[1] = grain :: create(N,1,SBSMS_WINDOW);
|
|
|
|
x1[0] = grain :: create(N,1,SBSMS_WINDOW);
|
|
x1[1] = grain :: create(N,1,SBSMS_WINDOW);
|
|
|
|
x2[0] = grain :: create(N,1,SBSMS_WINDOW);
|
|
x2[1] = grain :: create(N,1,SBSMS_WINDOW);
|
|
|
|
bPeakSet = false;
|
|
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_init(&dataMutex,NULL);
|
|
pthread_mutex_init(&tplbMutex[0],NULL);
|
|
pthread_mutex_init(&tplbMutex[1],NULL);
|
|
pthread_mutex_init(&bufferMutex,NULL);
|
|
pthread_mutex_init(&trackMutex[0],NULL);
|
|
pthread_mutex_init(&trackMutex[1],NULL);
|
|
#endif
|
|
}
|
|
|
|
sms :: ~sms()
|
|
{
|
|
reset();
|
|
free(pData);
|
|
free(mag0[0]);
|
|
free(mag0[1]);
|
|
free(mag1[0]);
|
|
free(mag1[1]);
|
|
free(mag2[0]);
|
|
free(mag2[1]);
|
|
free(dec);
|
|
free(peak1);
|
|
grain :: destroy(x0[0]);
|
|
grain :: destroy(x0[1]);
|
|
grain :: destroy(x1[0]);
|
|
grain :: destroy(x1[1]);
|
|
grain :: destroy(x2[0]);
|
|
grain :: destroy(x2[1]);
|
|
delete sines2;
|
|
for(int c=0;c<channels;c++) {
|
|
for(long k=trackPointListBuffer[c]->readPos; k<trackPointListBuffer[c]->writePos; k++) {
|
|
tplist *tpl = trackPointListBuffer[c]->read(k);
|
|
for(tplist::iterator tpi = tpl->begin();
|
|
tpi != tpl->end();
|
|
tpi++) {
|
|
delete (*tpi);
|
|
}
|
|
}
|
|
}
|
|
delete trackPointListBuffer[0];
|
|
delete trackPointListBuffer[1];
|
|
delete trax[0];
|
|
delete trax[1];
|
|
free(trax);
|
|
}
|
|
|
|
void sms :: reset()
|
|
{
|
|
assigntime[0] = 0;
|
|
assigntime[1] = 0;
|
|
marktime[0] = 0;
|
|
marktime[1] = 0;
|
|
currtime = 0;
|
|
synthtime = 0;
|
|
h2cum = 0;
|
|
samplePos = 0;
|
|
|
|
for(int c=0;c<channels;c++) {
|
|
for(list<track*>::iterator tt=trax[c]->begin();
|
|
tt != trax[c]->end();
|
|
) {
|
|
track *t = (*tt);
|
|
list<track*>::iterator eraseMe = tt;
|
|
tt++;
|
|
trax[c]->erase(eraseMe);
|
|
ta->destroy(t);
|
|
}
|
|
}
|
|
|
|
hSynth.clear();
|
|
hAssign[0].clear();
|
|
hAssign[1].clear();
|
|
sines2->clear();
|
|
}
|
|
|
|
|
|
//1/65535
|
|
#define LOG_MIN (-11.09033963005365f)
|
|
//4
|
|
#define LOG_MAX 1.38629436111989f
|
|
|
|
unsigned short sms :: encodeRealLog(real x)
|
|
{
|
|
x = log(x);
|
|
if(x<LOG_MIN) return 0;
|
|
else if(x>LOG_MAX) return USHRT_MAX;
|
|
else return (unsigned short)round2int((real)USHRT_MAX*(x-LOG_MIN)/(LOG_MAX-LOG_MIN));
|
|
}
|
|
|
|
real sms :: decodeRealLog(unsigned short x)
|
|
{
|
|
return exp(LOG_MIN + (LOG_MAX-LOG_MIN)*(real)x/(real)USHRT_MAX);
|
|
}
|
|
|
|
unsigned short sms :: encodeReal(real x, real min, real max)
|
|
{
|
|
if(x<min) return 0;
|
|
else if(x>max) return USHRT_MAX;
|
|
else return (unsigned short)round2int((real)USHRT_MAX*(x-min)/(max-min));
|
|
}
|
|
|
|
real sms :: decodeReal(unsigned short x, real min, real max)
|
|
{
|
|
return min + (max-min)*(real)x/(real)USHRT_MAX;
|
|
}
|
|
|
|
void sms :: readTrackPointsFromFile(FILE *fp)
|
|
{
|
|
unsigned short h1;
|
|
char *data = pData;
|
|
fread(data,sizeof(unsigned short)+sizeof(char),1,fp);
|
|
h1 = *((unsigned short*)data); data += sizeof(unsigned short);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&dataMutex);
|
|
#endif
|
|
hSynth.write(h1);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&dataMutex);
|
|
#endif
|
|
char ch = *((char*)data);
|
|
channels = ch;
|
|
for(int c=0;c<channels;c++) {
|
|
unsigned short nTracks = 0;
|
|
fread(&nTracks,sizeof(unsigned short),1,fp);
|
|
for(int k=0;k<nTracks;k++) {
|
|
unsigned short index;
|
|
unsigned short precursorIndex;
|
|
char flags;
|
|
bool tailStart;
|
|
bool tailEnd;
|
|
bool bStart;
|
|
bool bEnd;
|
|
tpoint *tp = new tpoint();
|
|
|
|
char *data = pData;
|
|
fread(data,datasize,1,fp);
|
|
flags = *((char*)data); data += sizeof(char);
|
|
index = *((unsigned short*)data); data += sizeof(unsigned short);
|
|
precursorIndex = *((unsigned short*)data); data += sizeof(unsigned short);
|
|
unsigned short f;
|
|
unsigned short y;
|
|
unsigned short y0;
|
|
unsigned short ph;
|
|
f = *((unsigned short*)data); data += sizeof(unsigned short);
|
|
y = *((unsigned short*)data); data += sizeof(unsigned short);
|
|
y0 = *((unsigned short*)data); data += sizeof(unsigned short);
|
|
ph = *((unsigned short*)data);
|
|
tp->f = decodeReal(f,0.0f,PI);
|
|
tp->y = decodeRealLog(y)*(real)M;
|
|
tp->y0 = decodeRealLog(y0)*(real)M;
|
|
tp->ph = decodeReal(ph,-PI,PI);
|
|
|
|
bStart = (flags & 1)?true:false;
|
|
tailStart = (flags & 2)?true:false;
|
|
bEnd = (flags & 4)?true:false;
|
|
tailEnd = (flags & 8)?true:false;
|
|
tp->h = h1;
|
|
tp->M = M;
|
|
|
|
track *t = ta->getTrack(index);
|
|
if(t && !bStart) {
|
|
t->push_back_tpoint(tp);
|
|
if(bEnd) {
|
|
t->end = assigntime[c];
|
|
}
|
|
if(tailEnd) {
|
|
t->tailEnd = true;
|
|
}
|
|
} else {
|
|
track *t1;
|
|
if(precursorIndex != USHRT_MAX) {
|
|
track *precursor = ta->getTrack(precursorIndex);
|
|
t1 = ta->create(precursor,this,res,index);
|
|
if(precursor) precursor->descendant = t1;
|
|
} else {
|
|
t1 = ta->create(NULL,this,res,index);
|
|
}
|
|
if(tailStart)
|
|
t1->tailStart = true;
|
|
t1->push_back_tpoint(tp);
|
|
t1->m_p = tp->ph;
|
|
t1->m_pDescendant = tp->ph;
|
|
t1->currtime = assigntime[c];
|
|
t1->start = assigntime[c];
|
|
addTrack(c,t1);
|
|
}
|
|
}
|
|
assigntime[c]++;
|
|
}
|
|
}
|
|
|
|
void sms :: zeroPad(long n)
|
|
{
|
|
sines2->grow(n);
|
|
sines2->writePos += n;
|
|
}
|
|
|
|
int sms :: writeTrackPointsToFile(FILE *fp)
|
|
{
|
|
int bytes = 0;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&dataMutex);
|
|
#endif
|
|
unsigned short h1 = hSynth.read(hSynth.readPos);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&dataMutex);
|
|
#endif
|
|
if(fp) {
|
|
fwrite(&h1,sizeof(unsigned short),1,fp); bytes += sizeof(unsigned short);
|
|
char c = channels;
|
|
fwrite(&c,sizeof(char),1,fp); bytes += sizeof(char);
|
|
for(int c=0;c<channels;c++) {
|
|
unsigned short nTracks = 0;
|
|
long start = ftell(fp);
|
|
fwrite(&nTracks,sizeof(unsigned short),1,fp); bytes += sizeof(unsigned short);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&trackMutex[c]);
|
|
#endif
|
|
for(list<track*>::iterator tt=trax[c]->begin();
|
|
tt != trax[c]->end();
|
|
) {
|
|
track *t = (*tt);
|
|
if(t->end < synthtime) {
|
|
if(!t->descendant||t->descendant->isDone()) {
|
|
list<track*>::iterator eraseMe = tt;
|
|
tt++;
|
|
trax[c]->erase(eraseMe);
|
|
ta->destroy(t);
|
|
} else {
|
|
tt++;
|
|
}
|
|
} else if(synthtime >= t->start) {
|
|
track *t = (*tt);
|
|
t->currtime = synthtime;
|
|
trackpoint *tp = t->getTrackPoint(synthtime);
|
|
if(tp) {
|
|
char flags = 0;
|
|
bool bStart;
|
|
bool bEnd;
|
|
bool tailStart;
|
|
bool tailEnd;
|
|
if(t->isStart(synthtime))
|
|
bStart = true;
|
|
else
|
|
bStart = false;
|
|
if(t->tailStart && t->isStart(synthtime))
|
|
tailStart = true;
|
|
else
|
|
tailStart = false;
|
|
if(t->isEnd(synthtime))
|
|
bEnd = true;
|
|
else
|
|
bEnd = false;
|
|
if(t->tailEnd && t->isEnd(synthtime))
|
|
tailEnd = true;
|
|
else
|
|
tailEnd = false;
|
|
|
|
if(bStart)
|
|
flags += 1;
|
|
if(tailStart)
|
|
flags += 2;
|
|
if(bEnd)
|
|
flags += 4;
|
|
if(tailEnd)
|
|
flags += 8;
|
|
|
|
fwrite(&flags,sizeof(char),1,fp); bytes += sizeof(char);
|
|
fwrite(&(t->index),sizeof(unsigned short),1,fp); bytes += sizeof(unsigned short);
|
|
if(t->precursor) {
|
|
fwrite(&(t->precursor->index),sizeof(unsigned short),1,fp); bytes += sizeof(unsigned short);
|
|
} else {
|
|
unsigned short none = USHRT_MAX;
|
|
fwrite(&none,sizeof(unsigned short),1,fp); bytes += sizeof(unsigned short);
|
|
}
|
|
unsigned short f = encodeReal(tp->f,0.0f,PI);
|
|
unsigned short y = encodeRealLog(tp->y/(real)M);
|
|
unsigned short y0 = encodeRealLog(tp->y0/(real)M);
|
|
unsigned short ph = encodeReal(tp->ph,-PI,PI);
|
|
fwrite(&f,sizeof(unsigned short),1,fp); bytes += sizeof(unsigned short);
|
|
fwrite(&y,sizeof(unsigned short),1,fp); bytes += sizeof(unsigned short);
|
|
fwrite(&y0,sizeof(unsigned short),1,fp); bytes += sizeof(unsigned short);
|
|
fwrite(&ph,sizeof(unsigned short),1,fp); bytes += sizeof(unsigned short);
|
|
nTracks++;
|
|
}
|
|
tt++;
|
|
} else {
|
|
break;
|
|
}
|
|
}
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&trackMutex[c]);
|
|
#endif
|
|
long end = ftell(fp);
|
|
fseek(fp,start,SEEK_SET);
|
|
fwrite(&nTracks,sizeof(unsigned short),1,fp);
|
|
fseek(fp,end,SEEK_SET);
|
|
}
|
|
}
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&dataMutex);
|
|
#endif
|
|
hSynth.advance(1);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&dataMutex);
|
|
#endif
|
|
synthtime++;
|
|
return bytes;
|
|
}
|
|
|
|
void sms :: setH(unsigned short h)
|
|
{
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&dataMutex);
|
|
#endif
|
|
hAssign[0].write(h);
|
|
hAssign[1].write(h);
|
|
hSynth.write(h);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&dataMutex);
|
|
#endif
|
|
}
|
|
|
|
long sms :: addTrackPoints(grain *g0, grain *g1, grain *g2)
|
|
{
|
|
// set magnitude profile of signel constant frequency peak
|
|
if(!bPeakSet) {
|
|
for(int k=-Nover2;k<=Nover2;k++) {
|
|
peak1[k+N] = norm2(g1->peak[(k+N)%N]);
|
|
}
|
|
bPeakSet = true;
|
|
}
|
|
|
|
setH(g1->h);
|
|
// split channels
|
|
c2evenodd(g0, x0[0], x0[1]);
|
|
c2evenodd(g1, x1[0], x1[1]);
|
|
c2evenodd(g2, x2[0], x2[1]);
|
|
|
|
for(int c=0;c<channels;c++) {
|
|
calcmags(mag0[c], x0[c],p);
|
|
calcmags(mag1[c], x1[c],1);
|
|
calcmags(mag2[c], x2[c],q);
|
|
extractTrackPoints(x1[c],mag0[c],mag1[c],mag2[c],currtime,trackPointListBuffer[c],g1->h,c);
|
|
}
|
|
currtime++;
|
|
return 1;
|
|
}
|
|
|
|
real sms :: merit(trackpoint *tp0, trackpoint *tp1, real m0, real m1, real dti2, real dMCoeff, real *df, real maxDF2)
|
|
{
|
|
(*df) = m1*tp1->f - m0*tp0->f;
|
|
real df2 = square(*df);
|
|
if(df2 > maxDF2) return df2;
|
|
real dM = dBApprox((m1*tp1->y)/(m0*tp0->y));
|
|
return (df2+square(dMCoeff*dM));
|
|
}
|
|
|
|
bool sms :: nearestTrackPoint(tplist *tpl, trackpoint *tp0, real m0, real m1, real dti2, tplist::iterator *minpos, real *minMerit2, real maxMerit2, real maxDF2)
|
|
{
|
|
(*minMerit2) = 1e9;
|
|
if(tpl == NULL) return false;
|
|
bool bSet = false;
|
|
|
|
for(tplist::iterator tpi=tpl->begin();
|
|
tpi!=tpl->end();
|
|
++tpi) {
|
|
|
|
trackpoint *tp1 = (*tpi);
|
|
real df;
|
|
real Merit2 = merit(tp0,tp1,m0,m1,dti2,dMCoeff,&df,maxDF2);
|
|
if(m0!=m1) Merit2*=localFavorRatio;
|
|
if(Merit2 < (*minMerit2) && Merit2 < maxMerit2 && square(df) < maxDF2) {
|
|
(*minMerit2) = Merit2;
|
|
(*minpos) = tpi;
|
|
bSet = true;
|
|
}
|
|
}
|
|
return bSet;
|
|
}
|
|
|
|
bool sms :: contTrack(track *t, trackpoint *tp, int c)
|
|
{
|
|
bool allowCont = true;
|
|
trackpoint *last = t->back();
|
|
real dbIncr = dBApprox(tp->y/last->y);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&trackMutex[c]);
|
|
#endif
|
|
if(dbIncr > maxdBIncr) {
|
|
tp->y0 = last->y;
|
|
t->push_back(tp);
|
|
} else {
|
|
t->push_back(tp);
|
|
}
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&trackMutex[c]);
|
|
#endif
|
|
return allowCont;
|
|
}
|
|
|
|
bool sms :: adoptTrack(track *precursor,
|
|
sms *lender,
|
|
trackpoint *tp,
|
|
real m,
|
|
real dt,
|
|
int c)
|
|
{
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&lender->trackMutex[c]);
|
|
#endif
|
|
trackpoint *last = precursor->back();
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&lender->trackMutex[c]);
|
|
#endif
|
|
real dbIncr = dBApprox((m*tp->y)/last->y);
|
|
|
|
bool bDiscont;
|
|
if(dbIncr > maxdBIncrStitch) {
|
|
bDiscont = true;
|
|
} else {
|
|
bDiscont = false;
|
|
}
|
|
if(tp->M>last->M) {
|
|
if(dt==1.0) {
|
|
if(lender->res==2) {
|
|
trackpoint *tp0 = new trackpoint();
|
|
tp0->h = tp->h*2/lender->res;
|
|
tp0->y = 0.5*tp->y;
|
|
if(bDiscont)
|
|
tp0->y0 = last->y;
|
|
else
|
|
tp0->y0 = tp0->y;
|
|
tp0->f = 0.5*tp->f;
|
|
tp0->ph = tp->ph;
|
|
tp0->time = last->time+1;
|
|
tp0->M = last->M;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&lender->trackMutex[c]);
|
|
#endif
|
|
precursor->push_back(tp0);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&lender->trackMutex[c]);
|
|
#endif
|
|
if(!tp->owner)
|
|
tp->bDelete = true;
|
|
} else {
|
|
trackpoint *tpend = new trackpoint();
|
|
tpend->h = tp->h*2/lender->res;
|
|
tpend->y = 0;
|
|
tpend->y0 = 0;
|
|
tpend->f = 0.5*tp->f;
|
|
tpend->ph = tp->ph;
|
|
tpend->time = last->time+1;
|
|
tpend->M = last->M;
|
|
|
|
trackpoint *tp0 = new trackpoint();
|
|
tp0->h = last->h*lender->res/2;
|
|
tp0->y = 0;
|
|
tp0->y0 = 0;
|
|
tp0->f = last->f*2.0;
|
|
tp0->ph = last->ph;
|
|
tp0->time = last->time/lender->res;
|
|
tp0->M = M;
|
|
track *t = ta->create(precursor,this,res);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&lender->trackMutex[c]);
|
|
#endif
|
|
precursor->push_back(tpend);
|
|
precursor->endTrack(false);
|
|
precursor->descendant = t;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&lender->trackMutex[c]);
|
|
#endif
|
|
t->startTrack(tp0,false);
|
|
if(bDiscont)
|
|
tp->y0 = 2.0*last->y;
|
|
t->push_back(tp);
|
|
addTrack(c,t);
|
|
}
|
|
} else if(dt==2.0) {
|
|
track *t = ta->create(precursor,this,res);
|
|
trackpoint *tpend0 = new trackpoint();
|
|
trackpoint *tpend1 = new trackpoint();
|
|
trackpoint *tp0 = new trackpoint();
|
|
|
|
if(bDiscont)
|
|
tp->y0 = 2.0*last->y;
|
|
|
|
tp0->h = (last->h*lender->res)/2;
|
|
tp0->y = 0;
|
|
tp0->y0 = 0;
|
|
tp0->f = last->f*2.0f;
|
|
tp0->ph = last->ph;
|
|
tp0->M = M;
|
|
tp0->time = last->time/lender->res;
|
|
|
|
tpend0->h = last->h;
|
|
tpend0->y = 0.5f*last->y;
|
|
tpend0->y0 = tpend0->y;
|
|
tpend0->f = 0.5f*(last->f+0.5f*tp->f);
|
|
tpend0->ph = canon(last->ph + 0.5f*(last->f+tpend0->f)*last->h);
|
|
tpend0->time = last->time+1;
|
|
tpend0->M = last->M;
|
|
|
|
tpend1->h = (tp->h*2)/lender->res;
|
|
tpend1->y = 0;
|
|
tpend1->y0 = 0;
|
|
tpend1->f = 0.5f*tp->f;
|
|
tpend1->ph = tp->ph;
|
|
tpend1->time = last->time+2;
|
|
tpend1->M = last->M;
|
|
|
|
t->startTrack(tp0,false);
|
|
t->push_back(tp);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&lender->trackMutex[c]);
|
|
#endif
|
|
precursor->push_back(tpend0);
|
|
precursor->push_back(tpend1);
|
|
precursor->endTrack(false);
|
|
precursor->descendant = t;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&lender->trackMutex[c]);
|
|
#endif
|
|
addTrack(c,t);
|
|
} else {
|
|
abort();
|
|
}
|
|
} else {
|
|
track *t = ta->create(precursor,this,res);
|
|
|
|
if(bDiscont)
|
|
tp->y0 = 0.5f*last->y;
|
|
|
|
trackpoint *tp0 = new trackpoint();
|
|
trackpoint *tpend = new trackpoint();
|
|
|
|
tp0->h = (last->h*2)/res;
|
|
tp0->y = 0;
|
|
tp0->y0 = 0;
|
|
tp0->f = 0.5f*last->f;
|
|
tp0->ph = last->ph;
|
|
tp0->time = last->time*res;
|
|
tp0->M = M;
|
|
|
|
tpend->h = (tp->h*res)/2;
|
|
tpend->y = 0;
|
|
tpend->y0 = 0;
|
|
tpend->f = tp->f*2.0f;
|
|
tpend->time = last->time+1;
|
|
tpend->ph = tp->ph;
|
|
tpend->M = last->M;
|
|
|
|
t->startTrack(tp0,false);
|
|
if(res==2) {
|
|
trackpoint *tp1 = new trackpoint();
|
|
tp1->h = (last->h*2)/res;
|
|
tp1->y = 0.5f*tp->y0;
|
|
tp1->y0 = tp1->y;
|
|
tp1->f = 0.5f*(0.5f*last->f+tp->f);
|
|
tp1->ph = canon(tp0->ph + 0.5f*(tp0->f + tp1->f)*last->h);
|
|
tp1->time = last->time*res+1;
|
|
tp1->M = M;
|
|
t->push_back(tp1);
|
|
}
|
|
t->push_back(tp);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&lender->trackMutex[c]);
|
|
#endif
|
|
precursor->push_back(tpend);
|
|
precursor->endTrack(false);
|
|
precursor->descendant = t;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&lender->trackMutex[c]);
|
|
#endif
|
|
addTrack(c,t);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void sms :: markDuplicatesLo(long offset, sms *lo, long offsetlo, int c)
|
|
{
|
|
if(!lo) return;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&dataMutex);
|
|
#endif
|
|
unsigned short ha = hAssign[c].read(hAssign[c].readPos);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&dataMutex);
|
|
#endif
|
|
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&lo->tplbMutex[c]);
|
|
#endif
|
|
tplist *trackPointsL1 =
|
|
lo->trackPointListBuffer[c]->read(lo->trackPointListBuffer[c]->readPos+offset/res+offsetlo);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&lo->tplbMutex[c]);
|
|
#endif
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&tplbMutex[c]);
|
|
#endif
|
|
tplist *trackPointsM1 =
|
|
trackPointListBuffer[c]->read(trackPointListBuffer[c]->readPos+offset);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&tplbMutex[c]);
|
|
#endif
|
|
|
|
tplist trackPoints0;
|
|
|
|
for(tplist::iterator tpi=trackPointsM1->begin();
|
|
tpi!=trackPointsM1->end();
|
|
tpi++) {
|
|
|
|
if((*tpi)->f > maxFMatch) break;
|
|
trackPoints0.push_back((*tpi));
|
|
}
|
|
|
|
bool bDone = false;
|
|
bool bLastDitch = false;
|
|
|
|
while(!bDone) {
|
|
int nToCont = 0;
|
|
int nCont = 0;
|
|
|
|
for(tplist::iterator tpi = trackPoints0.begin();
|
|
tpi!=trackPoints0.end();
|
|
tpi++) {
|
|
real F;
|
|
tplist::iterator minL0; bool minL0Set = nearestTrackPoint(trackPointsL1,*tpi,1.0,0.5,1.0,&minL0,&F,maxMerit2Match, maxDF2Match);
|
|
if(minL0Set) {
|
|
nToCont++;
|
|
(*tpi)->cont = (*minL0);
|
|
} else {
|
|
(*tpi)->cont = NULL;
|
|
}
|
|
}
|
|
|
|
if(trackPointsL1 && bStitch) {
|
|
for(tplist::reverse_iterator tpi = trackPointsL1->rbegin();
|
|
tpi!=trackPointsL1->rend();
|
|
tpi++) {
|
|
real F;
|
|
(*tpi)->cont = NULL;
|
|
if((*tpi)->f < minFMatch) break;
|
|
tplist::iterator minL1; bool minL1Set = nearestTrackPoint(&trackPoints0,*tpi,0.5,1.0,1.0,&minL1,&F,maxMerit2Match, maxDF2Match);
|
|
if(minL1Set) (*tpi)->cont = *minL1;
|
|
}
|
|
}
|
|
|
|
for(tplist::iterator tpi = trackPoints0.begin();
|
|
tpi!=trackPoints0.end();
|
|
) {
|
|
trackpoint *tp0 = (*tpi);
|
|
trackpoint *tp1 = tp0->cont;
|
|
if(tp1 != NULL) {
|
|
if(bLastDitch || tp1->cont == tp0) {
|
|
nCont++;
|
|
bool bAlreadyMarked = false;
|
|
if(offset%res == 0) {
|
|
if(tp1->dup[1] != NULL || tp0->dup[1] != NULL)
|
|
bAlreadyMarked = true;
|
|
} else {
|
|
if(tp1->dup[2-2*offsetlo] != NULL || tp0->dup[2*offsetlo] != NULL)
|
|
bAlreadyMarked = true;
|
|
}
|
|
if(!bAlreadyMarked) {
|
|
if(offset%res == 0) {
|
|
tp1->dup[1] = tp0;
|
|
tp0->dup[1] = tp1;
|
|
} else {
|
|
tp1->dup[2-2*offsetlo] = tp0;
|
|
tp0->dup[2*offsetlo] = tp1;
|
|
}
|
|
}
|
|
tplist::iterator eraseMe = tpi;
|
|
tpi++;
|
|
trackPoints0.erase(eraseMe);
|
|
continue;
|
|
}
|
|
}
|
|
tpi++;
|
|
}
|
|
|
|
bDone = (nToCont-nCont == 0);
|
|
bLastDitch = (!bDone && nCont==0);
|
|
}
|
|
marktime[c]++;
|
|
}
|
|
|
|
void sms :: markDuplicates(long offset, sms *hi, sms *lo, int c)
|
|
{
|
|
if(offset%res==0) {
|
|
markDuplicatesLo(offset,lo,offset%res,c);
|
|
} else {
|
|
markDuplicatesLo(offset,lo,0,c);
|
|
markDuplicatesLo(offset,lo,1,c);
|
|
}
|
|
}
|
|
|
|
void sms :: addTrack(int c, track *t)
|
|
{
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&trackMutex[c]);
|
|
#endif
|
|
list<track*>::reverse_iterator i=trax[c]->rbegin();
|
|
while(i!=trax[c]->rend()) {
|
|
track *t0 = *i;
|
|
if(t->start >= t0->start) {
|
|
break;
|
|
}
|
|
i++;
|
|
}
|
|
trax[c]->insert(i.base(),t);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&trackMutex[c]);
|
|
#endif
|
|
}
|
|
|
|
void sms :: deleteTrackPoint(trackpoint *tp)
|
|
{
|
|
delete tp;
|
|
}
|
|
|
|
bool sms :: connectTrackPoints(trackpoint *tp0, trackpoint *tp1, sms *hi, sms *lo, real dtlo, int c)
|
|
{
|
|
bool isConn;
|
|
bool allowCont;
|
|
if(tp0->M == tp1->M) {
|
|
allowCont = contTrack(tp0->owner, tp1, c);
|
|
isConn = allowCont;
|
|
} else if(tp0->M < tp1->M) {
|
|
isConn = true;
|
|
allowCont = lo->adoptTrack(tp0->owner, this, tp1, 0.5, dtlo, c);
|
|
} else {
|
|
isConn = true;
|
|
allowCont = hi->adoptTrack(tp0->owner, this, tp1, 2.0, 1.0, c);
|
|
}
|
|
tp0->bConnected = true;
|
|
if(isConn) {
|
|
tp1->bConnected = true;
|
|
if(tp0->dupcont != NULL) {
|
|
trackpoint *dup = tp0->dupcont;
|
|
dup->bConnected = true;
|
|
if(!dup->owner)
|
|
dup->bDelete = true;
|
|
}
|
|
|
|
trackpoint *dup2 = tp0->dup[2];
|
|
if(dup2 && dup2 != tp1 && !dup2->owner) {
|
|
dup2->bDelete = true;
|
|
}
|
|
|
|
for(int d=0;d<3;d++) {
|
|
if(tp1->dup[d] && !tp1->dup[d]->owner && (d<2 || tp1->dup[d]->M < tp1->M)) {
|
|
tp1->dup[d]->bDelete = true;
|
|
}
|
|
}
|
|
}
|
|
return allowCont;
|
|
}
|
|
|
|
void sms :: advanceTrackPoints(int c)
|
|
{
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&tplbMutex[c]);
|
|
#endif
|
|
trackPointListBuffer[c]->advance(1);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&tplbMutex[c]);
|
|
#endif
|
|
}
|
|
|
|
long sms :: assignTrackPoints(long offset, sms *hi, sms *lo, int c)
|
|
{
|
|
if(offset%res == 0)
|
|
return assignTrackPoints_(offset,hi,lo,1.0,0,c);
|
|
else
|
|
return assignTrackPoints_(offset,hi,lo,2.0,1,c);
|
|
}
|
|
|
|
long sms :: assignTrackPoints_(long offset, sms *hi, sms *lo, real dtlo, long offsetlo, int c)
|
|
{
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&dataMutex);
|
|
#endif
|
|
unsigned short ha = hAssign[c].read(hAssign[c].readPos);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&dataMutex);
|
|
#endif
|
|
|
|
#ifdef MULTITHREADED
|
|
if(hi) pthread_mutex_lock(&hi->tplbMutex[c]);
|
|
#endif
|
|
tplist *trackPointsH1 =
|
|
hi?hi->trackPointListBuffer[c]->read(hi->trackPointListBuffer[c]->readPos+hi->res*offset):NULL;
|
|
#ifdef MULTITHREADED
|
|
if(hi) pthread_mutex_unlock(&hi->tplbMutex[c]);
|
|
#endif
|
|
|
|
#ifdef MULTITHREADED
|
|
if(lo) pthread_mutex_lock(&lo->tplbMutex[c]);
|
|
#endif
|
|
tplist *trackPointsL1 =
|
|
lo?lo->trackPointListBuffer[c]->read(lo->trackPointListBuffer[c]->readPos+offset/res+offsetlo):NULL;
|
|
#ifdef MULTITHREADED
|
|
if(lo) pthread_mutex_unlock(&lo->tplbMutex[c]);
|
|
#endif
|
|
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&tplbMutex[c]);
|
|
#endif
|
|
tplist *trackPointsM1 =
|
|
trackPointListBuffer[c]->read(trackPointListBuffer[c]->readPos+offset);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&tplbMutex[c]);
|
|
#endif
|
|
|
|
tplist trackPoints0;
|
|
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&trackMutex[c]);
|
|
#endif
|
|
for(list<track*>::iterator tt=trax[c]->begin();
|
|
tt != trax[c]->end(); ) {
|
|
track *t = (*tt);
|
|
t->bEnd = true;
|
|
if(t->isEnded() || t->back()->time+1 != assigntime[c]) { t->bEnd = false; tt++; continue; }
|
|
trackPoints0.push_back(t->back());
|
|
tt++;
|
|
}
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&trackMutex[c]);
|
|
#endif
|
|
|
|
for(tplist::iterator tpi=trackPointsM1->begin();
|
|
tpi!=trackPointsM1->end();
|
|
) {
|
|
trackpoint *tp = (*tpi);
|
|
if(tp->bDelete) {
|
|
tplist::iterator eraseMe = tpi;
|
|
tpi++;
|
|
trackPointsM1->erase(eraseMe);
|
|
deleteTrackPoint(tp);
|
|
} else {
|
|
tpi++;
|
|
}
|
|
}
|
|
|
|
if(trackPointsL1) {
|
|
for(tplist::iterator tpi=trackPointsL1->begin();
|
|
tpi!=trackPointsL1->end();
|
|
) {
|
|
trackpoint *tp = (*tpi);
|
|
if(tp->bDelete) {
|
|
tplist::iterator eraseMe = tpi;
|
|
tpi++;
|
|
trackPointsL1->erase(eraseMe);
|
|
deleteTrackPoint(tp);
|
|
} else {
|
|
tpi++;
|
|
}
|
|
}
|
|
}
|
|
|
|
if(trackPointsH1) {
|
|
for(tplist::iterator tpi=trackPointsH1->begin();
|
|
tpi!=trackPointsH1->end();
|
|
) {
|
|
trackpoint *tp = (*tpi);
|
|
if(tp->bDelete) {
|
|
tplist::iterator eraseMe = tpi;
|
|
tpi++;
|
|
trackPointsH1->erase(eraseMe);
|
|
deleteTrackPoint(tp);
|
|
} else {
|
|
tpi++;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool bDone = false;
|
|
bool bLastDitch = false;
|
|
|
|
while(!bDone) {
|
|
int nToCont = 0;
|
|
int nCont = 0;
|
|
|
|
for(tplist::iterator tpi=trackPointsM1->begin();
|
|
tpi!=trackPointsM1->end();
|
|
tpi++) {
|
|
real F;
|
|
(*tpi)->bConnect = false;
|
|
(*tpi)->bConnected = false;
|
|
tplist::iterator minM1; bool minM1Set = nearestTrackPoint(&trackPoints0,*tpi,1.0,1.0,1.0,&minM1,&F,maxMerit2,maxDF2);
|
|
if(minM1Set) (*tpi)->cont = *minM1;
|
|
else (*tpi)->cont = NULL;
|
|
}
|
|
|
|
if(trackPointsL1 && bStitch) {
|
|
for(tplist::reverse_iterator tpi = trackPointsL1->rbegin();
|
|
tpi!=trackPointsL1->rend();
|
|
tpi++) {
|
|
(*tpi)->cont = NULL;
|
|
(*tpi)->bConnect = false;
|
|
(*tpi)->bConnected = false;
|
|
if((*tpi)->f < minFMatch) break;
|
|
real F;
|
|
tplist::iterator minL1; bool minL1Set = nearestTrackPoint(&trackPoints0,*tpi,0.5,1.0,square(1.0/dtlo),&minL1,&F,maxMerit2,maxDF2);
|
|
if(minL1Set) (*tpi)->cont = *minL1;
|
|
}
|
|
}
|
|
|
|
if(trackPointsH1 && bStitch) {
|
|
for(tplist::iterator tpi = trackPointsH1->begin();
|
|
tpi!=trackPointsH1->end();
|
|
tpi++) {
|
|
(*tpi)->cont = NULL;
|
|
(*tpi)->bConnect = false;
|
|
(*tpi)->bConnected = false;
|
|
real F;
|
|
if((*tpi)->f < maxFMatch) {
|
|
tplist::iterator minH1; bool minH1Set = nearestTrackPoint(&trackPoints0,*tpi,2.0,1.0,1.0,&minH1,&F,maxMerit2,maxDF2);
|
|
if(minH1Set) (*tpi)->cont = *minH1;
|
|
}
|
|
}
|
|
}
|
|
|
|
for(tplist::iterator tpi = trackPoints0.begin();
|
|
tpi!=trackPoints0.end();
|
|
tpi++) {
|
|
|
|
(*tpi)->dupcont = NULL;
|
|
(*tpi)->bConnected = false;
|
|
real FM0, FL0, FH0;
|
|
tplist::iterator minM0; bool minM0Set = nearestTrackPoint(trackPointsM1,*tpi,1.0,1.0,1.0,&minM0,&FM0,maxMerit2,maxDF2);
|
|
tplist::iterator minL0; bool minL0Set = nearestTrackPoint(trackPointsL1,*tpi,1.0,0.5,square(1.0/dtlo),&minL0,&FL0,maxMerit2,maxDF2);
|
|
tplist::iterator minH0; bool minH0Set = nearestTrackPoint(trackPointsH1,*tpi,1.0,2.0,1.0,&minH0,&FH0,maxMerit2,maxDF2);
|
|
|
|
if(minM0Set &&
|
|
(!bStitch
|
|
||(FM0<=FH0 && FM0<=FL0)
|
|
||(minL0Set && FL0<=FH0 && FL0<=FM0 && (*minL0)->dup[1-offsetlo] == (*minM0))
|
|
||(minH0Set && FH0<=FL0 && FH0<=FM0 && (*minH0)->dup[1] == (*minM0)))
|
|
) {
|
|
if(offsetlo == 0 && minL0Set && (*minL0)->dup[1] == (*minM0)) {
|
|
(*tpi)->dupcont = *minL0;
|
|
} else if(minH0Set && (*minH0)->dup[1] == (*minM0)) {
|
|
(*tpi)->dupcont = *minH0;
|
|
}
|
|
(*tpi)->contF = FM0;
|
|
(*tpi)->cont = *minM0;
|
|
nToCont++;
|
|
} else if(bStitch && minL0Set && FL0<=FM0 && FL0<=FH0) {
|
|
if(minM0Set && (*minL0)->dup[1-offsetlo] == (*minM0)) {
|
|
(*tpi)->dupcont = *minM0;
|
|
}
|
|
(*tpi)->contF = FL0;
|
|
(*tpi)->cont = *minL0;
|
|
nToCont++;
|
|
} else if(bStitch && minH0Set && FH0<=FM0 && FH0<=FL0) {
|
|
if(minM0Set && (*minH0)->dup[1] == (*minM0)) {
|
|
(*tpi)->dupcont = *minM0;
|
|
}
|
|
(*tpi)->contF = FH0;
|
|
(*tpi)->cont = *minH0;
|
|
nToCont++;
|
|
} else {
|
|
(*tpi)->cont = NULL;
|
|
}
|
|
}
|
|
|
|
// Nominal connections - may be conflicts to be resolved
|
|
for(tplist::iterator tpi = trackPoints0.begin();
|
|
tpi!=trackPoints0.end();
|
|
tpi++
|
|
) {
|
|
trackpoint *tp0 = (*tpi);
|
|
trackpoint *tp1 = tp0->cont;
|
|
if(tp1 != NULL) {
|
|
if(bLastDitch || (tp1->cont == tp0)) {
|
|
tp1->bConnect = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Make connections and resolve conflicts between duplicates and between last ditch connections
|
|
for(tplist::iterator tpi = trackPoints0.begin();
|
|
tpi!=trackPoints0.end();
|
|
) {
|
|
|
|
trackpoint *tp0 = (*tpi);
|
|
trackpoint *tp1 = tp0->cont;
|
|
|
|
if(tp0->bDelete) {
|
|
tplist::iterator eraseMe = tpi;
|
|
tpi++;
|
|
trackPoints0.erase(eraseMe);
|
|
continue;
|
|
}
|
|
if(tp1 != NULL && tp1->bConnect) {
|
|
if(tp0->dupcont != NULL && tp0->dupcont->bConnect) {
|
|
if(!tp1->bConnected && !tp0->dupcont->bConnected) {
|
|
if(!tp0->bConnected && !tp1->bDelete && (tp0->dupcont->cont == NULL || tp0->contF <= tp0->dupcont->cont->contF)) {
|
|
nCont++;
|
|
if(connectTrackPoints(tp0,tp1,hi,lo,dtlo,c))
|
|
tp0->owner->bEnd = false;
|
|
tp0->dupcont->bConnect = false;
|
|
} else if(tp0->dupcont->cont != NULL && !tp0->dupcont->cont->bConnected && !tp0->dupcont->bDelete && !tp0->dupcont->cont->bDelete) {
|
|
nCont++;
|
|
if(connectTrackPoints(tp0->dupcont->cont,tp0->dupcont,hi,lo,dtlo,c))
|
|
tp0->dupcont->cont->owner->bEnd = false;
|
|
tp1->bConnect = false;
|
|
}
|
|
}
|
|
} else if(!tp0->bConnected && !tp1->bConnected && !tp1->bDelete) {
|
|
nCont++;
|
|
if(connectTrackPoints(tp0,tp1,hi,lo,dtlo,c)) {
|
|
tp0->owner->bEnd = false;
|
|
}
|
|
}
|
|
}
|
|
if(tp0->bConnected || tp0->bDelete) {
|
|
tplist::iterator eraseMe = tpi;
|
|
tpi++;
|
|
trackPoints0.erase(eraseMe);
|
|
} else {
|
|
tpi++;
|
|
}
|
|
}
|
|
|
|
for(tplist::iterator tpi=trackPointsM1->begin();
|
|
tpi!=trackPointsM1->end();
|
|
) {
|
|
trackpoint *tp = (*tpi);
|
|
if(tp->bConnected ||tp->bDelete) {
|
|
tplist::iterator eraseMe = tpi;
|
|
tpi++;
|
|
trackPointsM1->erase(eraseMe);
|
|
} else {
|
|
tpi++;
|
|
}
|
|
if(tp->bDelete) {
|
|
deleteTrackPoint(tp);
|
|
}
|
|
}
|
|
|
|
if(trackPointsL1) {
|
|
for(tplist::iterator tpi=trackPointsL1->begin();
|
|
tpi!=trackPointsL1->end();
|
|
) {
|
|
trackpoint *tp = (*tpi);
|
|
if(tp->bConnected||tp->bDelete) {
|
|
tplist::iterator eraseMe = tpi;
|
|
tpi++;
|
|
trackPointsL1->erase(eraseMe);
|
|
} else {
|
|
tpi++;
|
|
}
|
|
if(tp->bDelete) {
|
|
deleteTrackPoint(tp);
|
|
}
|
|
}
|
|
}
|
|
|
|
if(trackPointsH1) {
|
|
for(tplist::iterator tpi=trackPointsH1->begin();
|
|
tpi!=trackPointsH1->end();
|
|
) {
|
|
trackpoint *tp = (*tpi);
|
|
if(tp->bConnected || tp->bDelete) {
|
|
tplist::iterator eraseMe = tpi;
|
|
tpi++;
|
|
trackPointsH1->erase(eraseMe);
|
|
} else {
|
|
tpi++;
|
|
}
|
|
if(tp->bDelete) {
|
|
deleteTrackPoint(tp);
|
|
}
|
|
}
|
|
}
|
|
|
|
bDone = (nToCont-nCont == 0);
|
|
bLastDitch = (!bDone && nCont==0);
|
|
}
|
|
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&trackMutex[c]);
|
|
#endif
|
|
for(list<track*>::iterator tt=trax[c]->begin();
|
|
tt != trax[c]->end(); ) {
|
|
track *t = (*tt);
|
|
if(t->bEnd) {
|
|
t->endTrack(true);
|
|
}
|
|
if(t->size() < minNpts && !t->descendant) {
|
|
list<track*>::iterator eraseMe = tt;
|
|
tt++;
|
|
trax[c]->erase(eraseMe);
|
|
ta->destroy(t);
|
|
} else {
|
|
tt++;
|
|
}
|
|
}
|
|
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&trackMutex[c]);
|
|
#endif
|
|
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&dataMutex);
|
|
#endif
|
|
hAssign[c].advance(1);
|
|
assigntime[c]++;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&dataMutex);
|
|
#endif
|
|
|
|
return 1;
|
|
}
|
|
|
|
void sms :: stereoPhaseLock()
|
|
{
|
|
if(bPhaseLock && channels==2) {
|
|
// Stereo phase locking
|
|
tplist trackPoints0;
|
|
tplist trackPoints1;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&trackMutex[0]);
|
|
pthread_mutex_lock(&trackMutex[1]);
|
|
#endif
|
|
for(list<track*>::iterator tt=trax[0]->begin();
|
|
tt != trax[0]->end();
|
|
tt++) {
|
|
track *t = (*tt);
|
|
trackpoint *tp = t->getTrackPoint(assigntime[0]);
|
|
if(tp)
|
|
trackPoints0.push_back(tp);
|
|
}
|
|
for(list<track*>::iterator tt=trax[1]->begin();
|
|
tt != trax[1]->end();
|
|
tt++) {
|
|
track *t = (*tt);
|
|
trackpoint *tp = t->getTrackPoint(assigntime[1]);
|
|
if(tp)
|
|
trackPoints1.push_back(tp);
|
|
}
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&trackMutex[0]);
|
|
pthread_mutex_unlock(&trackMutex[1]);
|
|
#endif
|
|
|
|
bool bDone = false;
|
|
bool bLastDitch = false;
|
|
while(!bDone) {
|
|
int nToCont = 0;
|
|
int nCont = 0;
|
|
|
|
for(tplist::iterator tpi = trackPoints0.begin();
|
|
tpi!=trackPoints0.end();
|
|
tpi++) {
|
|
real F;
|
|
tplist::iterator min1; bool min1Set = nearestTrackPoint(&trackPoints1,*tpi,1.0,1.0,1.0,&min1,&F,maxMerit2PhaseLock,maxDF2PhaseLock);
|
|
(*tpi)->bConnected = false;
|
|
if(min1Set) {
|
|
nToCont++;
|
|
(*tpi)->cont = (*min1);
|
|
} else {
|
|
(*tpi)->cont = NULL;
|
|
}
|
|
}
|
|
|
|
for(tplist::iterator tpi = trackPoints1.begin();
|
|
tpi!=trackPoints1.end();
|
|
tpi++) {
|
|
real F;
|
|
tplist::iterator min0; bool min0Set = nearestTrackPoint(&trackPoints0,*tpi,1.0,1.0,1.0,&min0,&F,maxMerit2PhaseLock,maxDF2PhaseLock);
|
|
(*tpi)->bConnected = false;
|
|
if(min0Set) {
|
|
(*tpi)->cont = (*min0);
|
|
} else {
|
|
(*tpi)->cont = NULL;
|
|
}
|
|
}
|
|
|
|
for(tplist::iterator tpi = trackPoints0.begin();
|
|
tpi!=trackPoints0.end();
|
|
) {
|
|
|
|
trackpoint *tp0 = (*tpi);
|
|
trackpoint *tp1 = tp0->cont;
|
|
|
|
if(tp1 != NULL && (bLastDitch || tp1->cont == tp0)) {
|
|
nCont++;
|
|
phaseLock(tp0,tp1,assigntime[0]);
|
|
tp0->bConnected = true;
|
|
tp1->bConnected = true;
|
|
}
|
|
if(tp0->bConnected) {
|
|
tplist::iterator eraseMe = tpi;
|
|
tpi++;
|
|
trackPoints0.erase(eraseMe);
|
|
} else {
|
|
tpi++;
|
|
}
|
|
}
|
|
|
|
for(tplist::iterator tpi=trackPoints1.begin();
|
|
tpi!=trackPoints1.end();
|
|
) {
|
|
trackpoint *tp = (*tpi);
|
|
if(tp->bConnected) {
|
|
tplist::iterator eraseMe = tpi;
|
|
tpi++;
|
|
trackPoints1.erase(eraseMe);
|
|
} else {
|
|
tpi++;
|
|
}
|
|
}
|
|
|
|
bDone = (nToCont == nCont);
|
|
bLastDitch = (!bDone && nCont==0);
|
|
}
|
|
}
|
|
}
|
|
|
|
long sms :: startNewTracks(long offset, int c)
|
|
{
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&tplbMutex[c]);
|
|
#endif
|
|
tplist *trackPoints = trackPointListBuffer[c]->read(trackPointListBuffer[c]->readPos+offset);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&tplbMutex[c]);
|
|
#endif
|
|
|
|
for(tplist::iterator tpi=trackPoints->begin();
|
|
tpi!=trackPoints->end();
|
|
) {
|
|
trackpoint *tp = (*tpi);
|
|
|
|
if(!tp->bDelete
|
|
&&
|
|
tp->y > startThresh*sqrtmagmax) {
|
|
track *t = ta->create(NULL,this,res);
|
|
t->startTrack(tp,true);
|
|
addTrack(c,t);
|
|
for(int d=0;d<2;d++) {
|
|
if(tp->dup[d])
|
|
if(!tp->dup[d]->owner)
|
|
tp->dup[d]->bDelete = true;
|
|
}
|
|
} else {
|
|
deleteTrackPoint(tp);
|
|
}
|
|
tplist::iterator eraseMe = tpi;
|
|
tpi++;
|
|
trackPoints->erase(eraseMe);
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
void sms :: phaseLock(trackpoint *tp0, trackpoint *tp1, long time)
|
|
{
|
|
track *t0 = tp0->owner;
|
|
track *t1 = tp1->owner;
|
|
if(t0->isStart(time)) {
|
|
t0->m_p = canon(t1->m_p + tp1->ph - tp0->ph);
|
|
} else if(t1->isStart(time)) {
|
|
t1->m_p = canon(t0->m_p + tp0->ph - tp1->ph);
|
|
}
|
|
real f = 0.5*(tp1->f + tp0->f);
|
|
tp0->f = f;
|
|
tp1->f = f;
|
|
}
|
|
|
|
void sms :: synthTracks(real a)
|
|
{
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&dataMutex);
|
|
#endif
|
|
unsigned short h1 = hSynth.read(hSynth.readPos);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&dataMutex);
|
|
#endif
|
|
real h2 = (a*(real)h1);
|
|
h2cum += h2;
|
|
int h2i = round2int(h2cum);
|
|
h2cum -= h2i;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&bufferMutex);
|
|
#endif
|
|
// extra slack for long fall times
|
|
int grow = h2i<<1;
|
|
sines2->N = grow;
|
|
sines2->grow(grow);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&bufferMutex);
|
|
#endif
|
|
|
|
for(int c=0;c<channels;c++) {
|
|
list<track*> tracks;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&trackMutex[c]);
|
|
#endif
|
|
for(list<track*>::iterator tt=trax[c]->begin();
|
|
tt!=trax[c]->end(); ) {
|
|
track *t = (*tt);
|
|
if(t->end <= synthtime) {
|
|
if(!t->descendant||t->descendant->isDone()) {
|
|
list<track*>::iterator eraseMe = tt;
|
|
tt++;
|
|
trax[c]->erase(eraseMe);
|
|
ta->destroy(t);
|
|
} else {
|
|
tt++;
|
|
}
|
|
} else if(synthtime >= t->start) {
|
|
trackpoint *tp = t->getTrackPoint(synthtime);
|
|
t->synth(sines2,sines2->writePos,c,synthtime,h2i);
|
|
tt++;
|
|
} else {
|
|
break;
|
|
}
|
|
}
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&trackMutex[c]);
|
|
#endif
|
|
}
|
|
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&bufferMutex);
|
|
#endif
|
|
sines2->writePos += h2i;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&bufferMutex);
|
|
#endif
|
|
samplePos += h2i;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&dataMutex);
|
|
#endif
|
|
hSynth.advance(1);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&dataMutex);
|
|
#endif
|
|
synthtime++;
|
|
}
|
|
|
|
long sms :: n_readable()
|
|
{
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&bufferMutex);
|
|
#endif
|
|
long n = outMixer->n_readable();
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&bufferMutex);
|
|
#endif
|
|
return n;
|
|
}
|
|
|
|
long sms :: read(audio *out, long n)
|
|
{
|
|
long n_read;
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&bufferMutex);
|
|
#endif
|
|
n_read = outMixer->read(out, n);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&bufferMutex);
|
|
#endif
|
|
return n_read;
|
|
}
|
|
|
|
void sms :: advance(long n)
|
|
{
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&bufferMutex);
|
|
#endif
|
|
outMixer->advance(n);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&bufferMutex);
|
|
#endif
|
|
}
|
|
|
|
void sms :: c2evenodd(grain *eo, grain *even, grain *odd) {
|
|
_c2evenodd(eo->freq, even->freq, odd->freq, eo->N);
|
|
}
|
|
|
|
void sms :: calcmags(real *mag, grain *g, real q) {
|
|
real q2 = 1.0/q;
|
|
for(int k=0;k<=Nover2;k++) {
|
|
real m = norm2(g->freq[k])*q2;
|
|
mag[k] = m;
|
|
if(magmax < m) magmax = m;
|
|
}
|
|
sqrtmagmax = sqrt(magmax);
|
|
}
|
|
|
|
peak *sms :: makePeak(real *mag, real *mag2, int k)
|
|
{
|
|
real v0,v1,v2;
|
|
v0 = mag2[k-1];
|
|
v1 = mag2[k];
|
|
v2 = mag2[k+1];
|
|
real y1 = v1-v0;
|
|
real y2 = v2-v0;
|
|
real a = 0.5*(y2 - 2*y1);
|
|
real b = a==0?2.0:1-y1/a;
|
|
|
|
peak *p = pa->create();
|
|
p->x = k-1+0.5*b;
|
|
p->k = k;
|
|
p->y2 = v0-0.25*a*b*b;
|
|
int k0 = round2int(p->x);
|
|
real kf = k0<p->x?p->x-k0:k0-p->x;
|
|
int k1 = k0<p->x?k0+1:k0-1;
|
|
p->y = (1.0-kf)*mag[k0] + kf*mag[k1];
|
|
p->tp = NULL;
|
|
p->tp2 = NULL;
|
|
p->tn = NULL;
|
|
p->tn2 = NULL;
|
|
return p;
|
|
}
|
|
|
|
void sms :: adjustPeaks(list<peak*> &peaks,
|
|
real *mag0,
|
|
real *mag1,
|
|
real *mag2,
|
|
real *dec,
|
|
int peakWidth
|
|
) {
|
|
|
|
for(int k=0;k<N;k++) {
|
|
dec[k] = 0.0;
|
|
}
|
|
|
|
// determine troughs and remove peaks based on minPeakTopRatio
|
|
// don't determine maginitude yet
|
|
for(list<peak *>::iterator tpi = peaks.begin();
|
|
tpi != peaks.end();
|
|
) {
|
|
list<peak*>::iterator eraseMe = tpi;
|
|
peak *p = (*tpi);
|
|
|
|
peak *pp = NULL;
|
|
if(tpi!=peaks.begin()) {
|
|
pp = *(--tpi);
|
|
tpi++;
|
|
}
|
|
|
|
peak *pn = NULL;
|
|
tpi++;
|
|
if(tpi != peaks.end()) {
|
|
pn = (*tpi);
|
|
}
|
|
|
|
if(p->tp && pp && pp->tn
|
|
&&
|
|
p->x - p->tp->x > p->x - p->tp2->x
|
|
&&
|
|
pp->tn->x - pp->x > pp->tn2->x - pp->x) {
|
|
} else {
|
|
p->tp = p->tp2;
|
|
}
|
|
|
|
if(p->tn && pn && pn->tp
|
|
&&
|
|
p->tn->x - p->x > p->tn2->x - p->x
|
|
&&
|
|
pn->x - pn->tp->x > pn->x - pn->tp2->x) {
|
|
} else {
|
|
p->tn = p->tn2;
|
|
}
|
|
|
|
peak *tp = p->tp;
|
|
peak *tn = p->tn;
|
|
|
|
int k1 = p->k;
|
|
int k0 = tp->k;
|
|
real kf0;
|
|
if(k0 < k1-peakWidth) {
|
|
k0 = k1-peakWidth;
|
|
kf0 = 0;
|
|
} else {
|
|
kf0 = k0 > tp->x ? k0 - tp->x : tp->x - k0;
|
|
}
|
|
int k2 = tn->k;
|
|
real kf2;
|
|
if(k2 > k1+peakWidth) {
|
|
k2 = k1+peakWidth;
|
|
kf2 = 0;
|
|
} else {
|
|
kf2 = k2 > tn->x ? k2 - tn->x : tn->x - k2;
|
|
}
|
|
|
|
real m = 0.0f;
|
|
real mtop = 0.0f;
|
|
|
|
real mbase = mag2[k0] + mag2[k2];
|
|
if(k0 < tp->x) {
|
|
real m0 = ((mag2[k0])+(mag2[k0+1]))*(1.0-kf0);
|
|
m += m0;
|
|
mtop += max(0.0f,(m0 - mbase)*(1.0-kf0));
|
|
} else {
|
|
real m0 = ((mag2[k0])+(mag2[k0+1]));
|
|
m += m0;
|
|
mtop += max(0.0f,m0 - mbase);
|
|
m0 = ((mag2[k0-1])+(mag2[k0]))*kf0;
|
|
m += m0;
|
|
mtop += max(0.0f,(m0 - mbase)*kf0);
|
|
}
|
|
|
|
if(k2 < tn->x) {
|
|
real m0 = ((mag2[k2-1])+(mag2[k2]));
|
|
m += m0;
|
|
mtop += max(0.0,m0 - mbase);
|
|
m0 = ((mag2[k2])+(mag2[k2+1]))*kf2;
|
|
m += m0;
|
|
mtop += max(0.0f,(m0 - mbase)*kf2);
|
|
} else {
|
|
real m0 = ((mag2[k2-1])+(mag2[k2]))*(1.0-kf2);
|
|
m += m0;
|
|
mtop += max(0.0f,(m0 - mbase)*(1.0-kf2));
|
|
}
|
|
|
|
for(int k=k0+1;k<k2-1;k++) {
|
|
real m0 = ((mag2[k])+(mag2[k+1]));
|
|
m += m0;
|
|
mtop += max(0.0f,m0 - mbase);
|
|
}
|
|
|
|
if(mtop < minPeakTopRatio*m) {
|
|
peaks.erase(eraseMe);
|
|
if(pp && (!pn || pp->y > pn->y)) {
|
|
pp->tn = p->tn;
|
|
pp->tn2 = p->tn2;
|
|
} else if(pn && (!pp || pn->y > pp->y)) {
|
|
pn->tp = p->tp;
|
|
pn->tp2 = p->tp2;
|
|
}
|
|
}
|
|
}
|
|
|
|
// determine magnitude
|
|
for(list<peak *>::iterator tpi = peaks.begin();
|
|
tpi != peaks.end();
|
|
tpi++
|
|
) {
|
|
peak *p = (*tpi);
|
|
|
|
peak *tp = p->tp;
|
|
peak *tn = p->tn;
|
|
|
|
int k1 = p->k;
|
|
int ko1 = k1 > p->x ? -1 : 1;
|
|
real kf1 = k1 > p->x ? k1 - p->x : p->x - k1;
|
|
int k0 = tp->k;
|
|
real kf0;
|
|
if(k0 < k1-peakWidth) {
|
|
k0 = k1-peakWidth;
|
|
kf0 = 0;
|
|
} else {
|
|
kf0 = k0 > tp->x ? k0 - tp->x : tp->x - k0;
|
|
}
|
|
int k2 = tn->k;
|
|
real kf2;
|
|
if(k2 > k1+peakWidth) {
|
|
k2 = k1+peakWidth;
|
|
kf2 = 0;
|
|
} else {
|
|
kf2 = k2 > tn->x ? k2 - tn->x : tn->x - k2;
|
|
}
|
|
|
|
real m1 = 0;
|
|
|
|
if(k0 < tp->x) {
|
|
m1 += ((mag1[k0])+(mag1[k0+1]))*(1.0-kf0);
|
|
} else {
|
|
m1 += ((mag1[k0])+(mag1[k0+1]));
|
|
m1 += ((mag1[k0-1])+(mag1[k0]))*kf0;
|
|
}
|
|
|
|
if(k2 < tn->x) {
|
|
m1 += ((mag1[k2-1])+(mag1[k2]));
|
|
m1 += ((mag1[k2])+(mag1[k2+1]))*kf2;
|
|
} else {
|
|
m1 += ((mag1[k2-1])+(mag1[k2]))*(1.0-kf2);
|
|
}
|
|
|
|
for(int k=k0+1;k<k2-1;k++) {
|
|
m1 += ((mag1[k])+(mag1[k+1]));
|
|
}
|
|
|
|
m1 *= 0.5;
|
|
|
|
for(int k=max(1,k1-peakWidth+1);k<=k0;k++) {
|
|
real d = (1.0-kf1)*peak1[k-k1+N] + kf1*peak1[k-k1+N+ko1];
|
|
real m = d*(p->y);
|
|
m1 += m;
|
|
dec[k] += m;
|
|
}
|
|
for(int k=k2;k<min(k1+peakWidth,Nover2-1);k++) {
|
|
real d = (1.0-kf1)*peak1[k-k1+N] + kf1*peak1[k-k1+N+ko1];
|
|
real m = d*(p->y);
|
|
m1 += m;
|
|
dec[k] += m;
|
|
}
|
|
p->m = m1;
|
|
}
|
|
|
|
for(list<peak *>::iterator tpi = peaks.begin();
|
|
tpi != peaks.end();
|
|
) {
|
|
peak *p = (*tpi);
|
|
|
|
list<peak*>::iterator eraseMe = tpi;
|
|
peak *pp = NULL;
|
|
if(tpi!=peaks.begin()) {
|
|
pp = *(--tpi);
|
|
tpi++;
|
|
}
|
|
peak *pn = NULL;
|
|
tpi++;
|
|
if(tpi != peaks.end()) {
|
|
pn = (*tpi);
|
|
}
|
|
|
|
peak *tp;
|
|
peak *tn;
|
|
tp = p->tp;
|
|
tn = p->tn;
|
|
|
|
int k1 = p->k;
|
|
int k0 = tp->k;
|
|
real kf0;
|
|
if(k0 < k1-peakWidth) {
|
|
k0 = k1-peakWidth;
|
|
kf0 = 0;
|
|
} else {
|
|
kf0 = k0 > tp->x ? k0 - tp->x : tp->x - k0;
|
|
}
|
|
int k2 = tn->k;
|
|
real kf2;
|
|
if(k2 > k1+peakWidth) {
|
|
k2 = k1+peakWidth;
|
|
kf2 = 0;
|
|
} else {
|
|
kf2 = k2 > tn->x ? k2 - tn->x : tn->x - k2;
|
|
}
|
|
|
|
real m = p->m;
|
|
real mdec = 0;
|
|
|
|
if(k0 < tp->x) {
|
|
mdec += (dec[k0]+dec[k0+1])*(1.0-kf0);
|
|
} else {
|
|
mdec += (dec[k0]+dec[k0+1]);
|
|
mdec += (dec[k0-1]+dec[k0])*kf0;
|
|
}
|
|
|
|
if(k2 < tn->x) {
|
|
mdec += (dec[k2-1]+dec[k2]);
|
|
mdec += (dec[k2]+dec[k2+1])*kf2;
|
|
} else {
|
|
mdec += (dec[k2-1]+dec[k2])*(1.0-kf2);
|
|
}
|
|
|
|
for(int k=k0+1;k<k2-1;k++) {
|
|
mdec += (dec[k]+dec[k+1]);
|
|
}
|
|
|
|
mdec *= 0.5;
|
|
|
|
if(mdec > maxDecRatio*m) {
|
|
peaks.erase(eraseMe);
|
|
m -= mdec;
|
|
if(pp && pn) {
|
|
real d = square(pn->x - p->x) + square(pp->x - p->x);
|
|
pp->m += m*square(pn->x - p->x)/d;
|
|
pn->m += m*square(pp->x - p->x)/d;
|
|
} else if(pp) {
|
|
pp->m += m;
|
|
} else if(pn) {
|
|
pn->m += m;
|
|
}
|
|
pa->destroy(p);
|
|
continue;
|
|
} else {
|
|
m -= mdec;
|
|
p->m = m;
|
|
}
|
|
}
|
|
}
|
|
|
|
void sms :: extractTrackPoints(grain *g,
|
|
real *mag0, real *mag1, real *mag2,
|
|
long currtime,
|
|
TrackPointListBuffer *trackPointListBuffer,
|
|
unsigned short h1,
|
|
int c)
|
|
{
|
|
real thmin = square(peakThresh)*magmax;
|
|
list<peak*> peaks;
|
|
list<peak*> troughs1;
|
|
list<peak*> troughs2;
|
|
|
|
peak *t0 = pa->create();
|
|
t0->k = 1;
|
|
t0->x = 1;
|
|
t0->y = mag1[0];
|
|
t0->y2 = mag2[0];
|
|
t0->tp = NULL;
|
|
t0->tp2 = NULL;
|
|
t0->tn = NULL;
|
|
t0->tn2 = NULL;
|
|
|
|
peak *t1 = pa->create();
|
|
t1->k = Nover2-1;
|
|
t1->x = Nover2-1;
|
|
t1->y = mag1[Nover2-1];
|
|
t1->y2 = mag2[Nover2-1];
|
|
t1->tp = NULL;
|
|
t1->tp2 = NULL;
|
|
t1->tn = NULL;
|
|
t1->tn2 = NULL;
|
|
|
|
troughs2.push_back(t0);
|
|
|
|
int k00=0;
|
|
int k01=Nover2-1;
|
|
int k10=0;
|
|
int k11=Nover2-1;
|
|
int kp = -1;
|
|
|
|
for(int k=1;k<Nover2-1;k++) {
|
|
if( (mag2[k] > thmin)
|
|
&&
|
|
(mag2[k] > mag2[k-1])
|
|
&&
|
|
(mag2[k] >= mag2[k+1])
|
|
) {
|
|
kp = k;
|
|
if(k>=kStart-peakWidth && k<=kEnd+peakWidth) {
|
|
peak *p = makePeak(mag1,mag2,k);
|
|
p->tn2 = t1;
|
|
if(!troughs1.empty() && troughs1.back()->tn == NULL) {
|
|
p->tp = troughs1.back();
|
|
troughs1.back()->tn = p;
|
|
}
|
|
if(!troughs2.empty() && troughs2.back()->tn2 == NULL) {
|
|
p->tp2 = troughs2.back();
|
|
troughs2.back()->tn2 = p;
|
|
}
|
|
peaks.push_back(p);
|
|
}
|
|
} else if((mag2[k] <= mag2[k-1])
|
|
&&
|
|
(mag2[k] <= mag2[k+1])
|
|
) {
|
|
peak *p = makePeak(mag1,mag2,k);
|
|
troughs2.push_back(p);
|
|
if(!peaks.empty()) {
|
|
peaks.back()->tn2 = p;
|
|
p->tp2 = peaks.back();
|
|
}
|
|
}
|
|
if((mag0[k] <= mag0[k-1])
|
|
&&
|
|
(mag0[k] <= mag0[k+1])
|
|
) {
|
|
if(peaks.empty())
|
|
k00 = k;
|
|
if(kp <= kEnd)
|
|
k01 = k;
|
|
}
|
|
if((mag1[k] <= mag1[k-1])
|
|
&&
|
|
(mag1[k] <= mag1[k+1])
|
|
) {
|
|
if(peaks.empty())
|
|
k10 = k;
|
|
if(kp <= kEnd)
|
|
k11 = k;
|
|
peak *p = makePeak(mag1,mag1,k);
|
|
troughs1.push_back(p);
|
|
if(!peaks.empty() && peaks.back()->tn == NULL) {
|
|
peaks.back()->tn = p;
|
|
p->tp = peaks.back();
|
|
}
|
|
}
|
|
}
|
|
real m0=0.0f;
|
|
real m1=0.0f;
|
|
for(int k=k00;k<=k01;k++) {
|
|
m0 += mag0[k];
|
|
m1 += mag1[k];
|
|
}
|
|
real f = (m1==0.0f)?1.0f:min(1.0f,m0/m1);
|
|
|
|
adjustPeaks(peaks,
|
|
mag0,
|
|
mag1,
|
|
mag2,
|
|
dec,
|
|
peakWidth);
|
|
|
|
tplist *trackPoints = new tplist;
|
|
for(list<peak*>::iterator pp=peaks.begin();
|
|
pp!=peaks.end();
|
|
pp++) {
|
|
|
|
peak *p = (*pp);
|
|
if(p->x>=kStart-0.5 && p->x<=kEnd+0.5 && p->m>0) {
|
|
real y = sqrt(f*p->m*mNorm);
|
|
trackpoint *tp = new trackpoint(g,p->x,y,N,M,h1,currtime);
|
|
trackPoints->push_back(tp);
|
|
}
|
|
}
|
|
|
|
pa->destroyAll();
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_lock(&tplbMutex[c]);
|
|
#endif
|
|
trackPointListBuffer->write(trackPoints);
|
|
#ifdef MULTITHREADED
|
|
pthread_mutex_unlock(&tplbMutex[c]);
|
|
#endif
|
|
}
|
|
|
|
}
|