void mgc2mgc(double *c1, const int m1, const double a1, const double g1,
double *c2, const int m2, const double a2, const double g2)
{
double a;
static double *ca = NULL;
static int size_a;
if (ca == NULL) {
ca = dgetmem(m1 + 1);
size_a = m1;
}
if (m1 > size_a) {
free(ca);
ca = dgetmem(m1 + 1);
size_a = m1;
}
a = (a2 - a1) / (1 - a1 * a2);
if (a == 0) {
movem(c1, ca, sizeof(*c1), m1 + 1);
gnorm(ca, ca, m1, g1);
gc2gc(ca, m1, g1, c2, m2, g2);
ignorm(c2, c2, m2, g2);
} else {
freqt(c1, m1, c2, m2, a);
gnorm(c2, c2, m2, g1);
gc2gc(c2, m2, g1, c2, m2, g2);
ignorm(c2, c2, m2, g2);
}
return;
}
//---------------------------------------------------------
void CGrid_IMCORR::gcorr(std::vector<std::vector<double> >ChipSearch, std::vector<std::vector<double> >ChipRef, double csmin, int mfit, double ddmx, std::vector<double> ioffrq, std::vector<double> nomoff, int& iacrej, double& streng, std::vector<double>& bfoffs, std::vector<double>& tlerrs, double ddact)
{
bfoffs.resize(3);
std::vector<double>unormc;
// compute raw cross-product sums
cross(unormc,ChipSearch,ChipRef);
// compute normalized cross-correlation values and compile statistics
std::vector<double>ccnorm;
std::vector<double>pkval;
std::vector<int>ipkcol;
std::vector<int>ipkrow;
std::vector<double>sums;
gnorm(ccnorm,pkval, ipkcol, ipkrow,sums,ChipSearch, ChipRef,unormc);
int ncol = (int)(ChipSearch[0].size()-ChipRef[0].size()+1);
int nrow = (int)(ChipSearch.size()-ChipRef.size()+1);
// Evaluate Strength of correlation peak
std::vector<double>cpval;
eval(ncol, nrow, ccnorm,pkval, ipkcol, ipkrow,sums,csmin, streng, iacrej,cpval);
std::vector<double>pkoffs;
// determine offsets of peak relative to nominal location
if (iacrej==1)
{
if (mfit != 4)
{
fitreg(cpval, mfit, pkoffs, tlerrs);
bfoffs[1] = (ipkcol[1] - 1) - nomoff[1] + pkoffs[1];
bfoffs[2] = (ipkrow[1] - 1) - nomoff[2] + pkoffs[2];
}
else
{
bfoffs[1] = (ipkcol[1] - 1) - nomoff[1];
bfoffs[2] = (ipkrow[1] - 1) - nomoff[2];
tlerrs[1] = 0.5;
tlerrs[2] = 0.5;
}
//
// Determine diagonal displacement from nominal and check against maximum
// acceptable value
ddact = sqrt(bfoffs[1]*bfoffs[1] + bfoffs[2]*bfoffs[2]);
if (ddmx > 0.0)
{
if (ddact > ddmx)
iacrej = 5;
}
else
{
if ( (bfoffs[1]*bfoffs[1] > ioffrq[1]*ioffrq[1])|| (bfoffs[2]*bfoffs[2] > ioffrq[2]*ioffrq[2]) )
iacrej = 5;
}
}
return;
}
double gammaCDF(double a, double x) {
double gln, p;
if (x <= 0.0 || a <= 0.0)
return 0.0;
else if (a > LARGE_A)
return gnorm(a, x);
else {
gln = lngamma(a);
if (x < (a + 1.0))
return gser(a, x, gln);
else
return (1.0 - gcf(a, x, gln));
}
}
int main(int argc, char **argv)
{
double g = GAMMA, *c;
int m = ORDER;
FILE *fp = stdin;
if ((cmnd = strrchr(argv[0], '/')) == NULL)
cmnd = argv[0];
else
cmnd++;
while (--argc)
if (**++argv == '-') {
switch (*(*argv + 1)) {
case 'g':
g = atof(*++argv);
--argc;
break;
case 'c':
g = atoi(*++argv);
--argc;
if (g < 1)
fprintf(stderr, "%s : value of c must be c>=1!\n", cmnd);
g = -1.0 / g;
break;
case 'm':
m = atoi(*++argv);
--argc;
break;
case 'h':
usage(0);
default:
fprintf(stderr, "%s : Invalid option '%c'!\n", cmnd, *(*argv + 1));
usage(1);
}
} else
fp = getfp(*argv, "rb");
c = dgetmem(m + 1);
while (freadf(c, sizeof(*c), m + 1, fp) == m + 1) {
gnorm(c, c, m, g);
fwritef(c, sizeof(*c), m + 1, stdout);
}
return (0);
}
//----------------------------------------------------------------------
//
//
//
doublevar Wannier_method::evaluate_local(const Array3 <dcomplex> & eikr,
Array2 <doublevar> & Rgen, Array2 <doublevar> & R) {
int norb=Rgen.GetDim(0);
Array2 <doublevar> gvec(3,3);
sys->getPrimRecipLattice(gvec);
Array1 <doublevar> gnorm(3);
gnorm=0;
for(int d=0; d < 3; d++) {
for(int d1=0; d1 < 3; d1++) {
gnorm(d)+=gvec(d,d1)*gvec(d,d1);
}
gnorm(d)=sqrt(gnorm(d));
}
Array2 <dcomplex> tmp(norb,norb),tmp2(norb,norb);
make_rotation_matrix(Rgen,R);
doublevar func=0;
for(int d=0; d< 3; d++) {
tmp=0.0;
for(int i=0; i< norb; i++) {
for(int j=0; j< norb; j++) {
for(int k=0; k< norb; k++) {
tmp(i,k)+=eikr(d,i,j)*R(k,j);
}
}
}
tmp2=0;
for(int i=0; i< norb; i++) {
for(int j=0; j< norb; j++) {
for(int k=0; k< norb; k++) {
tmp2(i,k)+=R(i,j)*tmp(j,k);
}
}
}
//cout << "========for d=" << d << endl;
for(int i=0; i< norb; i++) {
doublevar f= -log(norm(tmp2(i,i)))/(gnorm(d)*gnorm(d));
func+=f;
// cout << setw(9) << f;
}
}
//cout << endl;
return func/(3*norb);
}
void MeshMender::ProcessNormals(TriangleList& possibleNeighbors,
std::vector< Vertex >& theVerts,
std::vector< unsigned int >& mappingNewToOldVert,
D3DXVECTOR3 workingPosition)
{
NeighborGroupList neighborGroups;//a fresh group for each pass
//reset each triangle to prepare for smoothing group building
unsigned int i;
for(i = 0; i < possibleNeighbors.size(); ++i )
{
m_Triangles[ possibleNeighbors[i] ].Reset();
}
//now start building groups
CanSmoothNormalsChecker canSmoothNormalsChecker;
for( i = 0; i < possibleNeighbors.size(); ++i )
{
Triangle* currTri = &(m_Triangles[ possibleNeighbors[i] ]);
assert(currTri);
if(!currTri->handled)
{
BuildGroups(currTri,possibleNeighbors,
neighborGroups, theVerts,
&canSmoothNormalsChecker ,MinNormalsCreaseCosAngle );
}
}
std::vector<D3DXVECTOR3> groupNormalVectors;
for( i = 0; i < neighborGroups.size(); ++i )
{
//for each group, calculate the group normal
TriangleList& curGroup = neighborGroups[ i ];
D3DXVECTOR3 gnorm( 0.0f, 0.0f, 0.0f );
assert(curGroup.size()!=0 && "should not be a zero group here.");
for( size_t t = 0; t < curGroup.size(); ++t )//for each triangle in the group,
{
TriID tID = curGroup[ t ];
gnorm += m_Triangles[ tID ].normal;
}
gnorm = glm::normalize(gnorm.GLMvec());//D3DXVec3Normalize( &gnorm, &gnorm );
groupNormalVectors.push_back( gnorm );
}
//next step, ensure that triangles in different groups are not
//sharing vertices. and give the shared vertex their new group vector
std::set<size_t> otherGroupsIndices;
for( i = 0; i < neighborGroups.size(); ++i )
{
TriangleList& curGroup = neighborGroups[ i ];
std::set<size_t> thisGroupIndices;
for( size_t t = 0; t < curGroup.size(); ++t ) //for each tri
{
TriID tID = curGroup[ t ];
for(size_t indx = 0; indx < 3 ; ++indx)//for each vert in that tri
{
//if it is at the positions in question
if( theVerts[ m_Triangles[tID].indices[indx] ].pos == workingPosition)
{
//see if another group is already using this vert
if(otherGroupsIndices.find( m_Triangles[tID].indices[indx] ) != otherGroupsIndices.end() )
{
//then we need to make a new vertex
Vertex ov;
ov = theVerts[ m_Triangles[tID].indices[indx] ];
ov.normal = groupNormalVectors[i];
size_t oldIndex = m_Triangles[tID].indices[indx];
size_t newIndex = theVerts.size();
theVerts.push_back(ov);
AppendToMapping( oldIndex , m_originalNumVerts , mappingNewToOldVert);
UpdateIndices(oldIndex,newIndex,curGroup);
}
else
{
//otherwise, just update it with the new vector
theVerts[ m_Triangles[tID].indices[indx] ].normal = groupNormalVectors[i];
}
//store that we have used this index, so other groups can check
thisGroupIndices.insert(m_Triangles[tID].indices[indx]);
}
}
}
for(std::set<size_t>::iterator it = thisGroupIndices.begin(); it!= thisGroupIndices.end() ; ++it)
{
otherGroupsIndices.insert(*it);
}
}
}
int main(int argc, char **argv)
{
int m1 = ORDER1, m2 = ORDER2, i;
double a1 = ALPHA1, a2 = ALPHA2, g1 = GAMMA1, g2 = GAMMA2, *c1, *c2;
Boolean norm1 = NORMFLG1, norm2 = NORMFLG2, mulg1 = MULGFLG1, mulg2 =
MULGFLG2;
FILE *fp = stdin;
if ((cmnd = strrchr(argv[0], '/')) == NULL)
cmnd = argv[0];
else
cmnd++;
while (--argc)
if (**++argv == '-') {
switch (*(*argv + 1)) {
case 'm':
m1 = atoi(*++argv);
--argc;
break;
case 'M':
m2 = atoi(*++argv);
--argc;
break;
case 'a':
a1 = atof(*++argv);
--argc;
break;
case 'A':
a2 = atof(*++argv);
--argc;
break;
case 'g':
g1 = atof(*++argv);
--argc;
break;
case 'c':
g1 = atoi(*++argv);
--argc;
if (g1 < 1)
fprintf(stderr, "%s : value of c must be c>=1!\n", cmnd);
g1 = -1.0 / g1;
break;
case 'G':
g2 = atof(*++argv);
--argc;
break;
case 'C':
g2 = atoi(*++argv);
--argc;
if (g2 < 1)
fprintf(stderr, "%s : value of C must be C>=1!\n", cmnd);
g2 = -1.0 / g2;
break;
case 'n':
norm1 = 1 - norm1;
break;
case 'N':
norm2 = 1 - norm2;
break;
case 'u':
mulg1 = 1 - mulg1;
break;
case 'U':
mulg2 = 1 - mulg2;
break;
case 'h':
usage(0);
default:
fprintf(stderr, "%s : Invalid option '%c'!\n", cmnd, *(*argv + 1));
usage(1);
}
} else
fp = getfp(*argv, "rb");
c1 = dgetmem(m1 + m2 + 2);
c2 = c1 + m1 + 1;
if (mulg1 && g1 == 0) {
fprintf(stderr,
"%s : gamma for input mgc coefficients should not equal to 0 if you specify -u option!\n",
cmnd);
usage(1);
}
while (freadf(c1, sizeof(*c1), m1 + 1, fp) == m1 + 1) {
if (norm1)
ignorm(c1, c1, m1, g1);
else if (mulg1)
c1[0] = (c1[0] - 1.0) / g1;
if (mulg1)
for (i = m1; i >= 1; i--)
c1[i] /= g1;
mgc2mgc(c1, m1, a1, g1, c2, m2, a2, g2);
if (norm2)
gnorm(c2, c2, m2, g2);
else if (mulg2)
c1[0] = c1[0] * g2 + 1.0;
if (mulg2)
for (i = m2; i >= 1; i--)
c2[i] *= g2;
fwritef(c2, sizeof(*c2), m2 + 1, stdout);
}
return (0);
}
//----------------------------------------------------------------------
void Wannier_method::optimize_rotation(Array3 <dcomplex> & eikr,
Array2 <doublevar> & R ) {
int norb=eikr.GetDim(1);
Array2 <doublevar> gvec(3,3);
sys->getPrimRecipLattice(gvec);
Array1 <doublevar> gnorm(3);
gnorm=0;
for(int d=0; d < 3; d++) {
for(int d1=0; d1 < 3; d1++) {
gnorm(d)+=gvec(d,d1)*gvec(d,d1);
}
gnorm(d)=sqrt(gnorm(d));
}
for(int i=0; i< norb; i++) {
cout << "rloc2 " << i << " ";
for(int d=0; d< 3; d++) {
cout << -log(norm(eikr(d,i,i)))/(gnorm(d)*gnorm(d)) << " ";
}
cout << endl;
}
Array2 <doublevar> Rgen(norb,norb),Rgen_save(norb,norb);
//R(norb,norb);
R.Resize(norb,norb);
//Array2 <dcomplex> tmp(norb,norb),tmp2(norb,norb);
//Shake up the angles, since often the original orbitals
//are at a maximum and derivatives are zero.
Array2 <doublevar> deriv(norb,norb);
Rgen=0.0;
for(int ii=0; ii< norb; ii++) {
for(int jj=ii+1; jj< norb; jj++) {
Rgen(ii,jj)=rng.gasdev()*pi*shake;
}
}
for(int step=0; step < max_opt_steps; step++) {
doublevar fbase=evaluate_local(eikr,Rgen,R);
for(int ii=0; ii <norb; ii++) {
cout << "deriv ";
for(int jj=ii+1; jj < norb; jj++) {
doublevar save_rgeniijj=Rgen(ii,jj);
doublevar h=1e-6;
Rgen(ii,jj)+=h;
doublevar func=evaluate_local(eikr,Rgen,R);
deriv(ii,jj)=(func-fbase)/h;
Rgen(ii,jj)=save_rgeniijj;
cout << deriv(ii,jj) << " ";
}
cout << endl;
}
doublevar rloc_thresh=0.0001;
Rgen_save=Rgen;
doublevar best_func=1e99, best_tstep=0.0;
doublevar bracket_tstep=0.0;
doublevar last_func=fbase;
for(doublevar tstep=0.01; tstep < 20.0; tstep*=2.0) {
doublevar func=eval_tstep(eikr,Rgen,Rgen_save,deriv,tstep,R);
cout << "tstep " << tstep << " func " << func << endl;
if(func > fbase or func > last_func) {
bracket_tstep=tstep;
break;
}
else last_func=func;
}
cout << "bracket_tstep " << bracket_tstep << endl;
doublevar resphi=2.-(1.+sqrt(5.))/2.;
doublevar a=0, b=resphi*bracket_tstep, c=bracket_tstep;
doublevar af=fbase, bf=eval_tstep(eikr,Rgen,Rgen_save,deriv,b,R), cf=eval_tstep(eikr,Rgen,Rgen_save,deriv,bracket_tstep,R);
cout << "first step a,b,c " << a << " " << b << " " << c
<< " funcs " << af << " " << bf << " " << cf << endl;
for(int it=0; it < 20; it++) {
doublevar d,df;
if( (c-b) > (b-a))
d=b+resphi*(c-b);
else
d=b-resphi*(b-a);
df=eval_tstep(eikr,Rgen,Rgen_save,deriv,d,R);
if(df < bf) {
if( (c-b) > (b-a) ) {
a=b;
af=bf;
b=d;
bf=df;
}
else {
c=b;
cf=bf;
b=d;
bf=df;
}
}
else {
if( (c-b) > (b-a) ) {
c=d;
cf=df;
}
else {
a=d;
af=df;
}
}
cout << "step " << it << " a,b,c " << a << " " << b << " " << c
<< " funcs " << af << " " << bf << " " << cf << endl;
}
best_tstep=b;
/*
bool made_move=false;
while (!made_move) {
for(doublevar tstep=0.00; tstep < max_tstep; tstep+=0.1*max_tstep) {
for(int ii=0; ii< norb;ii++) {
for(int jj=ii+1; jj < norb; jj++) {
Rgen(ii,jj)=Rgen_save(ii,jj)-tstep*deriv(ii,jj);
}
}
doublevar func=evaluate_local(eikr,Rgen,R);
if(func < best_func) {
best_func=func;
best_tstep=tstep;
}
cout << " tstep " << tstep << " " << func << endl;
}
if(abs(best_tstep) < 0.2*max_tstep)
max_tstep*=0.5;
else if(abs(best_tstep-max_tstep) < 1e-14)
max_tstep*=2.0;
else made_move=true;
}
*/
for(int ii=0; ii< norb; ii++) {
for(int jj=ii+1; jj < norb; jj++) {
Rgen(ii,jj)=Rgen_save(ii,jj)-best_tstep*deriv(ii,jj);
}
}
doublevar func2=evaluate_local(eikr,Rgen,R);
doublevar max_change=0;
for(int ii=0; ii < norb; ii++) {
for(int jj=ii+1; jj< norb; jj++) {
doublevar change=abs(Rgen(ii,jj)-Rgen_save(ii,jj));
if(change > max_change) max_change=change;
}
}
cout << "tstep " << best_tstep << " rms " << sqrt(func2) << " bohr max change " << max_change <<endl;
doublevar threshold=0.0001;
if(max_change < threshold) break;
if(abs(best_func-fbase) < rloc_thresh) break;
/*
bool moved=false;
for(int ii=0; ii< norb; ii++) {
for(int jj=ii+1; jj< norb; jj++) {
doublevar save_rgeniijj=Rgen(ii,jj);
doublevar best_del=0;
doublevar best_f=1e99;
for(doublevar del=-0.5; del < 0.5; del+=0.05) {
cout << "############ for del = " << del << endl;
Rgen(ii,jj)=save_rgeniijj+del;
doublevar func=evaluate_local(eikr,Rgen,R);
if(func < best_f) {
best_f=func;
best_del=del;
}
cout << "func " << func << endl;
}
Rgen(ii,jj)=save_rgeniijj+best_del;
if(abs(best_del) > 1e-12) moved=true;
}
}
if(!moved) break;
*/
}
make_rotation_matrix(Rgen,R);
}
// This function generates a Frame.
void MAGE::Vocoder::push( Frame * frame, bool ignoreVoicing )
{
int i;
if( !flagFirstPush )
{
movem( cc, c, sizeof( * cc ), m + 1 );
mc2b( frame->streams[mgcStreamIndex], cc, m, alpha );
if( stage != 0 ) /* MGLSA*/
{
gnorm( cc, cc, m, gamma );
cc[0] = log( cc[0] );
for( i = 1; i <= m; i++ )
cc[i] *= gamma;
}
}
else
{
flagFirstPush = false;
mc2b( frame->streams[mgcStreamIndex], c, m, alpha );
if( stage != 0 ) // MGLSA
{
gnorm( c, c, m, gamma );
c[0] = log( c[0] );
for( i = 1; i <= m; i++ )
c[i] *= gamma;
}
for( i = 0; i <= m; i++ )
cc[i] = c[i];
}
for( i = 0; i <= m; i++ )
inc[i] = ( cc[i] - c[i] ) * iprd / fprd;
switch( action )
{
case MAGE::overwrite:
this->f0 = this->actionValue; // Hz
break;
case MAGE::shift:
this->f0 = ( frame->streams[lf0StreamIndex][0] ) + ( this->actionValue ); // Hz
break;
case MAGE::scale:
this->f0 = ( frame->streams[lf0StreamIndex][0] ) * ( this->actionValue ); // Hz
break;
case MAGE::synthetic:
case MAGE::noaction:
default:
this->f0 = frame->streams[lf0StreamIndex][0] ;
}
this->f0 = fabs(this->f0 + (this->sinresult * this->vibamp));//V
if( this->f0 < 0 )
this->f0 = MAGE::defaultPitch;
this->t0 = MAGE::defaultSamplingRate / this->f0; // defaultSamplingRate = 48000
if( !ignoreVoicing )
this->voiced = frame->voiced;
this->nOfPopSinceLastPush = 0;
return;
}
int main(int argc, char **argv)
{
int m = ORDER, fprd = FPERIOD, iprd = IPERIOD, stage = STAGE, pd =
PADEORDER, i, j;
Boolean transpose = TRANSPOSE, ngain = NGAIN, inverse = INVERSE;
FILE *fp = stdin, *fpc = NULL;
double alpha = ALPHA, gamma = -1 / (double) STAGE, x, *c, *inc, *cc, *d;
if ((cmnd = strrchr(argv[0], '/')) == NULL)
cmnd = argv[0];
else
cmnd++;
while (--argc)
if (**++argv == '-') {
switch (*(*argv + 1)) {
case 'm':
m = atoi(*++argv);
--argc;
break;
case 'a':
alpha = atof(*++argv);
--argc;
break;
case 'c':
stage = atoi(*++argv);
--argc;
break;
case 'p':
fprd = atoi(*++argv);
--argc;
break;
case 'i':
iprd = atoi(*++argv);
--argc;
break;
case 't':
transpose = 1 - transpose;
break;
case 'v':
inverse = 1 - inverse;
break;
case 'k':
ngain = 1 - ngain;
break;
case 'P':
pd = atoi(*++argv);
--argc;
break;
case 'h':
usage(0);
default:
fprintf(stderr, "%s : Invalid option '%c'!\n", cmnd, *(*argv + 1));
usage(1);
}
} else if (fpc == NULL)
fpc = getfp(*argv, "rb");
else
fp = getfp(*argv, "rb");
if (fpc == NULL) {
fprintf(stderr, "%s : Cannot open cepstrum file!\n", cmnd);
return (1);
}
if (inverse) {
if (stage == 0) {
fprintf(stderr, "%s : gamma should not equal to 0 in Inverse MGLSA!\n",
cmnd);
usage(1);
}
}
if (stage != 0) { /* MGLSA */
gamma = -1 / (double) stage;
} else { /* MLSA */
if ((pd < 4) || (pd > 5)) {
fprintf(stderr, "%s : Order of Pade approximation should be 4 or 5!\n",
cmnd);
return (1);
}
}
c = (stage != 0) ? dgetmem(m + m + m + 3 + (m + 1) * stage) /* MGLSA */
: dgetmem(3 * (m + 1) + 3 * (pd + 1) + pd * (m + 2)); /* MLSA */
cc = c + m + 1;
inc = cc + m + 1;
d = inc + m + 1;
if (freadf(c, sizeof(*c), m + 1, fpc) != m + 1)
return (1);
mc2b(c, c, m, alpha);
if (stage != 0) { /* MGLSA */
gnorm(c, c, m, gamma);
c[0] = log(c[0]);
for (i = 1; i <= m; i++)
c[i] *= gamma;
}
for (;;) {
if (freadf(cc, sizeof(*cc), m + 1, fpc) != m + 1)
return (0);
mc2b(cc, cc, m, alpha);
if (stage != 0) {
gnorm(cc, cc, m, gamma);
cc[0] = log(cc[0]);
for (i = 1; i <= m; i++)
cc[i] *= gamma;
}
for (i = 0; i <= m; i++)
inc[i] = (cc[i] - c[i]) * iprd / fprd;
for (j = fprd, i = (iprd + 1) / 2; j--;) {
if (freadf(&x, sizeof(x), 1, fp) != 1)
return (0);
if (inverse) { /* IMGLSA */
if (!ngain)
x /= exp(c[0]);
if (transpose)
x = imglsadft(x, c, m, alpha, stage, d);
else
x = imglsadf(x, c, m, alpha, stage, d);
} else {
if (stage != 0) { /* MGLSA */
if (!ngain)
x *= exp(c[0]);
if (transpose)
x = mglsadft(x, c, m, alpha, stage, d);
else
x = mglsadf(x, c, m, alpha, stage, d);
} else { /* MLSA */
if (!ngain)
x *= exp(c[0]);
x = mlsadf(x, c, m, alpha, pd, d);
}
}
fwritef(&x, sizeof(x), 1, stdout);
if (!--i) {
for (i = 0; i <= m; i++)
c[i] += inc[i];
i = iprd;
}
}
movem(cc, c, sizeof(*cc), m + 1);
}
return (0);
}
