int main()
{
FILE *fp;
const int Nx=200, Ny=200;
double dx=0.01, dy = 0.01;
double x0, y0, x, y;
int ix, iy, i;
double *Image;
double sx, sy;
Image = (double*)malloc(Nx*Ny*sizeof(double));
sx=0.4;
sy=0.2;
y0=(-0.5*Ny + 0.5)*dy;
x0=(-0.5*Nx + 0.5)*dx;
i = 0;
for (iy=0; iy<Ny; iy++) {
y = y0 + dy*iy;
for (ix=0; ix<Nx; ix++) {
x = x0 + dx*ix;
Image[i] = Gauss(x, 0, sx)*Gauss(y, 0, sy);
i++;
//printf("%d\n", i);
}
//printf("%d\n", i);
}
fp = fopen("intensityscreen_image.dat", "wb");
fwrite(Image, sizeof(double), Nx*Ny, fp);
fclose(fp);
free(Image);
return 0;
}
void Compute_blur(float dx, float dy)
{
int sample = 15;
float sampleWeights[15];
D3DXVECTOR2 sampleOffsets[15];
D3DXVECTOR4 blurstuff[15];
//x,y offset, z offset
//I NEED TO STOP CHANGING THIS, the first sample is to be 0 offset on texel
blurstuff[0].z = Gauss(0);
//sampleWeights[0] = Gauss(0);
blurstuff[0].x = 0;
blurstuff[0].y = 0;
//sampleOffsets[0] = D3DXVECTOR2(0,0);
float totalW = blurstuff[0].z;
for (int i = 0; i < 15 / 2; i++)
{
// Store weights for the positive and negative taps.
float weight = Gauss(i + 1);
blurstuff[i * 2 + 1].z = weight;
blurstuff[i * 2 + 2].z = weight;
totalW += weight * 2;
float sampleOffset = i * 2 + 1.5f;
D3DXVECTOR2 delta = D3DXVECTOR2(dx,dy)*sampleOffset;
// Store texture coordinate offsets for the positive and negative taps.
blurstuff[i * 2 + 1].x = delta.x;
blurstuff[i * 2 + 1].y = delta.y;
blurstuff[i * 2 + 2].x = -delta.x;
blurstuff[i * 2 + 2].y = -delta.y;
}
// Normalize the list of sample weightings, so they will always sum to one.
for (int i = 0; i < 15; i++)
{
//blurstuff[i].z = 1;
blurstuff[i].z /= totalW;
}
//std::copy(&sampleOffsets[0], &sampleOffsets[15], blur_cb_data.offset);
//std::copy(&sampleWeights[0], &sampleWeights[15], blur_cb_data.weight);
std::copy(&blurstuff[0], &blurstuff[15], blur_cb_data.offset);
//blur_cb_data.weight = sampleWeights;
}
bool IntersectPlanes (const TPlane& s1, const TPlane& s2, const TPlane& s3, TVector3 *p) {
double A[3][4];
double x[3];
double retval;
A[0][0] = s1.nml.x;
A[0][1] = s1.nml.y;
A[0][2] = s1.nml.z;
A[0][3] = -s1.d;
A[1][0] = s2.nml.x;
A[1][1] = s2.nml.y;
A[1][2] = s2.nml.z;
A[1][3] = -s2.d;
A[2][0] = s3.nml.x;
A[2][1] = s3.nml.y;
A[2][2] = s3.nml.z;
A[2][3] = -s3.d;
retval = Gauss ((double*) A, 3, x);
if (retval != 0) {
return false;
} else {
p->x = x[0];
p->y = x[1];
p->z = x[2];
return true;
}
}
int Eigenvectors(Ttensor t, Tvec vecs[])
{
tg = &t;
double vals[3];
int fnd = Eigenvalues(t, vals);
for (int i=0; i<fnd; i++) Gauss(t - ID*vals[i], NULL_COOR, vecs[i]);
return fnd;
}
void Mask::buildGauss(float Sig)
{
Dimention = int(5 * Sig);
if(Data != NULL)
delete[] Data;
Data = Gauss(Sig, Dimention);
}
/*===============================================================*/
void MiddlePoint(int p1,int p2,int CurrentLevel)
{ int middle;
middle=(p1+p2)/2;
if(CurrentLevel>MAX_LEVEL) return;
if((middle!=p1) && (middle!=p2))
{ Array[middle]=(Array[p1]+Array[p2])/2.0+Delta[CurrentLevel]*Gauss();
MiddlePoint(p1,middle,CurrentLevel+1);
MiddlePoint(middle,p2,CurrentLevel+1);
}
}
// Initialize the vector field.
// wlen_x: width of wave tile, in meters
// wlen_y: length of wave tile, in meters
void OceanSimulator::initHeightMap(OceanParameter& params, D3DXVECTOR2* out_h0, float* out_omega)
{
int i, j;
D3DXVECTOR2 K, Kn;
D3DXVECTOR2 wind_dir;
D3DXVec2Normalize(&wind_dir, ¶ms.wind_dir);
float a = params.wave_amplitude * 1e-7f; // It is too small. We must scale it for editing.
float v = params.wind_speed;
float dir_depend = params.wind_dependency;
int height_map_dim = params.dmap_dim;
float patch_length = params.patch_length;
// initialize random generator.
srand(0);
for (i = 0; i <= height_map_dim; i++)
{
// K is wave-vector, range [-|DX/W, |DX/W], [-|DY/H, |DY/H]
K.y = (-height_map_dim / 2.0f + i) * (2 * D3DX_PI / patch_length);
for (j = 0; j <= height_map_dim; j++)
{
K.x = (-height_map_dim / 2.0f + j) * (2 * D3DX_PI / patch_length);
float phil = (K.x == 0 && K.y == 0) ? 0 : sqrtf(Phillips(K, wind_dir, v, a, dir_depend));
out_h0[i * (height_map_dim + 4) + j].x = float(phil * Gauss() * HALF_SQRT_2);
out_h0[i * (height_map_dim + 4) + j].y = float(phil * Gauss() * HALF_SQRT_2);
// The angular frequency is following the dispersion relation:
// out_omega^2 = g*k
// The equation of Gerstner wave:
// x = x0 - K/k * A * sin(dot(K, x0) - sqrt(g * k) * t), x is a 2D vector.
// z = A * cos(dot(K, x0) - sqrt(g * k) * t)
// Gerstner wave shows that a point on a simple sinusoid wave is doing a uniform circular
// motion with the center (x0, y0, z0), radius A, and the circular plane is parallel to
// vector K.
out_omega[i * (height_map_dim + 4) + j] = sqrtf(GRAV_ACCEL * sqrtf(K.x * K.x + K.y * K.y));
}
}
}
SnakeClass::SnakeClass(HDIB m_hDIB)
{
int n,nBitCount;
LPSTR lpDIB;
BYTE *lpSrc;
hDIB = m_hDIB;
//锁定DIB，返回DIB内存首地址
lpDIB = (LPSTR) ::GlobalLock((HGLOBAL)hDIB);
//图像宽度
lWidth = ::DIBWidth(lpDIB);
//图像高度
lHeight = ::DIBHeight(lpDIB);
//找到DIB像素起始位置
lpDIBBits = ::FindDIBBits(lpDIB);
//获得DIB文件头
lpbmi = (LPBITMAPINFO)lpDIB;
//每像素所占位数
nBitCount = lpbmi->bmiHeader.biBitCount;
//计算图像每行的字节数
lLineBytes = (lWidth*nBitCount/8+3)/4*4;
//开辟新空间存储DIB
ImageData = new double[lWidth*lHeight];
for(int i = 0; i<lHeight; i++)
{
for(int j = 0; j<lWidth; j++)
{
lpSrc = (unsigned char*)lpDIBBits + lLineBytes*(lHeight-1-i)+j*nBitCount/8;
n=i*lWidth+j;
if(nBitCount==8)
ImageData[n]=*lpSrc;
if(nBitCount==24)
//24位图中计算亮度值时，RGB三分量权值分别为0.3,0.59,0.11
ImageData[n]=(*(lpSrc+1)*0.59f+*(lpSrc+2)*0.11f+*(lpSrc+0)*0.3f);
}
}
DrawContour=false;
p_NumPos=0;
pp=0;
p=NULL;
top=NULL;
Gauss();
Gradient();
}
int inverse(Matrix *matrix, Matrix *inverse_matri) {
size_t n = matrix->col_len;
init_matrix(inverse_matri, n, n);
float *arr = matrix->arr;
float *inver_arr = inverse_matri->arr;
Gauss(arr, inver_arr, n);
//inverse_define(matrix, inverse_matri);
return 0;
}
int main()
{
FILE *fp;
const int Nx=200, Ny=200, NE=200;
double dx=0.01, dy = 0.01, Emin=0.5, Emax=100;
double x0, y0, x, y, r, E, dE, Ec, Ec0=20, Ec1=70;
int ix, iy, iE, i;
double *Image;
double sx, sy, sE;
Image = (double*)malloc(Nx*Ny*NE*sizeof(double));
sx=0.4;
sy=0.2;
sE=4;
y0=(-0.5*Ny + 0.5)*dy;
x0=(-0.5*Nx + 0.5)*dx;
dE = (Emax - Emin)/(NE-1);
i = 0;
for (iE=0; iE<NE; iE++) {
E=Emin+dE*iE;
for (iy=0; iy<Ny; iy++) {
y = y0 + dy*iy;
for (ix=0; ix<Nx; ix++) {
x = x0 + dx*ix;
r=sqrt(x*x + y*y);
Ec = Ec0+Ec1*r;
Image[i] = Gauss(x, 0, sx)*Gauss(y, 0, sy)*Gauss(E, Ec, sE);
i++;
//printf("%d\n", i);
}
}
//printf("%d\n", i);
}
fp = fopen("beamscreen_image.dat", "wb");
fwrite(Image, sizeof(double), Nx*Ny*NE, fp);
fclose(fp);
free(Image);
return 0;
}
void UVaOceanSimulatorComponent::InitHeightMap(FOceanData& Params, TResourceArray<FVector2D>& out_h0, TResourceArray<float>& out_omega)
{
int32 i, j;
FVector2D K, Kn;
FVector2D wind_dir = Params.WindDirection;
wind_dir.Normalize();
float a = Params.WaveAmplitude * 1e-7f; // It is too small. We must scale it for editing.
float v = Params.WindSpeed;
float dir_depend = Params.WindDependency;
int height_map_dim = Params.DispMapDimension;
float patch_length = Params.PatchLength;
for (i = 0; i <= height_map_dim; i++)
{
// K is wave-vector, range [-|DX/W, |DX/W], [-|DY/H, |DY/H]
K.Y = (-height_map_dim / 2.0f + i) * (2 * PI / patch_length);
for (j = 0; j <= height_map_dim; j++)
{
K.X = (-height_map_dim / 2.0f + j) * (2 * PI / patch_length);
float phil = (K.X == 0 && K.Y == 0) ? 0 : sqrtf(Phillips(K, wind_dir, v, a, dir_depend));
out_h0[i * (height_map_dim + 4) + j].X = float(phil * Gauss() * HALF_SQRT_2);
out_h0[i * (height_map_dim + 4) + j].Y = float(phil * Gauss() * HALF_SQRT_2);
// The angular frequency is following the dispersion relation:
// out_omega^2 = g*k
// The equation of Gerstner wave:
// x = x0 - K/k * A * sin(dot(K, x0) - sqrt(g * k) * t), x is a 2D vector.
// z = A * cos(dot(K, x0) - sqrt(g * k) * t)
// Gerstner wave shows that a point on a simple sinusoid wave is doing a uniform circular
// motion with the center (x0, y0, z0), radius A, and the circular plane is parallel to
// vector K.
out_omega[i * (height_map_dim + 4) + j] = sqrtf(GRAV_ACCEL * sqrtf(K.X * K.X + K.Y * K.Y));
}
}
}
void PhysicalUnits::PrintUnits()
{
printf("%24s = %-14.3e %s\n", "[Length]", Length, "cm");
printf("%24s = %-14.3e %s\n", "[Mass]", Mass, "gm");
printf("%24s = %-14.3e %s\n", "[Time]", Time, "s");
printf("%24s = %-14.3e %s\n", "[Velocity]", Length/Time, "cm/s");
printf("%24s = %-14.3e %s\n", "[Energy]", 1.0/Erg(), "erg");
printf("%24s = %-14.3e %s\n", "[B-field]", 1.0/Gauss(), "Gauss");
printf("%24s = %-14.3e %s\n", "MeV", MeV(), "[Energy]");
printf("%24s = %-14.3e %s\n", "LightSpeed", LightSpeed(), "[Velocity]");
printf("%24s = %-14.3e %s\n", "GramsPerCubicCentimeter", GramsPerCubicCentimeter(), "[Mass]/[Length]^3");
}
// DeJong's fourth function is:
//
// 30
// ___
// f4(xi) = \ { i * xi^4 + Gauss(0,1) }
// /__
// i=1
//
// where each x is in the range [-1.28,1.28]
float
DeJong4(GAGenome & c)
{
GABin2DecGenome & genome = (GABin2DecGenome &)c;
float value = 0;
for(int i=0; i<30; i++){
float v = genome.phenotype(i);
v *= v; // xi^2
v *= v; // xi^4
v *= i;
v += Gauss(0,1);
value += v;
}
return(value);
}
double *Random::GaussArray (double *array, int num, double stdev)
{
double *out, fac, rsq, v1, v2;
int i, num2;
if (stdev == 0.0)
out = array;
else
{
out = (double *) malloc (num * sizeof (double));
/* Since the gaussian function generates pairs of deviates, this fills in */
/* the array up to an even number by pairs, then handles the possible last */
/* member separately. */
num2 = (num / 2) * 2;
for (i = 0; i < num2; i += 2)
{
do
{
v1 = 2.0 * Rand () - 1.0; // pick two uniform numbers in the square
v2 = 2.0 * Rand () - 1.0; // extending from -1 to +1 in each direction
rsq = v1 * v1 + v2 * v2; // see if they are unit circle
}
while (rsq >= 1.0 || rsq == 0.0); // and if they are not, try again
fac = sqrt (-2.0 * log (rsq) / rsq);
out [i] = array [i] + stdev * v1 * fac;
out [i+1] = array [i+1] + stdev * v2 * fac;
}
if (num % 2 == 1)
{
if (haveExtra)
{
haveExtra = false;
out [i] = array [i] + stdev * Extra;
}
else
out [i] = array [i] + stdev * Gauss ();
}
}
return (out);
}
void GaussianBlur::updateKernel(double sigma, int size)
{
_sigma = sigma;
_size = size;
kernel.resize(_size, _size);
double sum = 0;
for (int i = 0; i < _size; i++)
for (int j = 0; j < _size; j++)
{
kernel(i,j) = Gauss(_sigma, i-_size/2, j-_size/2);
sum += kernel(i,j);
}
for (int i = 0; i < _size; i++)
for (int j = 0; j < _size; j++)
kernel(i,j) /= sum;
}
double* Krylov::getPoly ()
{
double** Q = new double* [size];
for (int i=0;i<size;i++)
{
Q[i] = new double[size+1];
}
double* c = new double [size];
for (int i=0;i<size;i++)
{
c[i]= c_0[i];
}
for (int i=0;i<size+1;i++)
{
for (int j=0;j<size;j++)
{
Q[j][((size+1)+size-1-i)%(size+1)] = c[j];
}
c = mulMatrix (c);
}
Gauss gauss = Gauss(Q,size);
int m = 0;
double* p = gauss.solve (m);
if (m==size)
{
return p;
}
else
{
printf ("Only %d steps on get Poly\n",m);
}
for (int i=0;i<size;i++) delete [] Q[i];
delete [] Q;
delete [] c;
return NULL;
}
static int
_gauss_Cmd_ (ClientData clientData,
Tcl_Interp *interp,
int argc,
char **argv)
{
/* Command line definition */
char * options[] = {
"SSSs",
"-fit","S",
NULL
};
char * helpMsg = {
(
" Fit the data by a Gaussian (y=prefactor*exp(-(x-m)*(x-m)/(sigma*sigma)).\n"
"\n"
"Arguments :\n"
" 3 signals - signal to fit, \n"
" signal containing the incertaity for each point, \n"
" signal containing the initial values (sigma (!=0),m,prefactor).\n"
" string - name of the result (s,h,d,chisq,goodness,covar --> size=14)\n."
"\n"
"Options :\n"
" -fit : signal containing the x-value (REALY)\n"
" --> return the bic fit in this signal (REALXY).\n"
)
};
Signal *datasignal,*sigmasignal, *valsignal;
Signal *result, *xsignal = NULL;
char *resultName;
int i,j,size;
real *X;
int ma;
int *ia;
real *a,**covar,**alpha;
real chisq;
int u;
if (arg_init(interp, argc, argv, options, helpMsg))
return TCL_OK;
if (arg_get(0, &datasignal, &sigmasignal, &valsignal, &resultName) == TCL_ERROR)
return TCL_ERROR;
if (arg_get(1, &xsignal) == TCL_ERROR)
return TCL_ERROR;
size = datasignal->size;
if(sigmasignal->size != size) {
Tcl_AppendResult (interp,
"The sigma signal should be of the same size as the input signal.",
(char *) NULL);
return TCL_ERROR;
}
if (datasignal->type == REALY)
{
X = (float *) malloc(sizeof(float)*size);
for(i=0;i<size;i++)
X[i] = datasignal->x0 + i*datasignal->dx;
}
else if (datasignal->type == REALXY)
{
X = (float *) malloc(sizeof(float)*size);
for(i=0;i<size;i++)
X[i] = datasignal->dataX[i];
}
else {
Tcl_AppendResult (interp,
"Bad type for the signal (only REALY or REALXY).",
(char *) NULL);
return TCL_ERROR;
}
ma = valsignal->size;
if (ma != 3) {
Tcl_AppendResult (interp,
"Bad number of initial value (we need 2 values sigma, m and prefactor).",
(char *) NULL);
return TCL_ERROR;
}
NonLinFitInit(&a,&ia,ma,&covar,&alpha);
for(i=0;i<valsignal->size;i++)
{
a[i+1]=valsignal->dataY[i];
ia[i+1] = 1;
}
NonLinFit(X-1,datasignal->dataY-1,sigmasignal->dataY-1,size,a,ia,ma,covar,alpha,
&chisq,&Gauss);
result = sig_new (REALY, 0, ma+ma*ma+1);
result->x0 = 0.0;
result->dx = 1;
u =0;
for(i=1;i<=ma;i++) {
result->dataY[u]=a[i];
u=u+1;
}
result->dataY[u++]=chisq;
result->dataY[u++]=NonLinFitConfidence(chisq,size,ma);
for(i=1;i<=ma;i++) {
for(j=1;j<=ma;j++)
{
result->dataY[u++]=covar[i][j];
}
}
if (!result)
return TCL_ERROR;
store_signal_in_dictionary(resultName, result);
if (arg_present(1)) {
if (xsignal->type != REALY) {
Tcl_AppendResult (interp,
"Type for xsignal must be REALY!!",
(char *) NULL);
return TCL_ERROR;
}
else {
sig_realy2realxy(xsignal);
sig_put_y_in_x(xsignal);
for (i=0;i<xsignal->size;i++)
Gauss(xsignal->dataX[i],a,&(xsignal->dataY[i]),NULL,ma);
}
}
NonLinFitDelete(a,ia,ma,covar,alpha);
return TCL_OK;
}
double N(double m, double sigma)
{
return m + Gauss(sigma);
}
/*-----------------------------------------------------------*
* procedure Calc_exc_rand *
* ~~~~~~~~~~~~~ *
* Computes comfort noise excitation *
* for SID and not-transmitted frames *
*-----------------------------------------------------------*/
void WebRtcG729fix_Calc_exc_rand(
int32_t L_exc_err[],
int16_t cur_gain, /* (i) : target sample gain */
int16_t *exc, /* (i/o) : excitation array */
int16_t *seed, /* (i) : current Vad decision */
int flag_cod /* (i) : encoder/decoder flag */
)
{
int16_t i, j, i_subfr;
int16_t temp1, temp2;
int16_t pos[4];
int16_t sign[4];
int16_t t0, frac;
int16_t *cur_exc;
int16_t g, Gp, Gp2;
int16_t excg[L_SUBFR], excs[L_SUBFR];
int32_t L_acc, L_ener, L_k;
int16_t max, hi, lo, inter_exc;
int16_t sh;
int16_t x1, x2;
if (cur_gain == 0) {
WebRtcSpl_ZerosArrayW16(exc, L_FRAME);
Gp = 0;
t0 = WebRtcSpl_AddSatW16(L_SUBFR,1);
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
if (flag_cod != FLAG_DEC)
WebRtcG729fix_update_exc_err(L_exc_err, Gp, t0);
}
return;
}
/* Loop on subframes */
cur_exc = exc;
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
/* generate random adaptive codebook & fixed codebook parameters */
/*****************************************************************/
temp1 = WebRtcG729fix_Random(seed);
frac = WebRtcSpl_SubSatW16((temp1 & (int16_t)0x0003), 1);
if(frac == 2) frac = 0;
temp1 = shr(temp1, 2);
t0 = WebRtcSpl_AddSatW16((temp1 & (int16_t)0x003F), 40);
temp1 = shr(temp1, 6);
temp2 = temp1 & (int16_t)0x0007;
pos[0] = WebRtcSpl_AddSatW16(shl(temp2, 2), temp2); /* 5 * temp2 */
temp1 = shr(temp1, 3);
sign[0] = temp1 & (int16_t)0x0001;
temp1 = shr(temp1, 1);
temp2 = temp1 & (int16_t)0x0007;
temp2 = WebRtcSpl_AddSatW16(shl(temp2, 2), temp2);
pos[1] = WebRtcSpl_AddSatW16(temp2, 1); /* 5 * x + 1 */
temp1 = shr(temp1, 3);
sign[1] = temp1 & (int16_t)0x0001;
temp1 = WebRtcG729fix_Random(seed);
temp2 = temp1 & (int16_t)0x0007;
temp2 = WebRtcSpl_AddSatW16(shl(temp2, 2), temp2);
pos[2] = WebRtcSpl_AddSatW16(temp2, 2); /* 5 * x + 2 */
temp1 = shr(temp1, 3);
sign[2] = temp1 & (int16_t)0x0001;
temp1 = shr(temp1, 1);
temp2 = temp1 & (int16_t)0x000F;
pos[3] = WebRtcSpl_AddSatW16((temp2 & (int16_t)1), 3); /* j+3*/
temp2 = (shr(temp2, 1)) & (int16_t)7;
temp2 = WebRtcSpl_AddSatW16(shl(temp2, 2), temp2); /* 5i */
pos[3] = WebRtcSpl_AddSatW16(pos[3], temp2);
temp1 = shr(temp1, 4);
sign[3] = temp1 & (int16_t)0x0001;
Gp = WebRtcG729fix_Random(seed) & (int16_t)0x1FFF; /* < 0.5 Q14 */
Gp2 = shl(Gp, 1); /* Q15 */
/* Generate gaussian excitation */
/********************************/
L_acc = 0L;
for(i=0; i<L_SUBFR; i++) {
temp1 = Gauss(seed);
L_acc = L_mac(L_acc, temp1, temp1);
excg[i] = temp1;
}
/*
Compute fact = alpha x cur_gain * sqrt(L_SUBFR / Eg)
with Eg = SUM(i=0->39) excg[i]^2
and alpha = 0.5
alpha x sqrt(L_SUBFR)/2 = 1 + FRAC1
*/
L_acc = WebRtcG729fix_Inv_sqrt(L_shr(L_acc,1)); /* Q30 */
WebRtcG729fix_L_Extract(L_acc, &hi, &lo);
/* cur_gain = cur_gainR << 3 */
temp1 = mult_r(cur_gain, FRAC1);
temp1 = WebRtcSpl_AddSatW16(cur_gain, temp1);
/* <=> alpha x cur_gainR x 2^2 x sqrt(L_SUBFR) */
L_acc = WebRtcG729fix_Mpy_32_16(hi, lo, temp1); /* fact << 17 */
sh = WebRtcSpl_NormW32(L_acc);
temp1 = extract_h(L_shl(L_acc, sh)); /* fact << (sh+1) */
sh = WebRtcSpl_SubSatW16(sh, 14);
for (i = 0; i < L_SUBFR; i++) {
temp2 = mult_r(excg[i], temp1);
temp2 = shr_r(temp2, sh); /* shl if sh < 0 */
excg[i] = temp2;
}
/* generate random adaptive excitation */
/****************************************/
WebRtcG729fix_Pred_lt_3(cur_exc, t0, frac, L_SUBFR);
/* compute adaptive + gaussian exc -> cur_exc */
/**********************************************/
max = 0;
for(i = 0; i < L_SUBFR; i++) {
temp1 = mult_r(cur_exc[i], Gp2);
temp1 = WebRtcSpl_AddSatW16(temp1, excg[i]); /* may overflow */
cur_exc[i] = temp1;
temp1 = abs_s(temp1);
if (temp1 > max)
max = temp1;
}
/* rescale cur_exc -> excs */
if (max == 0)
sh = 0;
else {
sh = WebRtcSpl_SubSatW16(3, WebRtcSpl_NormW16(max));
if (sh <= 0)
sh = 0;
}
for (i = 0; i < L_SUBFR; i++) {
excs[i] = shr(cur_exc[i], sh);
}
/* Compute fixed code gain */
/***************************/
/**********************************************************/
/*** Solve EQ(X) = 4 X**2 + 2 b X + c */
/**********************************************************/
L_ener = 0L;
for (i = 0; i < L_SUBFR; i++) {
L_ener = L_mac(L_ener, excs[i], excs[i]);
} /* ener x 2^(-2sh + 1) */
/* inter_exc = b >> sh */
inter_exc = 0;
for (i = 0; i < 4; i++) {
j = pos[i];
if (sign[i] == 0) {
inter_exc = WebRtcSpl_SubSatW16(inter_exc, excs[j]);
}
else {
inter_exc = WebRtcSpl_AddSatW16(inter_exc, excs[j]);
}
}
/* Compute k = cur_gainR x cur_gainR x L_SUBFR */
L_acc = L_mult(cur_gain, L_SUBFR);
L_acc = L_shr(L_acc, 6);
temp1 = extract_l(L_acc); /* cur_gainR x L_SUBFR x 2^(-2) */
L_k = L_mult(cur_gain, temp1); /* k << 2 */
temp1 = WebRtcSpl_AddSatW16(1, shl(sh,1));
L_acc = L_shr(L_k, temp1); /* k x 2^(-2sh+1) */
/* Compute delta = b^2 - 4 c */
L_acc = WebRtcSpl_SubSatW32(L_acc, L_ener); /* - 4 c x 2^(-2sh-1) */
inter_exc = shr(inter_exc, 1);
L_acc = L_mac(L_acc, inter_exc, inter_exc); /* 2^(-2sh-1) */
sh = WebRtcSpl_AddSatW16(sh, 1);
/* inter_exc = b x 2^(-sh) */
/* L_acc = delta x 2^(-2sh+1) */
if (L_acc < 0) {
/* adaptive excitation = 0 */
WEBRTC_SPL_MEMCPY_W16(cur_exc, excg, L_SUBFR);
temp1 = abs_s(excg[(int)pos[0]]) | abs_s(excg[(int)pos[1]]);
temp2 = abs_s(excg[(int)pos[2]]) | abs_s(excg[(int)pos[3]]);
temp1 = temp1 | temp2;
sh = ((temp1 & (int16_t)0x4000) == 0) ? (int16_t)1 : (int16_t)2;
inter_exc = 0;
for(i=0; i<4; i++) {
temp1 = shr(excg[(int)pos[i]], sh);
if(sign[i] == 0) {
inter_exc = WebRtcSpl_SubSatW16(inter_exc, temp1);
}
else {
inter_exc = WebRtcSpl_AddSatW16(inter_exc, temp1);
}
} /* inter_exc = b >> sh */
WebRtcG729fix_L_Extract(L_k, &hi, &lo);
L_acc = WebRtcG729fix_Mpy_32_16(hi, lo, K0); /* k x (1- alpha^2) << 2 */
temp1 = WebRtcSpl_SubSatW16(shl(sh, 1), 1); /* temp1 > 0 */
L_acc = L_shr(L_acc, temp1); /* 4k x (1 - alpha^2) << (-2sh+1) */
L_acc = L_mac(L_acc, inter_exc, inter_exc); /* delta << (-2sh+1) */
Gp = 0;
}
temp2 = Sqrt(L_acc); /* >> sh */
x1 = WebRtcSpl_SubSatW16(temp2, inter_exc);
x2 = negate(WebRtcSpl_AddSatW16(inter_exc, temp2)); /* x 2^(-sh+2) */
if(abs_s(x2) < abs_s(x1)) x1 = x2;
temp1 = WebRtcSpl_SubSatW16(2, sh);
g = shr_r(x1, temp1); /* shl if temp1 < 0 */
if (g >= 0) {
if (g > G_MAX)
g = G_MAX;
}
else {
if (WebRtcSpl_AddSatW16(g, G_MAX) < 0)
g = negate(G_MAX);
}
/* Update cur_exc with ACELP excitation */
for (i = 0; i < 4; i++) {
j = pos[i];
if (sign[i] != 0) {
cur_exc[j] = WebRtcSpl_AddSatW16(cur_exc[j], g);
}
else {
cur_exc[j] = WebRtcSpl_SubSatW16(cur_exc[j], g);
}
}
if (flag_cod != FLAG_DEC)
WebRtcG729fix_update_exc_err(L_exc_err, Gp, t0);
cur_exc += L_SUBFR;
} /* end of loop on subframes */
return;
}
////////////////////////
//输入参数为6个点x,y,z值，这些值保存在结构体指针中,
// 和目的点的SPoint结构，其中x,y已知，Z未知.
//返回参数为所获得z值
// ****所用到的数组变量从下标为1开始.从basic转换而来的
double CSomeFuncs::tend( SPoint* srcPnt, SPoint& desPnt )
{
const double eps1 = 0.0000001;
const int M_a = 6; //样点数
const int NT = 2; //方程式的最高次数为二次方
const int MM = 6; //数组a,b,c的实际大小
//MM可一通过如下方法算出：
// MM = 1;
// for( i = 1; i<= NT; i++)
// MM = MM + (i + 1);
double a[MM + 1][MM + 1], b[MM + 1], c[MM + 1];
double DATA[M_a + 1][6]; //样本的属性，data(1,1)--x , data(1,2)--y, data(1,3)--z,
// data(1,4)--Z_c,data(1,5)--err_z
// 注意，只用了DATA[1~6, 1~5]
int Status = 1;
int i, j, k, JB;
//初始化DATA的值
for( i = 1; i <= M_a; i++ )
{
DATA[i][1] = srcPnt[i - 1].x;
DATA[i][2] = srcPnt[i - 1].y;
DATA[i][3] = srcPnt[i - 1].z;
}
// SET UP COEFICINT MATRAX
c[1] = 1.0;
// C ARRAY IS USED AS TEMPRARY VARIATION
// CALCULATE NUMBER OF COEFFICINTS
// Reset
for( i = 1; i <= MM; i++ )
{
b[i] = 0.0;
// B ARRAY IS C ARRAY IN THE FORMULA
for ( j = 1; j <= MM; j++ )
a[i][j] = 0.0;
}
// to create a matrix
for( i = 1; i <= M_a; i++ ) //M_a为样本数
{
JB = 1;
for( j = 1; j <= NT; j++ )
{
for( k = j; k >= 0; k-- )
{
JB = JB + 1;
c[JB] = pow( DATA[i][1], k ) * pow( DATA[i][2], ( j - k ) );
}
}
for( j = 1; j <= MM; j++ )
{
b[j] = b[j] + c[j] * DATA[i][3];
for( k = 1; k <= MM; k++ )
a[j][k] = a[j][k] + c[j] * c[k];
}
}
Gauss( a, b, MM, Status ); //调用高斯函数，同时b返回数据
// CALCULATE ESTIMATE VALUE DATA(I,4) AND DEVIATION DATA(I,5) FOR ANY POINT
for( i = 1; i <= M_a; i++ )
{
JB = 1;
for ( j = 1; j <= NT; j++ )
for( k = j; k <= 0; k-- )
{
JB = JB + 1;
c[JB] = pow( DATA[i][1], k ) * pow( DATA[i][2], ( j - k ) );
}
DATA[i][4] = 0.0;
for( j = 1; j <= MM; j++ )
DATA[i][4] = DATA[i][4] + b[j] * c[j];
DATA[i][5] = DATA[i][3] - DATA[i][4];
}
//计算函数返回值
// z(x,y)=b1+b2*x+b3*y+b4*x*x+b5*x*y+b6*y*y+...+EPSON
desPnt.z = ( b[1] + b[2] * desPnt.x + b[3] * desPnt.y + b[4] * desPnt.x * desPnt.x + b[5] * desPnt.x * desPnt.y + b[6] * desPnt.y * desPnt.y + eps1 );
return desPnt.z;
}//end sub
int main(int argc, char **argv)
{
int mpu9150_fd;
unsigned int m = 1903;
FILE *from_fd,*to_fd;
int magn_fd;
double *ax,*ay;
double count_B2;//水平磁场分量
mpu9150_fd = open("/dev/mpu9150",O_RDWR);
if (mpu9150_fd < 0){
printf("can't open /dev/mpu9150\n");
return -1;
}else{
printf("Now,you are in /dev/mpu9150\n");
}
magn_fd = open("/dev/magn",O_RDWR);
if (magn_fd < 0){
printf("can't open /dev/magn\n");
return -1;
}else{
printf("Now,you are in /dev/magn\n");
}
ax = (double *)malloc(sizeof (double)* m);
if(NULL==ax){
printf("error to malloc ax\n");
exit(1);
}
ay = (double *)malloc(sizeof (double)* m);
if(NULL==ay){
printf("error to malloc ay\n");
exit(1);
}
from_fd = fopen("/data_file.txt","r");
if(from_fd == NULL){
printf("open '/data_file.txt' failed!");
return -1;
}
for(int i=0;i<m;i++){
fscanf(from_fd,"%lf%*c%lf",&ax[i],&ay[i]); //中间跳过空格,水平分量值读入数组中
}
fclose(from_fd);
count_B2 = ((MAX(ax,m) - MIN(ax,m))/2.0)*((MAX(ax,m) - MIN(ax,m))/2.0);
g_ellipse_parameter.F = -count_B2;
g_equation_temp_value.bb = (double *)malloc(sizeof (double)* 5);
for(int i=0;i<5;i++){
g_equation_temp_value.bb[i] = count_B2; //初始化线性常量
}
g_equation_temp_value = equation_temp_value(ax,ay,g_equation_temp_value,m); //相关运算，得到设定方程的临时参数
free(ay);
ay = NULL;
free(ax);
ax = NULL;
Gauss(g_equation_temp_value.V,g_equation_temp_value.bb,5); //计算得到拟合椭圆参数，存储在bb[0]~bb[4]中
free(g_equation_temp_value.V);
g_ellipse_parameter = ellipse_parameter_calculate(g_equation_temp_value,g_ellipse_parameter);//计算得到椭圆倾斜角等量
free(g_equation_temp_value.bb);
cos_phi=cos(g_ellipse_parameter.orientation_rad);
sin_phi=sin(g_ellipse_parameter.orientation_rad);
to_fd = fopen("/magn_data_output.txt","w+");
if(to_fd == NULL){
printf("open '/magn_data_output.txt' failed!");
return -1;
}
sleep(2);
Accel_Correct(mpu9150_fd); //消除固有误差（放置不水平等问题）
while(1){
Get_AccelGyro_Data(mpu9150_fd);
//Get_9150Magn_Data(mpu9150_fd); //未驱动使用；
Coordinate_Transformation(); //将9150坐标系变换到磁力计lis3mdl坐标系;
Calculate_PitchRoll(); //计算俯仰角和侧倾角
Calculate_Heading(magn_fd,to_fd);
Printf_AccelGyroMagn_Output();
usleep(20*1000);
}
close(mpu9150_fd);
close(magn_fd);
fclose(to_fd);
return 0;
}
Ttensor Inverse(Ttensor t)
{
Ttensor t1;
Gauss(t, ID, t1);
return t1;
}
/*-----------------------------------------------------------*
* procedure Calc_exc_rand *
* ~~~~~~~~~~~~~ *
* Computes comfort noise excitation *
* for SID and not-transmitted frames *
*-----------------------------------------------------------*/
void Calc_exc_rand(
Word32 L_exc_err[4],
Word16 cur_gain, /* (i) : target sample gain */
Word16 *exc, /* (i/o) : excitation array */
Word16 *seed, /* (i) : current Vad decision */
Flag flag_cod /* (i) : encoder/decoder flag */
)
{
Word16 i, j, i_subfr;
Word16 temp1, temp2;
Word16 pos[4];
Word16 sign[4];
Word16 t0, frac;
Word16 *cur_exc;
Word16 g, Gp, Gp2;
Word16 excg[L_SUBFR], excs[L_SUBFR];
Word32 L_acc, L_ener, L_k;
Word16 max, hi, lo, inter_exc;
Word16 sh;
Word16 x1, x2;
if(cur_gain == 0) {
for(i=0; i<L_FRAME; i++) {
exc[i] = 0;
}
Gp = 0;
t0 = add(L_SUBFR,1);
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
if(flag_cod != FLAG_DEC) update_exc_err(L_exc_err, Gp, t0);
}
return;
}
/* Loop on subframes */
cur_exc = exc;
for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
/* generate random adaptive codebook & fixed codebook parameters */
/*****************************************************************/
temp1 = Random(seed);
frac = sub((temp1 & (Word16)0x0003), 1);
if(sub(frac, 2) == 0) frac = 0;
temp1 = shr(temp1, 2);
t0 = add((temp1 & (Word16)0x003F), 40);
temp1 = shr(temp1, 6);
temp2 = temp1 & (Word16)0x0007;
pos[0] = add(shl(temp2, 2), temp2); /* 5 * temp2 */
temp1 = shr(temp1, 3);
sign[0] = temp1 & (Word16)0x0001;
temp1 = shr(temp1, 1);
temp2 = temp1 & (Word16)0x0007;
temp2 = add(shl(temp2, 2), temp2);
pos[1] = add(temp2, 1); /* 5 * x + 1 */
temp1 = shr(temp1, 3);
sign[1] = temp1 & (Word16)0x0001;
temp1 = Random(seed);
temp2 = temp1 & (Word16)0x0007;
temp2 = add(shl(temp2, 2), temp2);
pos[2] = add(temp2, 2); /* 5 * x + 2 */
temp1 = shr(temp1, 3);
sign[2] = temp1 & (Word16)0x0001;
temp1 = shr(temp1, 1);
temp2 = temp1 & (Word16)0x000F;
pos[3] = add((temp2 & (Word16)1), 3); /* j+3*/
temp2 = (shr(temp2, 1)) & (Word16)7;
temp2 = add(shl(temp2, 2), temp2); /* 5i */
pos[3] = add(pos[3], temp2);
temp1 = shr(temp1, 4);
sign[3] = temp1 & (Word16)0x0001;
Gp = Random(seed) & (Word16)0x1FFF; /* < 0.5 Q14 */
Gp2 = shl(Gp, 1); /* Q15 */
/* Generate gaussian excitation */
/********************************/
L_acc = 0L;
for(i=0; i<L_SUBFR; i++) {
temp1 = Gauss(seed);
L_acc = L_mac(L_acc, temp1, temp1);
excg[i] = temp1;
}
/*
Compute fact = alpha x cur_gain * sqrt(L_SUBFR / Eg)
with Eg = SUM(i=0->39) excg[i]^2
and alpha = 0.5
alpha x sqrt(L_SUBFR)/2 = 1 + FRAC1
*/
L_acc = Inv_sqrt(L_shr(L_acc,1)); /* Q30 */
L_Extract(L_acc, &hi, &lo);
/* cur_gain = cur_gainR << 3 */
temp1 = mult_r(cur_gain, FRAC1);
temp1 = add(cur_gain, temp1);
/* <=> alpha x cur_gainR x 2^2 x sqrt(L_SUBFR) */
L_acc = Mpy_32_16(hi, lo, temp1); /* fact << 17 */
sh = norm_l(L_acc);
temp1 = extract_h(L_shl(L_acc, sh)); /* fact << (sh+1) */
sh = sub(sh, 14);
for(i=0; i<L_SUBFR; i++) {
temp2 = mult_r(excg[i], temp1);
temp2 = shr_r(temp2, sh); /* shl if sh < 0 */
excg[i] = temp2;
}
/* generate random adaptive excitation */
/****************************************/
Pred_lt_3(cur_exc, t0, frac, L_SUBFR);
/* compute adaptive + gaussian exc -> cur_exc */
/**********************************************/
max = 0;
for(i=0; i<L_SUBFR; i++) {
temp1 = mult_r(cur_exc[i], Gp2);
temp1 = add(temp1, excg[i]); /* may overflow */
cur_exc[i] = temp1;
temp1 = abs_s(temp1);
if(sub(temp1,max) > 0) max = temp1;
}
/* rescale cur_exc -> excs */
if(max == 0) sh = 0;
else {
sh = sub(3, norm_s(max));
if(sh <= 0) sh = 0;
}
for(i=0; i<L_SUBFR; i++) {
excs[i] = shr(cur_exc[i], sh);
}
/* Compute fixed code gain */
/***************************/
/**********************************************************/
/*** Solve EQ(X) = 4 X**2 + 2 b X + c */
/**********************************************************/
L_ener = 0L;
for(i=0; i<L_SUBFR; i++) {
L_ener = L_mac(L_ener, excs[i], excs[i]);
} /* ener x 2^(-2sh + 1) */
/* inter_exc = b >> sh */
inter_exc = 0;
for(i=0; i<4; i++) {
j = pos[i];
if(sign[i] == 0) {
inter_exc = sub(inter_exc, excs[j]);
}
else {
inter_exc = add(inter_exc, excs[j]);
}
}
/* Compute k = cur_gainR x cur_gainR x L_SUBFR */
L_acc = L_mult(cur_gain, L_SUBFR);
L_acc = L_shr(L_acc, 6);
temp1 = extract_l(L_acc); /* cur_gainR x L_SUBFR x 2^(-2) */
L_k = L_mult(cur_gain, temp1); /* k << 2 */
temp1 = add(1, shl(sh,1));
L_acc = L_shr(L_k, temp1); /* k x 2^(-2sh+1) */
/* Compute delta = b^2 - 4 c */
L_acc = L_sub(L_acc, L_ener); /* - 4 c x 2^(-2sh-1) */
inter_exc = shr(inter_exc, 1);
L_acc = L_mac(L_acc, inter_exc, inter_exc); /* 2^(-2sh-1) */
sh = add(sh, 1);
/* inter_exc = b x 2^(-sh) */
/* L_acc = delta x 2^(-2sh+1) */
if(L_acc < 0) {
/* adaptive excitation = 0 */
Copy(excg, cur_exc, L_SUBFR);
temp1 = abs_s(excg[(int)pos[0]]) | abs_s(excg[(int)pos[1]]);
temp2 = abs_s(excg[(int)pos[2]]) | abs_s(excg[(int)pos[3]]);
temp1 = temp1 | temp2;
sh = ((temp1 & (Word16)0x4000) == 0) ? (Word16)1 : (Word16)2;
inter_exc = 0;
for(i=0; i<4; i++) {
temp1 = shr(excg[(int)pos[i]], sh);
if(sign[i] == 0) {
inter_exc = sub(inter_exc, temp1);
}
else {
inter_exc = add(inter_exc, temp1);
}
} /* inter_exc = b >> sh */
L_Extract(L_k, &hi, &lo);
L_acc = Mpy_32_16(hi, lo, K0); /* k x (1- alpha^2) << 2 */
temp1 = sub(shl(sh, 1), 1); /* temp1 > 0 */
L_acc = L_shr(L_acc, temp1); /* 4k x (1 - alpha^2) << (-2sh+1) */
L_acc = L_mac(L_acc, inter_exc, inter_exc); /* delta << (-2sh+1) */
Gp = 0;
}
temp2 = Sqrt(L_acc); /* >> sh */
x1 = sub(temp2, inter_exc);
x2 = negate(add(inter_exc, temp2)); /* x 2^(-sh+2) */
if(sub(abs_s(x2),abs_s(x1)) < 0) x1 = x2;
temp1 = sub(2, sh);
g = shr_r(x1, temp1); /* shl if temp1 < 0 */
if(g >= 0) {
if(sub(g, G_MAX) > 0) g = G_MAX;
}
else {
if(add(g, G_MAX) < 0) g = negate(G_MAX);
}
/* Update cur_exc with ACELP excitation */
for(i=0; i<4; i++) {
j = pos[i];
if(sign[i] != 0) {
cur_exc[j] = add(cur_exc[j], g);
}
else {
cur_exc[j] = sub(cur_exc[j], g);
}
}
if(flag_cod != FLAG_DEC) update_exc_err(L_exc_err, Gp, t0);
cur_exc += L_SUBFR;
} /* end of loop on subframes */
return;
}
//重调和方程
bool Biharmonic_PDE(const Biharmonic_PDE_Data& pde,vector<double>& result)
{
double u,temp;
string str;
temp = pde.a*pde.a - 4*pde.b;
if(temp <= 0) // a^2-4b <= 0 不考虑
return false;
u = 0.5*(pde.a + sqrt(temp));
Elliptic_PDE_Data pde_e;
pde_e.var_symbol = pde.var_symbol;
pde_e.xh = pde.xh; pde_e.yh = pde.yh;
pde_e.xLeft = pde.xLeft; pde_e.xRight = pde.xRight;
pde_e.yLeft = pde.yLeft; pde_e.yRight = pde.yRight;
pde_e.pExp = "1";
NumtoString<double>(pde_e.qExp,pde.b/u);
NumtoString<double>(str,-1/u); pde_e.fExp = str + "*(" + pde.fExp + ")";
NumtoString<double>(str,1/u); str = str + "*(" + pde.hExp + ")" + "-(" + pde.gExp + ")";
pde_e.xLeftExp = pde_e.xRightExp = pde_e.yLeftExp = pde_e.yRightExp = str;
vector<double> result_t;
//用求解椭圆形方程的方法求解
if(!Elliptic_PDE(pde_e,result_t))
return false;
{
NumtoString<double>(pde_e.qExp,u);
pde_e.xLeftExp = pde_e.xRightExp = pde_e.yLeftExp = pde_e.yRightExp = pde.gExp;
for(vector<double>::size_type i=0;i<result_t.size();i++)
result_t.at(i) = result_t.at(i) * (-u);
int dim,M,N;
string exp,postexp_xL,postexp_xR,postexp_yL,postexp_yR,
postexp_p,postexp_q;
Expression exp_calc(pde_e.var_symbol);
M = (int)((pde.xRight - pde.xLeft)/pde.xh);
N = (int)((pde.yRight - pde.yLeft)/pde.yh);
dim = (N-1)*(M-1);
Matrix<double> matrix = Matrix<double>(dim,dim+1,0.0); //增广矩阵
result = vector<double>(dim);
result.clear();
//初始化工作
exp = pde_e.xLeftExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_xL);
exp = pde_e.xRightExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_xR);
exp = pde_e.yLeftExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_yL);
exp = pde_e.yRightExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_yR);
exp = pde_e.pExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_p);
exp = pde_e.qExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_q);
//构造增广矩阵
vector<double> values(2);
int pos,level;
double xi,yj;
double ftemp,r1,r2,r3,r4,temp,sum;
for(int j=1;j<=N-1;j++)
{
for(int i=1;i<=M-1;i++)
{
level = (i-1) + (j-1)*(M-1); //判断u(i,j)对应系数矩阵的第几行(从0开始)
//计算f(i,j) 由解出第一个椭圆型方程的解获得
ftemp = result_t.at(level);
/*
xi = pde.xLeft + i*pde.xh;
yj = pde.yLeft + j*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_f,values,ftemp))
return false;
*/
//对(i,j)周围点进行分析
for(int k=1;k<=4;k++)
{
switch(k)
{
case 1: //(i+1,j)点
//计算p(i+1/2,j)
xi = pde.xLeft + (i+0.5)*pde.xh;
yj = pde.yLeft + j*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_p,values,r1))
return false;
r1 = r1 / (pde.xh*pde.xh);
if(i+1 == M) //如果在边界
{
//计算u(M,j)
xi = pde.xLeft + M*pde.xh;
yj = pde.yLeft + j*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_yR,values,temp))
return false;
ftemp += temp*r1;
}
else //如果不在边界
{
pos = (i+1-1) + (j-1)*(M-1);
matrix(level,pos) = -r1;
}
break;
case 2: //(i,j+1)点
//计算p(i,j+1/2)
xi = pde.xLeft + i*pde.xh;
yj = pde.yLeft + (j+0.5)*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_p,values,r2))
return false;
r2 = r2 / (pde.yh*pde.yh);
if(j+1 == N) //如果在边界上
{
//计算u(i,j+1)
xi = pde.xLeft + i*pde.xh;
yj = pde.yLeft + N*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_xR,values,temp))
return false;
ftemp += temp*r2;
}
else
{
pos = (i-1) + (j+1-1)*(M-1);
matrix(level,pos) = -r2;
}
break;
case 3: //计算(i-1,j)点
//计算p(i-1/2,j)
xi = pde.xLeft + (i-0.5)*pde.xh;
yj = pde.yLeft + j*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_p,values,r3))
return false;
r3 = r3 / (pde.xh*pde.xh);
if(i-1 == 0)
{
//计算u(i-1,j)
xi = pde.xLeft;
yj = pde.yLeft + j*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_yL,values,temp))
return false;
ftemp += temp*r3;
}
else
{
pos = (i-1-1) + (j-1)*(M-1);
matrix(level,pos) = -r3;
}
break;
case 4: //计算(i,j-1)
//计算p(i,j-1/2)
xi = pde.xLeft + i*pde.xh;
yj = pde.yLeft + (j-0.5)*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_p,values,r4))
return false;
r4 = r4 / (pde.yh*pde.yh);
if(j-1 == 0)
{
//计算u(i,j-1)
xi = pde.xLeft + i*pde.xh;
yj = pde.yLeft;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_xL,values,temp))
return false;
ftemp += temp*r4;
}
else
{
pos = (i-1) + (j-1-1)*(M-1);
matrix(level,pos) = -r4;
}
break;
}
}
//计算q(i,j)
xi = pde.xLeft + i*pde.xh;
yj = pde.yLeft + j*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_q,values,temp))
return false;
sum = r1+r2+r3+r4;
pos = (i-1) + (j-1)*(M-1);
matrix(level,pos) = sum + temp;
matrix(level,dim) = ftemp;
}
}
//使用Gauss计算方程组
if(!Gauss(matrix,result))
return false;
}
return true;
}
//---------------------------------------------------------------------------
void __fastcall THSVFormOrg::sb_Hue_gainScroll(TObject * Sender,
TScrollCode ScrollCode, int &ScrollPos)
{
double sigma;
if (ScrollCode != scEndScroll) {
return;
}
int tbl_n = 24;
for (int i = 0; i < tbl_n; i++) {
Hue_table_t[i] = Hue_table[i];
Sat_table_t[i] = Sat_table[i];
Val_table_t[i] = Val_table[i];
}
//將使用者輸入的角度換成table表中的index
int tbl_idx = Get_select_idx(StrToFloat(le_ChAangle->Text));
show_gain(sb_Hue_gain->Position, sb_Sat_gain->Position, sb_Val_gain->Position);
int gain_h = sb_Hue_gain->Position; // Hue值為Gain
int gain_s = sb_Sat_gain->Position - Sat_table[tbl_idx]; // 調整值與table數值的差異為gain
int gain_v = sb_Val_gain->Position - Val_table[tbl_idx];
if (gain_h > 768 / 2) {
gain_h -= 768;
} else if (gain_h < (-1) * 768 / 2) {
gain_h += 768;
}
btn_set->Enabled = true;
if (cb_Hue_rotation->Checked == false) {
int tmp_H[9], tmp_S[9], tmp_V[9];
Get_Adj_tbl(tmp_H, tmp_S, tmp_V, tbl_idx);
int low = (4 - sb_dif_n->Position);
int high = sb_dif_p->Position;
bool dif_ok = CheckDif(high, low, tmp_H, gain_h);
if (dif_ok == 0)
return;
sb_dif_p->Position = high;
sb_dif_n->Position = 4 - low;
tmp_S[4] += gain_s;
tmp_V[4] += gain_v;
double ratio;
if (high == 4)
sigma = 0.4;
else if (high == 3)
sigma = 0.5;
else if (high == 2)
sigma = 0.6;
for (int i = 1; i < high; i++) {
ratio = pow(double (high - i) / high, 2) + (double) i / high * Gauss(i, sigma);
tmp_H[i + 4] = tmp_H[i + 4] + gain_h * ratio;
tmp_S[i + 4] = tmp_S[i + 4] + gain_s * ratio;
tmp_V[i + 4] = tmp_V[i + 4] + gain_v * ratio;
}
if (low == 4)
sigma = 0.4;
else if (low == 3)
sigma = 0.5;
else if (low == 2)
sigma = 0.6;
for (int i = 1; i < low; i++) {
ratio = pow(double (low - i) / low, 2) + (double) i / low * Gauss(i, sigma);
tmp_H[4 - i] = tmp_H[4 - i] + gain_h * ratio;
tmp_S[4 - i] = tmp_S[4 - i] + gain_s * ratio;
tmp_V[4 - i] = tmp_V[4 - i] + gain_v * ratio;
}
Set_Adj_tbl(tmp_H, tmp_S, tmp_V, tbl_idx);
if (!CheckHueIncrease(high, low, tmp_H)) {
ShowMessage("Hue value has inverse.");
btn_set->Enabled = false;
return;
}
} else { //rotation
for (int i = 0; i < tbl_n; i++) {
Hue_table_t[i] = (Hue_table[i] + gain_h + 768) % 768;
Sat_table_t[i] = Sat_table[i] + gain_s;
Val_table_t[i] = Val_table[i] + gain_v;
}
}
for (int i = 0; i < tbl_n; i++) {
sg_HSV->Cells[1][i + 1] = FloatToStr((double) Hue_table_t[i] / 768 * 360);
sg_HSV->Cells[2][i + 1] = FloatToStr((double) (Sat_table_t[i]) / 32);
sg_HSV->Cells[3][i + 1] = Val_table_t[i];
}
for (int i = 0; i < 3; i++) {
sg_6HSV->Cells[i][0] = sg_HSV->Cells[i + 1][1]; //Red
sg_6HSV->Cells[i][1] = sg_HSV->Cells[i + 1][5]; //Yellow
sg_6HSV->Cells[i][2] = sg_HSV->Cells[i + 1][9]; //Green
sg_6HSV->Cells[i][3] = sg_HSV->Cells[i + 1][13]; //Cyan
sg_6HSV->Cells[i][4] = sg_HSV->Cells[i + 1][17]; //Blue
sg_6HSV->Cells[i][5] = sg_HSV->Cells[i + 1][21]; //Magenta
sg_6HSV->Cells[i][6] = sg_HSV->Cells[i + 1][tbl_idx + 1];
}
}
//椭圆形方程
bool Elliptic_PDE(const Elliptic_PDE_Data& pde,vector<double>& result)
{
int dim,M,N;
string exp,postexp_xL,postexp_xR,postexp_yL,postexp_yR,
postexp_p,postexp_q,postexp_f;
Expression exp_calc(pde.var_symbol);
if(!(pde.xRight>pde.xLeft && pde.xh>0))
return false;
if(!(pde.yRight>pde.yLeft && pde.yh>0))
return false;
M = (int)((pde.xRight - pde.xLeft)/pde.xh);
N = (int)((pde.yRight - pde.yLeft)/pde.yh);
dim = (N-1)*(M-1);
Matrix<double> matrix = Matrix<double>(dim,dim+1,0.0); //增广矩阵
result = vector<double>(dim); //重置result向量大小
result.clear();
//初始化工作
exp = pde.xLeftExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_xL);
exp = pde.xRightExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_xR);
exp = pde.yLeftExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_yL);
exp = pde.yRightExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_yR);
exp = pde.pExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_p);
exp = pde.qExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_q);
exp = pde.fExp;
exp_calc.DealString(exp);
exp_calc.TransExp(exp,postexp_f);
//构造增广矩阵
vector<double> values(2);
int pos,level;
double xi,yj;
double ftemp,r1,r2,r3,r4,temp,sum;
for(int j=1;j<=N-1;j++)
{
for(int i=1;i<=M-1;i++)
{
level = (i-1) + (j-1)*(M-1); //判断u(i,j)对应系数矩阵的第几行(从0开始)
//计算f(i,j)
xi = pde.xLeft + i*pde.xh;
yj = pde.yLeft + j*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_f,values,ftemp))
return false;
//对(i,j)周围点进行分析
for(int k=1;k<=4;k++)
{
switch(k)
{
case 1: //(i+1,j)点
//计算p(i+1/2,j)
xi = pde.xLeft + (i+0.5)*pde.xh;
yj = pde.yLeft + j*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_p,values,r1))
return false;
r1 = r1 / (pde.xh*pde.xh);
if(i+1 == M) //如果在边界
{
//计算u(M,j)
xi = pde.xLeft + M*pde.xh;
yj = pde.yLeft + j*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_yR,values,temp))
return false;
ftemp += temp*r1;
}
else //如果不在边界
{
pos = (i+1-1) + (j-1)*(M-1);
matrix(level,pos) = -r1;
}
break;
case 2: //(i,j+1)点
//计算p(i,j+1/2)
xi = pde.xLeft + i*pde.xh;
yj = pde.yLeft + (j+0.5)*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_p,values,r2))
return false;
r2 = r2 / (pde.yh*pde.yh);
if(j+1 == N) //如果在边界上
{
//计算u(i,j+1)
xi = pde.xLeft + i*pde.xh;
yj = pde.yLeft + N*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_xR,values,temp))
return false;
ftemp += temp*r2;
}
else
{
pos = (i-1) + (j+1-1)*(M-1);
matrix(level,pos) = -r2;
}
break;
case 3: //计算(i-1,j)点
//计算p(i-1/2,j)
xi = pde.xLeft + (i-0.5)*pde.xh;
yj = pde.yLeft + j*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_p,values,r3))
return false;
r3 = r3 / (pde.xh*pde.xh);
if(i-1 == 0)
{
//计算u(i-1,j)
xi = pde.xLeft;
yj = pde.yLeft + j*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_yL,values,temp))
return false;
ftemp += temp*r3;
}
else
{
pos = (i-1-1) + (j-1)*(M-1);
matrix(level,pos) = -r3;
}
break;
case 4: //计算(i,j-1)
//计算p(i,j-1/2)
xi = pde.xLeft + i*pde.xh;
yj = pde.yLeft + (j-0.5)*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_p,values,r4))
return false;
r4 = r4 / (pde.yh*pde.yh);
if(j-1 == 0)
{
//计算u(i,j-1)
xi = pde.xLeft + i*pde.xh;
yj = pde.yLeft;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_xL,values,temp))
return false;
ftemp += temp*r4;
}
else
{
pos = (i-1) + (j-1-1)*(M-1);
matrix(level,pos) = -r4;
}
break;
}
}
//计算q(i,j)
xi = pde.xLeft + i*pde.xh;
yj = pde.yLeft + j*pde.yh;
values.at(0) = xi; values.at(1) = yj;
if(!exp_calc.CompValue(postexp_q,values,temp))
return false;
sum = r1+r2+r3+r4;
pos = (i-1) + (j-1)*(M-1);
matrix(level,pos) = sum + temp;
matrix(level,dim) = ftemp;
}
}
/*
//矩阵输出到文件
ofstream outf("out_ellipptic.txt");
streambuf *default_buf = cout.rdbuf();
cout.rdbuf(outf.rdbuf());
cout<<matrix;
cout.rdbuf(default_buf);
*/
//使用Gauss计算方程组
if(!Gauss(matrix,result))
return false;
return true;
}
long double MainWindow::CosGauss(double rho)
{
return std::cos(rho - m_rho0)*Gauss(rho)*m_amplitude;
}
void LinearRegressionAnalyzer::DoAnalyze() {
BaseAnalyzer::DoAnalyze();
QList<double> yList = matrix[0];
QList<QList<double> > xMatrix;
for (int i=1;i<matrix.size();i++) {
xMatrix.append(matrix[i]);
}
QList<QList<double> > xNMatrix;
for (int i=0;i<xMatrix.size();i++) {
QList<double> xNiMatrix;
double avg = average(xMatrix[i]);
QList<double> razn;
for (int j=0;j<xMatrix[i].size();j++) {
razn.append(fabs(xMatrix[i][j] - avg));
}
double R = maxElement(razn);
for (int j=0;j<xMatrix[i].size();j++)
xNiMatrix.append((xMatrix[i][j] - avg)/R);
xNMatrix.append(xNiMatrix);
}
QList<double> bMatrix;
double temp = 0.0;
for (int i=0;i<yList.size();i++)
temp += yList[i];
bMatrix.append(temp);
for (int i=0;i<xNMatrix.size();i++) {
temp = 0.0;
for (int j=0;j<xNMatrix[i].size();j++) {
temp += yList[j] * xNMatrix[i][j];
}
bMatrix.append(temp);
}
QList<QList<double> > aMatrix;
QList<double> firstRow;
firstRow.append(yList.size());
for (int i=0;i<xNMatrix.size();i++)
firstRow.append(sumElements(xNMatrix[i]));
aMatrix.append(firstRow);
for (int row=0;row<xNMatrix.size();row++) {
QList<double> tempRow;
for (int col=0;col<bMatrix.size();col++) {
if (col<=row) {
tempRow.append(0.0);
}
else {
if (col>0) {
tempRow.append(sumListsElements(xNMatrix[col-1], xNMatrix[row]));
}
}
}
aMatrix.append(tempRow);
}
for (int row = 0;row<aMatrix.size();row++) {
for (int col = 0; col < aMatrix[row].size();col++) {
if (row <= col)
continue;
aMatrix[row][col] = aMatrix[col][row];
}
}
results = Gauss(aMatrix, bMatrix);
QList<double> y2List;
for (int i=0;i<xNMatrix[0].size();i++) {
temp = 0.0;
for (int j= 0;j<results.size();j++) {
if (j<results.size()-1)
temp += results[j] * xNMatrix[j][i];
else
temp += results[j];
}
y2List.append(temp);
}
double y2Avg = average(y2List);
double yAvg = average(yList);
double RUp = 0.0, RDown = 0.0;
for (int i=0;i<yList.size();i++) {
RUp += (y2List[i]-y2Avg)*(y2List[i]-y2Avg);
RDown += (yList[i] - yAvg)*(yList[i] - yAvg);
}
R2 = RUp/RDown;
}
