double GLGPU3DDataset::Flux(int face) const { // TODO: pre-compute the flux switch (face) { case 0: return -dx() * dy() * Bz(); case 1: return -dy() * dz() * Bx(); case 2: return -dz() * dx() * By(); case 3: return dx() * dy() * Bz(); case 4: return dy() * dz() * Bx(); case 5: return dz() * dx() * By(); default: assert(false); } return 0.0; }
static void test_mgcr(ITER *ip, int i, MAT *Q, MAT *R) #endif { VEC vt, vt1; static MAT *R1 = MNULL; static VEC *r = VNULL, *r1 = VNULL; VEC *rr; int k, j; Real sm; /* check Q*Q^T = I */ vt.dim = vt.max_dim = ip->b->dim; vt1.dim = vt1.max_dim = ip->b->dim; Q = m_resize(Q, i + 1, ip->b->dim); R1 = m_resize(R1, i + 1, i + 1); r = v_resize(r, ip->b->dim); r1 = v_resize(r1, ip->b->dim); MEM_STAT_REG(R1, TYPE_MAT); MEM_STAT_REG(r, TYPE_VEC); MEM_STAT_REG(r1, TYPE_VEC); m_zero(R1); for (k = 1; k <= i; k++) for (j = 1; j <= i; j++) { vt.ve = Q->me[k]; vt1.ve = Q->me[j]; R1->me[k][j] = in_prod(&vt, &vt1); } for (j = 1; j <= i; j++) R1->me[j][j] -= 1.0; #ifndef MEX if (m_norm_inf(R1) > MACHEPS * ip->b->dim) printf(" ! (mgcr:) m_norm_inf(Q*Q^T) = %g\n", m_norm_inf(R1)); #endif /* check (r_i,Ap_j) = 0 for j <= i */ ip->Ax(ip->A_par, ip->x, r); v_sub(ip->b, r, r); rr = r; if (ip->Bx) { ip->Bx(ip->B_par, r, r1); rr = r1; } #ifndef MEX printf(" ||r|| = %g\n", v_norm2(rr)); #endif sm = 0.0; for (j = 1; j <= i; j++) { vt.ve = Q->me[j]; sm = max(sm, in_prod(&vt,rr)); } #ifndef MEX if (sm >= MACHEPS * ip->b->dim) printf(" ! (mgcr:) max_j (r,Ap_j) = %g\n", sm); #endif }
// ABGR int value_to_color_888(float v, float max) { int s; int R; int G; int B; float fact = max; v = v / fact; B = (int)255. * Bx(v); G = (int)255. * Gx(v); R = (int)255. * Rx(v); s = (R & 0xFF) + ((G & 0xFF) << 8) + ((B & 0xFF) << 16); return s; }
// Normalises a value v in 0. and 1. and converts that to a rgb 565 short int int value_to_color_565(float v, float max) { int s; int R; int G; int B; float fact = max; v = v / fact; B = (int)31. * Bx(v); G = (int)63. * Gx(v); R = (int)31. * Rx(v); s = (R % 32) + ((G % 64) << 5) + ((B % 32) << 11); // if (v == 1.) // s = 0xFE00; //printf("vla=%f, max=%f\n", v, max); return s; }
int sc_main( int ac, char* av[] ) { sc_uint_base Bx(WIDTH), By(WIDTH); for(unsigned int i = 0; i < WIDTH; i++){ cout << "i = " << i << endl; for(unsigned int j=0; j < COUNT; j++ ){ /* ( By.range(WIDTH-1, i+1), By.range(i,0) ) = Bx; sc_assert( By == Bx ); */ Bx = j; sc_assert( (bool) Bx[i] == (j & (1<<i)? 1 : 0) ); By[i] = Bx[i]; sc_assert( (bool) By[i] == (j & (1<<i)? 1 : 0) ); } } return 0; }
float GLGPUDataset::QP(const float X0[], const float X1[]) const { const float *L = Lengths(), *O = Origins(); float d[3] = {X1[0] - X0[0], X1[1] - X0[1], X1[2] - X0[2]}; int p[3] = {0}; // 0: not crossed; 1: positive; -1: negative for (int i=0; i<3; i++) { d[i] = X1[i] - X0[i]; if (d[i]>L[i]/2) {d[i] -= L[i]; p[i] = 1;} else if (d[i]<-L[i]/2) {d[i] += L[i]; p[i] = -1;} } const float X[3] = {X0[0] - O[0], X0[1] - O[1], X0[2] - O[2]}; if (By()>0 && p[0]!=0) { // By>0 return p[0] * L[0] * (Bz()*X[1] - By()*X[2]); } else if (p[1]!=0) { return p[1] * L[1] * (Bx()*X[2] - Bz()*X[0]); } else return 0.0; }
int sc_main( int ac, char* av[] ) { sc_int_base Bx(WIDTH),By(WIDTH); for(unsigned int i = 0; i < WIDTH; i++){ cout << "i = " << i << endl; for(unsigned int j=0; j < COUNT; j++ ){ /* ( By.range(WIDTH-1, i+1), By.range(i,0) ) = Bx; assert( By == Bx ); */ Bx = j; By.range(WIDTH-1,i) = Bx.range(WIDTH-1,i); if(i >= 1) By.range(i-1,0) = Bx.range(i-1,0); assert( Bx == By ); } } return 0; }
double GLGPU3DDataset::GaugeTransformation(const double X0[], const double X1[]) const { double gx, gy, gz; double dx = X1[0] - X0[0], dy = X1[1] - X0[1], dz = X1[2] - X0[2]; double x = X0[0] + 0.5*dx, y = X0[1] + 0.5*dy, z = X0[2] + 0.5*dz; if (By()>0) { // Y-Z gauge gx = dx * Kex(); gy =-dy * x * Bz(); // -dy*x^hat*Bz gz = dz * x * By(); // dz*x^hat*By } else { // X-Z gauge gx = dx * y * Bz() + dx * Kex(); // dx*y^hat*Bz + dx*K gy = 0; gz =-dz * y * Bx(); // -dz*y^hat*Bx } return gx + gy + gz; }
void STWarp<T>::multiscaleIteration(WarpingField<T> &warpField) { for (int warpIter = 0; warpIter < params.warpIterations; ++warpIter) { if(params.verbosity > 1) { printf(" - warp iteration %02d\n",warpIter+1); } // Get derivatives if(params.verbosity > 1) { printf(" - computing derivatives..."); } Video<T> Bx(videoB->size()); Video<T> By(videoB->size()); Video<T> Bt(videoB->size()); Video<T> C(videoB->size()); computePartialDerivatives(warpField,Bx,By,Bt,C); if(params.verbosity >1) { printf("done.\n"); } // marginal warpField update WarpingField<T> dWarpField = WarpingField<T>(warpField.size()); warpingIteration(warpField, Bx, By, Bt, C, dWarpField); if (params.limitUpdate) { if(params.verbosity > 1) { printf(" - limiting warp update to [-1,1]"); } dWarpField.clamp(-1,1); } // w <- w + dw warpField.add(dWarpField); denoiseWarpingField(warpField); } // end of warp iteration }
/* Loads an binary STL file by filename * Returns 0 on success and -1 on failure */ int Shape::loadBinarySTL(string filename) { // if(getFileType(filename) != BINARY_STL) { // return -1; // } triangles.clear(); Min.x = Min.y = Min.z = numeric_limits<double>::infinity(); Max.x = Max.y = Max.z = -numeric_limits<double>::infinity(); ifstream file; file.open(filename.c_str()); if(file.fail()) { cerr << _("Error: Unable to open stl file - ") << filename << endl; return -1; } /* Binary STL files have a meaningless 80 byte header * followed by the number of triangles */ file.seekg(80, ios_base::beg); unsigned int num_triangles; unsigned char buffer[4]; file.read(reinterpret_cast <char *> (buffer), 4); // Read platform independent 32-bit little-endian int. num_triangles = buffer[0] | buffer[1] << 8 | buffer[2] << 16 | buffer[3] << 24; triangles.reserve(num_triangles); for(uint i = 0; i < num_triangles; i++) { double a,b,c; a = read_double (file); b = read_double (file); c = read_double (file); Vector3d N(a,b,c); a = read_double (file); b = read_double (file); c = read_double (file); Vector3d Ax(a,b,c); a = read_double (file); b = read_double (file); c = read_double (file); Vector3d Bx(a,b,c); a = read_double (file); b = read_double (file); c = read_double (file); Vector3d Cx(a,b,c); // done in Triangle /* Recalculate normal vector - can't trust an STL file! */ // Vector3d AA=Cx-Ax; // Vector3d BB=Cx-Bx; // N = AA.cross(BB).getNormalized(); /* attribute byte count - sometimes contains face color information but is useless for our purposes */ unsigned short byte_count; file.read(reinterpret_cast <char *> (buffer), 2); byte_count = buffer[0] | buffer[1] << 8; // Repress unused variable warning. (void)&byte_count; Triangle T(Ax,Bx,Cx); //cout << "bin triangle "<< N << ":\n\t" << Ax << "/\n\t"<<Bx << "/\n\t"<<Cx << endl; triangles.push_back(T); } file.close(); CenterAroundXY(); scale_factor = 1.0; scale_factor_x=scale_factor_y=scale_factor_z = 1.0; double vol = volume(); if (vol < 0) { invertNormals(); vol = -vol; } cout << _("Shape has volume ") << vol << " mm^3"<<endl; return 0; }
void Filter::Kalman(Joint joint, double &dx, double &dy) { Kalmans[Kalman_count++] = joint; Kalman_count = Kalman_count % Kalman_limit; Kalman_num++; if (Kalman_num > Kalman_limit) { Kalman_num = Kalman_limit; } if (Kalman_num < Kalman_limit) { dx = joint.Position.X; dy = joint.Position.Y; return; } else { //X, Y int haha; haha = 1; double x[5], y[5]; int pos = Kalman_count; for (int i = 0; i < 5; i++) { x[i] = Kalmans[pos].Position.X; y[i] = Kalmans[pos].Position.Y; pos++; pos = pos%Kalman_limit; } //求系数Ax, Ay double Ax[5] = { /*a0*/ x[0], /*a1*/ 4 * (x[1] - x[0]) - 3 * x[2] + 4 * x[3] / 3 - x[4] / 4, /*a2*/ 11 * x[4] / 24 - 7 * x[3] / 3 + 19 * x[2] / 4 - 13 * (x[1] - x[0]) / 3, /*a3*/ x[4] / 3 - 7 * x[3] / 6 + x[2] - (x[1] - x[0]) / 2, /*a4*/ (x[4] - 4 * x[3] + 6 * x[2] + 4 * (x[1] - x[0])) / 24 }; double Ay[5] = { /*a0*/ y[0], /*a1*/ 4 * (y[1] - y[0]) - 3 * y[2] + 4 * y[3] / 3 - y[4] / 4, /*a2*/ 11 * y[4] / 24 - 7 * y[3] / 3 + 19 * y[2] / 4 - 13 * (y[1] - y[0]) / 3, /*a3*/ y[4] / 3 - 7 * y[3] / 6 + y[2] - (y[1] - y[0]) / 2, /*a4*/ (y[4] - 4 * y[3] + 6 * y[2] + 4 * (y[1] - y[0])) / 24 }; //求转换矩阵Fx, Fy Matrix Fx(4, 4, new double[16]{ 1, 1, -0.5, (Ax[1] + 6 * Ax[3] - 4 * Ax[4]) / (24 * Ax[4]), 0, 1, 1, 0.5, 0, 0, 1, 1, 0, 0, 0, 1}); Matrix Fy(4, 4, new double[16]{ 1, 1, -0.5, (Ay[1] + 6 * Ay[3] - 4 * Ay[4]) / (24 * Ay[4]), 0, 1, 1, 0.5, 0, 0, 1, 1, 0, 0, 0, 1}); //求ε(t|t-1) Matrix ex(4, 4), ey(4, 4); ex = Fx*Kalman_ex*(!Fx); ey = Fy*Kalman_ey*(!Fy); //cout << "ex" << endl; ex.print(); //cout << "ey" << endl; ey.print(); Matrix Bx(4, 1), By(4, 1); //cout << "!Kalman_C" << endl; (!Kalman_C).print(); //cout << "Kalman_vx" << endl; Kalman_vx.print(); //cout << "Kalman_C" << endl; Kalman_C.print(); //cout << "!Kalman_C" << endl; (!Kalman_C).print(); //cout << "Kalman_C*ex" << endl; (Kalman_C*ex).print(); //cout << "Kalman_C*ex*(!Kalman_C)" << endl; (Kalman_C*ex*(!Kalman_C)).print(); //cout << "(~(Kalman_vx + Kalman_C*ex*(!Kalman_C)))" << endl; //(~(Kalman_vx + Kalman_C*ex*(!Kalman_C))).print(); Bx = ex*(!Kalman_C)*(~(Kalman_vx + Kalman_C*ex*(!Kalman_C))); //cout << "Bx" << endl; Bx.print(); By = ey*(!Kalman_C)*(~(Kalman_vy + Kalman_C*ey*(!Kalman_C))); Matrix I4(4, 4); I4.SetIdentity(); Kalman_Sx = (I4 - Bx*Kalman_C)*(Fx*Kalman_Sx + Kalman_Gx) + Bx*Matrix(1, 1, new double[1] {joint.Position.X}); //cout << "Kalman_Sx" << endl; Kalman_Sx.print(); Kalman_Sy = (I4 - By*Kalman_C)*(Fy*Kalman_Sy + Kalman_Gy) + By*Matrix(1, 1, new double[1] {joint.Position.Y}); Kalman_ex = ex - Bx*Kalman_C*ex; Kalman_ey = ey - By*Kalman_C*ey; dx = Kalman_Sx.at(0, 0); dy = Kalman_Sy.at(0, 0); } }
float Az(const float X[3], int slot=0) const {if (By()>0) return -X[0]*By(); else return X[1]*Bx();}
VEC *iter_cg1(ITER *ip) #endif { STATIC VEC *r = VNULL, *p = VNULL, *q = VNULL, *z = VNULL; Real alpha; double inner,nres; VEC *rr; /* rr == r or rr == z */ if (ip == INULL) error(E_NULL,"iter_cg"); if (!ip->Ax || !ip->b) error(E_NULL,"iter_cg"); if ( ip->x == ip->b ) error(E_INSITU,"iter_cg"); if (!ip->stop_crit) error(E_NULL,"iter_cg"); if ( ip->eps <= 0.0 ) ip->eps = MACHEPS; r = v_resize(r,ip->b->dim); p = v_resize(p,ip->b->dim); q = v_resize(q,ip->b->dim); MEM_STAT_REG(r,TYPE_VEC); MEM_STAT_REG(p,TYPE_VEC); MEM_STAT_REG(q,TYPE_VEC); if (ip->Bx != (Fun_Ax)NULL) { z = v_resize(z,ip->b->dim); MEM_STAT_REG(z,TYPE_VEC); rr = z; } else rr = r; if (ip->x != VNULL) { if (ip->x->dim != ip->b->dim) error(E_SIZES,"iter_cg"); ip->Ax(ip->A_par,ip->x,p); /* p = A*x */ v_sub(ip->b,p,r); /* r = b - A*x */ } else { /* ip->x == 0 */ ip->x = v_get(ip->b->dim); ip->shared_x = FALSE; v_copy(ip->b,r); } if (ip->Bx) (ip->Bx)(ip->B_par,r,p); else v_copy(r,p); inner = in_prod(p,r); nres = sqrt(fabs(inner)); if (ip->info) ip->info(ip,nres,r,p); if ( nres == 0.0) return ip->x; for ( ip->steps = 0; ip->steps <= ip->limit; ip->steps++ ) { ip->Ax(ip->A_par,p,q); inner = in_prod(q,p); if (sqrt(fabs(inner)) <= MACHEPS*ip->init_res) error(E_BREAKDOWN,"iter_cg1"); alpha = in_prod(p,r)/inner; v_mltadd(ip->x,p,alpha,ip->x); v_mltadd(r,q,-alpha,r); rr = r; if (ip->Bx) { ip->Bx(ip->B_par,r,z); rr = z; } nres = in_prod(r,rr); if (nres < 0.0) { warning(WARN_RES_LESS_0,"iter_cg"); break; } nres = sqrt(fabs(nres)); if (ip->info) ip->info(ip,nres,r,z); if (ip->steps == 0) ip->init_res = nres; if ( ip->stop_crit(ip,nres,r,z) ) break; alpha = -in_prod(rr,q)/inner; v_mltadd(rr,p,alpha,p); } #ifdef THREADSAFE V_FREE(r); V_FREE(p); V_FREE(q); V_FREE(z); #endif return ip->x; }
bool File::load_binarySTL(vector<Triangle> &triangles, uint max_triangles, bool readnormals) { ifstream file; ustring filename = _file->get_path(); file.open(filename.c_str(), ifstream::in | ifstream::binary); if(file.fail()) { cerr << _("Error: Unable to open stl file - ") << filename << endl; return false; } // cerr << "loading bin " << filename << endl; /* Binary STL files have a meaningless 80 byte header * followed by the number of triangles */ file.seekg(80, ios_base::beg); unsigned int num_triangles; unsigned char buffer[4]; file.read(reinterpret_cast <char *> (buffer), 4); // Read platform independent 32-bit little-endian int. num_triangles = buffer[0] | buffer[1] << 8 | buffer[2] << 16 | buffer[3] << 24; uint step = 1; if (max_triangles > 0 && max_triangles < num_triangles) { step = ceil(num_triangles/max_triangles); triangles.reserve(max_triangles); } else triangles.reserve(num_triangles); uint i = 0; for(; i < num_triangles; i+=step) { if (step>1) file.seekg(84 + 50*i, ios_base::beg); double a,b,c; a = read_double (file); b = read_double (file); c = read_double (file); Vector3d N(a,b,c); a = read_double (file); b = read_double (file); c = read_double (file); Vector3d Ax(a,b,c); a = read_double (file); b = read_double (file); c = read_double (file); Vector3d Bx(a,b,c); a = read_double (file); b = read_double (file); c = read_double (file); Vector3d Cx(a,b,c); if (file.eof()) { cerr << _("Unexpected EOF reading STL file - ") << filename << endl; break; } /* attribute byte count - sometimes contains face color information but is useless for our purposes */ unsigned short byte_count; file.read(reinterpret_cast <char *> (buffer), 2); byte_count = buffer[0] | buffer[1] << 8; // Repress unused variable warning. (void)&byte_count; Triangle T = Triangle(Ax,Bx,Cx); if (readnormals) if (T.Normal.dot(N) < 0) T.invertNormal(); // cout << "bin triangle "<< N << ":\n\t" << Ax << "/\n\t"<<Bx << "/\n\t"<<Cx << endl; triangles.push_back(T); } file.close(); return true; // cerr << "Read " << i << " triangles of " << num_triangles << " from file" << endl; }