static void mdlOutputs(SimStruct *S,int_T tid) { InputRealPtrsType uPtrs0 = ssGetInputPortRealSignalPtrs(S,0); InputRealPtrsType uPtrs1 = ssGetInputPortRealSignalPtrs(S,1); real_T prev = ssGetRWorkValue(S,0); bool dataPort = PARAM(2)[0]; int_T i; #ifndef MATLAB_MEX_FILE rosShmData_t *shm = (rosShmData_t *)ssGetPWorkValue(S,0); SEM *sem = (SEM *)ssGetPWorkValue(S,1); #endif char_T *msg; unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1); UNUSED_ARG(tid); /* not used in single tasking mode */ if (U0(0) > 0.5 && U0(0) > prev) { msg = (char_T *)malloc(strlen); mxGetString(ssGetSFcnParam(S,1), msg, strlen); #ifndef MATLAB_MEX_FILE if (dataPort) { for (i = 0; i < ssGetInputPortWidth(S,1); ++i) { asprintf(&msg, "%s %f", msg, U1(i)); } } if (rt_sem_wait_if(sem) != 0) { memcpy(shm->msg.text, msg, MAX_LOG_MSG_SIZE); shm->msg.state = NEW_VALUE; rt_sem_signal(sem); } #else switch ((int)PARAM(0)[0]) { case 1: printf("DEBUG"); break; case 2: printf("INFO"); break; case 3: printf("WARN"); break; case 4: printf("ERROR"); break; case 5: printf("FATAL"); break; default: printf("NONE"); break; } printf(": %s", msg); if (dataPort) { for (i = 0; i < ssGetInputPortWidth(S,1); ++i) { printf(" %f", U1(i)); } } printf("\n"); #endif free(msg); } ssSetRWorkValue(S,0,U0(0)); }
void ChainStiffnessKernel::get_per_layer_dynamical_matrices(double qx, double qy, double_complex **D, double *xcshift, double *ycshift) { if (nu_ != 1) { error_->all(FLERR,"ChainStiffnessKernel::get_per_layer_dynamical_matrices:" "Only works for nu == 1."); } SquareMatrix<double_complex> U0(dim_, D[0]), U(dim_, D[1]), V(dim_, D[2]); U0.fill_with(0.0); U.fill_with(0.0); V.fill_with(0.0); U0[0][0] = 1.0; U0[1][1] = 1.0; U0[2][2] = 1.0; U[0][0] = 2.0; U[1][1] = 2.0; U[2][2] = 2.0; V[0][0] = -1.0; V[1][1] = -1.0; V[2][2] = -1.0; }
void TriInverse(UpperTriMatrixView<T> U, ptrdiff_t nhi) { #ifdef XDEBUG Matrix<T> U0(U); #endif if (U.size() > 0) { if (nhi == 0) DiagMatrixViewOf(U.diag()).invertSelf(); else if (U.iscm() || U.isrm()) DoInverse(U,nhi); else { UpperTriMatrix<T> temp = U; DoInverse(temp.view(),nhi); U = temp; } } #ifdef XDEBUG Matrix<T> eye = U*U0; if (Norm(eye-T(1)) > 0.0001*(Norm(U0)+Norm(U))) { cerr<<"UpperTriMatrix Inverse:\n"; cerr<<"U = "<<TMV_Text(U)<<" "<<U0<<endl; cerr<<"Uinv = "<<U<<endl; cerr<<"Uinv*U = "<<U*U0<<endl; cerr<<"U*Uinv = "<<U0*U<<endl; abort(); } #endif }
bool ElasticBeam::initElement (const std::vector<int>& MNPC, const FiniteElement& fe, const Vec3&, size_t, LocalIntegral& elmInt) { if (!this->initElement(MNPC,elmInt)) return false; Vec3 e1 = fe.XC[1] - fe.XC[0]; // Initial local X-axis const Vector& eV = elmInt.vec.front(); if (!eV.empty()) { // Fetch nodal displacements Vec3 U0(eV.ptr()); Vec3 U1(eV.ptr()+eV.size()-npv); e1 += U1 - U0; // Deformed local X-axis } // Calculate the co-rotated element coordinate system if (e1.normalize() <= 1.0e-8) { std::cerr <<" *** ElasticBeam::initElement: Zero beam length"<< std::endl; return false; } if (fe.Tn.size() < 2) { std::cerr <<" *** ElasticBeam::initElement: No end rotations"<< std::endl; return false; } Vec3 e2(fe.Tn[0][1]+fe.Tn[1][1]); // Sum of the nodal Y-axes Vec3 e3(fe.Tn[0][2]+fe.Tn[1][2]); // Sum of the nodal Z-axes if (e3*e1 < e2*e1) { e2.cross(e3,e1); // Local Y-axis = e3xe1 / |e3xe1| e2.normalize(); e3.cross(e1,e2); // Local Z-axis = e1xe2 } else { e3.cross(e1,e2); // Local Z-axis = e1xe2 / |e1xe2| e3.normalize(); e2.cross(e3,e1); // Local Y-axis = e3xe1 } Matrix& Tlg = this->getLocalAxes(elmInt); Tlg.fillColumn(1,e1.ptr()); Tlg.fillColumn(2,e2.ptr()); Tlg.fillColumn(3,e3.ptr()); #if INT_DEBUG > 1 std::cout <<"ElasticBeam: local-to-global transformation matrix:"<< Tlg <<"ElasticBeam: T1n\n"<< fe.Tn[0] <<"ElasticBeam: T2n\n"<< fe.Tn[1]; #endif return true; }
int sc_main( int sc_argc, char *sc_argv[] ) { // Elaboration countdown U0("U0"); // Simulation sc_start(); // Cleanup return 0; }
//---------------------------------------------------------------------------- void ConvexHull2D::GenerateHullManyCollinear () { // Generate a lot of nearly collinear points. mNumVertices = 128; delete1(mVertices); mVertices = new1<Vector2f>(mNumVertices); Vector2f center(0.5f, 0.5f); Vector2f U0(Mathf::SymmetricRandom(), Mathf::SymmetricRandom()); U0.Normalize(); Vector2f U1 = U0.Perp(); float e0 = 0.5f*Mathf::UnitRandom(); float e1 = 0.5f*Mathf::UnitRandom(); float t; int i; for (i = 0; i < mNumVertices/4; ++i) { t = i/(mNumVertices/4.0f); mVertices[i] = center - e0*U0 - e1*U1 + 2.0f*e0*t*U0; } for (i = 0; i < mNumVertices/4; i++) { t = i/(mNumVertices/4.0f); mVertices[i + mNumVertices/4] = center + e0*U0 - e1*U1 + 2.0f*e1*t*U1; } for (i = 0; i < mNumVertices/4; i++) { t = i/(mNumVertices/4.0f); mVertices[i + mNumVertices/2] = center + e0*U0 + e1*U1 - 2.0f*e0*t*U0; } for (i = 0; i < mNumVertices/4; i++) { t = i/(mNumVertices/4.0f); mVertices[i + 3*mNumVertices/4] = center - e0*U0 + e1*U1 - 2.0f*e1*t*U1; } RegenerateHull(); assertion(mHull->GetDimension() == 2, "Incorrect dimension.\n"); }
void frameSolver2d::solve() { #if defined(HAVE_PETSC) linearSystemPETSc<double> *lsys = new linearSystemPETSc<double>; #elif defined(HAVE_GMM) linearSystemCSRGmm<double> *lsys = new linearSystemCSRGmm<double>; lsys->setGmres(1); lsys->setNoisy(1); #else linearSystemFull<double> *lsys = new linearSystemFull<double>; #endif if(pAssembler) delete pAssembler; pAssembler = new dofManager<double>(lsys); // fix dofs and create free ones createDofs(); // force vector std::vector<std::pair<GVertex *, std::vector<double> > >::iterator it = _nodalForces.begin(); for(; it != _nodalForces.end(); ++it) { MVertex *v = it->first->mesh_vertices[0]; const std::vector<double> &F = it->second; Dof DOFX(v->getNum(), 0); Dof DOFY(v->getNum(), 1); pAssembler->assemble(DOFX, F[0]); pAssembler->assemble(DOFY, F[1]); } // stifness matrix for(std::size_t i = 0; i < _beams.size(); i++) { fullMatrix<double> K(6, 6); computeStiffnessMatrix(i, K); _beams[i]._stiffness = K; MVertex *v0 = _beams[i]._element->getVertex(0); MVertex *v1 = _beams[i]._element->getVertex(1); Dof theta0(v0->getNum(), Dof::createTypeWithTwoInts(2, _beams[i]._rotationTags[0])); Dof theta1(v1->getNum(), Dof::createTypeWithTwoInts(2, _beams[i]._rotationTags[1])); Dof U0(v0->getNum(), 0); Dof U1(v1->getNum(), 0); Dof V0(v0->getNum(), 1); Dof V1(v1->getNum(), 1); Dof DOFS[6] = {U0, V0, theta0, U1, V1, theta1}; for(int j = 0; j < 6; j++) { for(int k = 0; k < 6; k++) { pAssembler->assemble(DOFS[j], DOFS[k], K(j, k)); } } } lsys->systemSolve(); // save the solution for(std::size_t i = 0; i < _beams.size(); i++) { MVertex *v0 = _beams[i]._element->getVertex(0); MVertex *v1 = _beams[i]._element->getVertex(1); Dof theta0(v0->getNum(), Dof::createTypeWithTwoInts(2, _beams[i]._rotationTags[0])); Dof theta1(v1->getNum(), Dof::createTypeWithTwoInts(2, _beams[i]._rotationTags[1])); Dof U0(v0->getNum(), 0); Dof U1(v1->getNum(), 0); Dof V0(v0->getNum(), 1); Dof V1(v1->getNum(), 1); Dof DOFS[6] = {U0, V0, theta0, U1, V1, theta1}; for(int j = 0; j < 6; j++) { pAssembler->getDofValue(DOFS[j], _beams[i]._displacement[j]); } } delete lsys; delete pAssembler; }
void MagneticBubbleBoundary::set_bc_z0_wall(std::valarray<double> &U) const { const int Nx = Mara->domain->get_N(1); const int Ny = Mara->domain->get_N(2); const int Ng = Mara->domain->get_Ng(); const double r0 = rotation_radius; ValarrayManager M(Mara->domain->aug_shape(), Mara->domain->get_Nq()); for (int i=0; i<Nx+2*Ng; ++i) { for (int j=0; j<Ny+2*Ng; ++j) { for (int k=0; k<Ng; ++k) { double x = Mara->domain->x_at(i); double y = Mara->domain->y_at(j); double r = sqrt(x*x + y*y); double Omega; switch (rotation_profile) { case RIGID_ROTATION: if (r < r0) { Omega = 1.0; } else { Omega = 0.0; } break; case RIGID_IN_KEPLARIAN_OUT: if (r < 0.5*r0) { Omega = 1.0; } else if (r > 2*r0) { Omega = 0.0; } else { Omega = pow(2*r/r0, -1.5); } break; } std::valarray<double> U1 = U[ M(i,j,2*Ng-k-1) ]; // reflected zone std::valarray<double> P1(8); // reflected zone primitive variables std::valarray<double> U0(8); std::valarray<double> P0(8); if (receive_primitive) { P1 = U1; // U was actually P (confusing, I know) } else { int err = Mara->fluid->ConsToPrim(&U1[0], &P1[0]); if (err) { /* fprintf(stderr, "[MagneticBubbleBoundary] unphysical boundary value\n"); std::cerr << Mara->fluid->PrintCons(&U1[0]) << std::endl; std::cerr << rmhd_c2p_get_error(err) << std::endl; */ // exit(1); continue; } } /* not reflecting vz ensures that v < 1 in the guard zones */ P0[0] = P1[0]; P0[1] = P1[1]; P0[2] = -y/r0 * Omega; P0[3] = x/r0 * Omega; P0[4] = 0.0;//-P1[4]; P0[5] = -P1[5]; P0[6] = -P1[6]; P0[7] = P1[7]; if (receive_primitive) { U0 = P0; // U really is P int err = rmhd_c2p_check_prim(&P0[0]); if (err) { std::cerr << rmhd_c2p_get_error(err) << std::endl; exit(1); } } else { int err = Mara->fluid->PrimToCons(&P0[0], &U0[0]); if (err) { std::cerr << rmhd_c2p_get_error(err) << std::endl; exit(1); } } U[ M(i,j,k) ] = U0; } } } }
void two_phase_3d_op_explicit(double phi[M][N][P], const double u0[M][N][P], double curvature_motion_part[M][N][P], double dt, double c1, double c2) { double mu = TP_MU; double nu = TP_NU; double lambda1 = TP_LAMBDA1; double lambda2 = TP_LAMBDA2; double dx = 1.0; double dy = 1.0; double dz = 1.0; double dx2 = dx * 2.0; double dy2 = dy * 2.0; double dz2 = dz * 2.0; double Dx_p, Dx_m; double Dy_p, Dy_m; double Dz_p, Dz_m; double Dx_0, Dy_0, Dz_0; double Dxx, Dyy, Dzz; double Dxy, Dxz, Dyz; double Grad, K; double stencil[3][3][3]; #pragma AP array_partition variable=stencil complete dim=0 double numer, denom; uint32_t i, j, k, l; for (i = 1; i < M - 1; i++) { for (j = 1; j < N - 1; j++) { for (k = 1; k < P - 1; k++) { #pragma AP pipeline /* stencil code */ stencil[0][0][0] = stencil[0][0][1]; stencil[0][1][0] = stencil[0][1][1]; stencil[0][2][0] = stencil[0][2][1]; stencil[0][0][1] = stencil[0][0][2]; stencil[0][1][1] = stencil[0][1][2]; stencil[0][2][1] = stencil[0][2][2]; stencil[0][0][2] = PHI(i - 1, j - 1, k + 1); stencil[0][1][2] = PHI(i - 1, j, k + 1); stencil[0][2][2] = PHI(i - 1, j + 1, k + 1); stencil[1][0][0] = stencil[1][0][1]; stencil[1][1][0] = stencil[1][2][1]; stencil[1][2][0] = stencil[1][2][1]; stencil[1][0][1] = stencil[1][0][2]; stencil[1][1][1] = stencil[1][1][2]; stencil[1][2][1] = stencil[1][2][2]; stencil[1][0][2] = PHI(i, j - 1, k + 1); stencil[1][1][2] = PHI(i, j, k + 1); stencil[1][2][2] = PHI(i, j + 1, k + 1); stencil[2][0][0] = stencil[2][0][1]; stencil[2][1][0] = stencil[2][1][1]; stencil[2][2][0] = stencil[2][2][1]; stencil[2][0][1] = stencil[2][0][2]; stencil[2][1][1] = stencil[2][1][2]; stencil[2][2][1] = stencil[2][2][2]; stencil[2][0][2] = PHI(i + 1, j - 1, k + 1); stencil[2][1][2] = PHI(i + 1, j, k + 1); stencil[2][2][2] = PHI(i + 1, j + 1, k + 1); /* regular calculation here */ Dx_p = (stencil[2][1][1] - stencil[1][1][1]) / dx; Dx_m = (stencil[1][1][1] - stencil[0][1][1]) / dx; Dy_p = (stencil[1][2][1] - stencil[1][1][1]) / dy; Dy_m = (stencil[1][1][1] - stencil[1][0][1]) / dy; Dz_p = (stencil[1][1][2] - stencil[1][1][1]) / dz; Dz_m = (stencil[1][1][1] - stencil[1][1][0]) / dz; Dx_0 = (stencil[2][1][1] - stencil[0][1][1]) / dx2; Dy_0 = (stencil[1][2][1] - stencil[1][0][1]) / dy2; Dz_0 = (stencil[1][1][2] - stencil[1][1][0]) / dz2; Dxx = (Dx_p - Dx_m) / dx; Dyy = (Dy_p - Dy_m) / dy; Dzz = (Dz_p - Dz_m) / dz; Dxy = (stencil[2][2][1] - stencil[2][0][1] - stencil[0][2][1] - stencil[0][0][1]) / (4 * dx * dy); Dxz = (stencil[2][1][2] - stencil[2][1][0] - stencil[0][1][2] + stencil[0][1][0]) / (4 * dx * dz); Dyz = (stencil[1][2][2] - stencil[1][2][0] - stencil[1][0][2] + stencil[1][0][0]) / (4 * dy * dz); Grad = (SQR(Dx_0) + SQR(Dy_0) + SQR(Dz_0)); denom = Grad; /* denom = denom^1.5 */ for (l = 0; l < 3; l++) { #pragma AP unroll denom *= denom; } q3_sqrt(denom); numer = (Dx_0 * Dx_0 * Dyy - 2.0 * Dx_0 * Dy_0 * Dxy + Dy_0 * Dy_0 * Dxx + Dx_0 * Dx_0 * Dzz - 2.0 * Dx_0 * Dz_0 * Dxz + Dz_0 * Dz_0 * Dxx + Dy_0 * Dy_0 * Dzz - 2.0 * Dy_0 * Dz_0 * Dyz + Dz_0 * Dz_0 * Dyy); K = numer / denom; CMP(i, j, k) = Grad * (mu * K + lambda1 * (U0(i, j, k) - c1) * (U0(i, j, k) - c1) - lambda2 * (U0(i, j, k) - c2) * (U0(i, j, k) - c2)); } } } neumann_bc(curvature_motion_part); for (k = 0; k < P; k++) { for (j = 0; j < N; j++) { for (i = 0; i < M; i++) { #pragma AP pipeline PHI(i, j, k) += CMP(i, j, k) * dt; } } } }
tmp<fvVectorMatrix> surfaceShearForce::correct(volVectorField& U) { // local reference to film model const kinematicSingleLayer& film = static_cast<const kinematicSingleLayer&>(owner_); // local references to film fields const volScalarField& mu = film.mu(); const volVectorField& Uw = film.Uw(); const volScalarField& delta = film.delta(); const volVectorField& Up = film.UPrimary(); // film surface linear coeff to apply to velocity tmp<volScalarField> tCs; typedef compressible::turbulenceModel turbModel; if (film.primaryMesh().foundObject<turbModel>("turbulenceProperties")) { // local reference to turbulence model const turbModel& turb = film.primaryMesh().lookupObject<turbModel>("turbulenceProperties"); // calculate and store the stress on the primary region const volSymmTensorField primaryReff(turb.devRhoReff()); // create stress field on film // - note boundary condition types (mapped) // - to map, the field name must be the same as the field on the // primary region volSymmTensorField Reff ( IOobject ( primaryReff.name(), film.regionMesh().time().timeName(), film.regionMesh(), IOobject::NO_READ, IOobject::NO_WRITE ), film.regionMesh(), dimensionedSymmTensor ( "zero", primaryReff.dimensions(), symmTensor::zero ), film.mappedFieldAndInternalPatchTypes<symmTensor>() ); // map stress from primary region to film region Reff.correctBoundaryConditions(); dimensionedScalar U0("SMALL", U.dimensions(), SMALL); tCs = Cf_*mag(-film.nHat() & Reff)/(mag(Up - U) + U0); } else { // laminar case - employ simple coeff-based model const volScalarField& rho = film.rho(); tCs = Cf_*rho*mag(Up - U); } dimensionedScalar d0("SMALL", delta.dimensions(), SMALL); // linear coeffs to apply to velocity const volScalarField& Cs = tCs(); volScalarField Cw("Cw", mu/(0.3333*(delta + d0))); Cw.min(1.0e+06); return ( - fvm::Sp(Cs, U) + Cs*Up // surface contribution - fvm::Sp(Cw, U) + Cw*Uw // wall contribution ); }
/*! * Function returns fac[] and D=[U0, ... V W] at this q vector * Note: called each time the 'run' or 'minimize' command is called * Multi-atom unit cells written as single atom cells, per Appendix A.3 */ void FTStiffnessKernel::get_dynamical_matrices(double qx, double qy, double_complex *xU0, double_complex *xU, double_complex *xV, double_complex dU) { // Number of atoms per surface unit cell. int nat = crystal_surface_->get_number_of_atoms(); assert(dim_ == 3*nat); mat<double_complex> U0(dim_, xU0); mat<double_complex> U(dim_, xU); mat<double_complex> V(dim_, xV); // Set all matrices to zero. U0.fill_with(0.0); U.fill_with(0.0); V.fill_with(0.0); mat<double, 3> cell(crystal_surface_->get_cell()); // Add off-site terms to U matrices. int nn = crystal_surface_->get_number_of_neighbors(); const CrystalSurface::lattice_neighbor_t *neigh = crystal_surface_->get_neighbors(); for (int n = 0; n < nn; n++, neigh++) { // Get index of first atom. int i = neigh->indexi; // Get distance in number of cells. This determines the phase factor // below. double cx[3]; crystal_surface_->cell_distance(i, *neigh, cx); // Compute phase. double_complex phase = exp(COMPLEX_NUMBER(0, cx[0]*qx+cx[1]*qy)); // ab tells us whether it's the U(ab==0),V(ab==1),W,... matrix int ab = nearbyint(cx[2]); assert(std::abs(ab-cx[2]) < 1e-6); // Everything with ab < -1 and ab > 1 is ignored since those are // neighbors that are farther than one layer away. If the // interaction reaches this far the size of the unit cell should // be increased. if (ab >= -1 && ab <= 1) { // Get index of second atom. int j = neigh->indexj; // Get force constants. //force_constants_->offsite(crystal_surface_, neigh, Y.data()); mat<double> Y(3, &D0ij_[9*n]); mat<double> Z(3, &D1ij_[9*n]); if (ab == 0) { // This needs to be added to the appropriate subblock of // U0 and U. for (int ii = 0; ii < 3; ii++) { for (int jj = 0; jj < 3; jj++) { U0[3*i+ii][3*j+jj] += phase*Y[ii][jj]; U[3*i+ii][3*j+jj] += phase*Z[ii][jj]; } } } else if (ab == -1) { // This needs to be added to the appropriate subblock of V. for (int ii = 0; ii < 3; ii++) { for (int jj = 0; jj < 3; jj++) { V[3*i+ii][3*j+jj] += phase*Z[ii][jj]; } } } } } // Add on-site terms to U0 and U. for (int i = 0; i < nat; i++) { //force_constants_->onsite(crystal_surface_, i, 0, Y.data()); mat<double> Y(3, &D0ii_[9*i]); for (int ii = 0; ii < 3; ii++) { for (int jj = 0; jj < 3; jj++) { U0[3*i+ii][3*i+jj] += Y[ii][jj]; } } //force_constants_->onsite(crystal_surface_, i, -1, Y.data()); mat<double> Z(3, &D1ii_[9*i]); for (int ii = 0; ii < 3; ii++) { for (int jj = 0; jj < 3; jj++) { U[3*i+ii][3*i+jj] += Z[ii][jj]; } } } #ifdef GFMD_DEBUG if (std::abs(qx) < 1e-6 && std::abs(qy) < 1e-6) { double_complex trU0 = 0.0, trU = 0.0; for (int i = 0; i < dim_; i++) { trU0 += U0[i][i]; trU += U[i][i]; } std::cout << "USER-GFMD: DEBUG - trace[U0(q=0)] = " << creal(trU0) << ", trace[U(q=0)] = " << creal(trU) << std::endl; } #endif }
void baz(int i) { if (!i) throw U0(); else throw U1(); }
vector<double> Plink::calcMantelHaenszel_IxJxK(vector<int> & X, vector<int> & Y, vector<int> & Z) { if (X.size() != Y.size() || Y.size() != Z.size() || X.size() != Z.size() ) error("Internal problem:\n problem in calcMantelHaenszel_IxJxK()...uneven input columns"); // Determine unique elements int nx=0, ny=0, nz=0; map<int,int> mx; map<int,int> my; map<int,int> mz; for (unsigned int i=0; i<X.size(); i++) { if (mx.find(X[i]) == mx.end()) mx.insert(make_pair(X[i],nx++)); if (my.find(Y[i]) == my.end()) my.insert(make_pair(Y[i],ny++)); if (mz.find(Z[i]) == mz.end()) mz.insert(make_pair(Z[i],nz++)); } // Generic function to calculate generalized IxJxK CMH // Assumes no missing data vector< vector<double> > N(nz); // observed counts vector< vector<double> > U(nz); // expected vector< vector< vector<double> > > V(nz); // variance matrix vector<vector<double> > Tx(nz); // marginal totals vector<vector<double> > Ty(nz); // .. vector<double> T(nz); // totals (per K) for (int k=0; k<nz; k++) { Tx[k].resize(nx); Ty[k].resize(ny); N[k].resize((nx-1)*(ny-1)); U[k].resize((nx-1)*(ny-1)); V[k].resize((nx-1)*(ny-1)); for (int k2=0; k2<(nx-1)*(ny-1); k2++) { N[k][k2] = U[k][k2] = 0; V[k][k2].resize((nx-1)*(ny-1)); for (int k3=0; k3<(nx-1)*(ny-1); k3++) V[k][k2][k3] = 0; } } // Consider each observation for (int i=0; i<X.size(); i++) { int vx = mx.find(X[i])->second; int vy = my.find(Y[i])->second; int vz = mz.find(Z[i])->second; // exclude nx + ny (upper limits) if (vx<nx-1 && vy<ny-1) N[vz][ vx + vy*(nx-1) ]++; Tx[vz][vx]++; Ty[vz][vy]++; T[vz]++; } // Determine valid clusters (at least 2 people) vector<bool> validK(nk,false); for (int k=0; k<nk; k++) if (T[k]>=2) validK[k]=true; // Calculate expecteds for (int k=0; k<nz; k++) { if (validK[k]) { for (int ix=0; ix<nx-1; ix++) for (int iy=0; iy<ny-1; iy++) { U[k][ix+iy*(nx-1)] = ( Tx[k][ix] * Ty[k][iy] ) / T[k]; for (int ix2=0; ix2<nx-1; ix2++) for (int iy2=0; iy2<ny-1; iy2++) { int dx=0; int dy=0; if (ix==ix2) dx=1; if (iy==iy2) dy=1; V[k][ix + iy*(nx-1)][ix2 + iy2*(nx-1)] = ( ( Tx[k][ix] * ( dx * T[k] - Tx[k][ix2] ) * Ty[k][iy] * ( dy *T[k] - Ty[k][iy2] ) ) / ( T[k]*T[k]*(T[k]-1) ) ); if (ix==ix2 && iy==iy2) V[k][ix + iy*(nx-1)][ix2 + iy2*(nx-1)] = abs(V[k][ix + iy*(nx-1)][ix2 + iy2*(nx-1)]); } } } } vector<vector<double> > V0((nx-1)*(ny-1)); for (int k2=0; k2<(nx-1)*(ny-1); k2++) V0[k2].resize((nx-1)*(ny-1)); vector<double> N0((nx-1)*(ny-1)); vector<double> U0((nx-1)*(ny-1)); // Sum N, U and V over K for (int k=0; k<nz; k++) { if (validK[k]) { for (int i=0; i<(nx-1)*(ny-1); i++) { N0[i] += N[k][i]; U0[i] += U[k][i]; for (int i2=0; i2<(nx-1)*(ny-1); i2++) V0[i][i2] += V[k][i][i2]; } } } bool flag = true; vector<double> tmp1((nx-1)*(ny-1),0); vector<double> tmp2((nx-1)*(ny-1),0); V0 = svd_inverse(V0,flag); for (int i=0; i<(nx-1)*(ny-1); i++) tmp1[i] = N0[i] - U0[i]; // Matrix mult -- rows by columns for (int i=0; i<(nx-1)*(ny-1); i++) for (int j=0; j<(nx-1)*(ny-1); j++) tmp2[j] += tmp1[i] * V0[i][j]; vector<double> result(2); // CMH Chi-square result[0]=0; for (int i=0; i<(nx-1)*(ny-1); i++) result[0] += tmp2[i] * tmp1[i]; // DF result[1] = (nx-1)*(ny-1); return result; }
void SumProduct::accumulateEigenCounts (vguard<vguard<double> >& rootCounts, vguard<vguard<vguard<gsl_complex> > >& eigenCounts, double weight) const { LogThisAt(8,"Accumulating eigencounts, column " << join(gappedCol,"") << ", weight " << weight << endl); accumulateRootCounts (rootCounts, weight); const auto rootNode = columnRoot(); const int A = model.alphabetSize(); vguard<double> U (A), D (A); vguard<gsl_complex> Ubasis (A), Dbasis (A); vguard<double> U0 (A), D0 (A); for (auto node : ungappedRows) if (node != rootNode) { LogThisAt(9,"Accumulating eigencounts, column " << join(gappedCol,"") << " node " << tree.seqName(node) << endl); const TreeNodeIndex parent = tree.parentNode(node); const TreeNodeIndex sibling = tree.getSibling(node); for (int cpt = 0; cpt < components(); ++cpt) { LogThisAt(9,"Accumulating eigencounts, column " << join(gappedCol,"") << " node " << tree.seqName(node) << " component #" << cpt << endl); const vguard<double>& U0 = F[cpt][node]; for (AlphTok i = 0; i < A; ++i) D0[i] = G[cpt][parent][i] * E[cpt][sibling][i]; const double maxU0 = *max_element (U0.begin(), U0.end()); const double maxD0 = *max_element (D0.begin(), D0.end()); const double norm = exp (colLogLike - logCptWeight[cpt] - logF[cpt][node] - logG[cpt][parent] - logE[cpt][sibling]) / (maxU0 * maxD0); // U[b] = U0[b] / maxU0; Ubasis[l] = sum_b U[b] * evecInv[l][b] for (AlphTok b = 0; b < A; ++b) U[b] = U0[b] / maxU0; for (AlphTok l = 0; l < A; ++l) { Ubasis[l] = gsl_complex_rect (0, 0); for (AlphTok b = 0; b < A; ++b) Ubasis[l] = gsl_complex_add (Ubasis[l], gsl_complex_mul_real (gsl_matrix_complex_get (eigen.evecInv[cpt], l, b), U[b])); } // D[a] = D0[a] / maxD0; Dbasis[k] = sum_a D[a] * evec[a][k] for (AlphTok a = 0; a < A; ++a) D[a] = D0[a] / maxD0; for (AlphTok k = 0; k < A; ++k) { Dbasis[k] = gsl_complex_rect (0, 0); for (AlphTok a = 0; a < A; ++a) Dbasis[k] = gsl_complex_add (Dbasis[k], gsl_complex_mul_real (gsl_matrix_complex_get (eigen.evec[cpt], a, k), D[a])); } // R = evec * evals * evecInv // exp(RT) = evec * exp(evals T) * evecInv // count(i,j|a,b,T) = Q / exp(RT)_ab // where... // Q = \sum_a \sum_b \int_{t=0}^T D_a exp(Rt)_ai R_ij exp(R(T-t))_jb U_b dt // = \sum_a \sum_b \int_{t=0}^T D_a (\sum_k evec_ak exp(eval_k t) evecInv_ki) R_ij (\sum_l evec_jl exp(eval_l (T-t)) evecInv_lb) U_b dt // = \sum_a \sum_b D_a \sum_k evec_ak evecInv_ki R_ij \sum_l evec_jl evecInv_lb U_b \int_{t=0}^T exp(eval_k t) exp(eval_l (T-t)) dt // = \sum_a \sum_b D_a \sum_k evec_ak evecInv_ki R_ij \sum_l evec_jl evecInv_lb U_b eigenSubCount(k,l,T) // = R_ij \sum_k evecInv_ki \sum_l evec_jl (\sum_a D_a evec_ak) (\sum_b U_b evecInv_lb) eigenSubCount(k,l,T) // = R_ij \sum_k evecInv_ki \sum_l evec_jl Dbasis_k Ubasis_l eigenSubCount(k,l,T) // eigenCounts[k][l] += Dbasis[k] * eigenSub[k][l] * Ubasis[l] / norm for (AlphTok k = 0; k < A; ++k) for (AlphTok l = 0; l < A; ++l) eigenCounts[cpt][k][l] = gsl_complex_add (eigenCounts[cpt][k][l], gsl_complex_mul_real (gsl_complex_mul (Dbasis[k], gsl_complex_mul (gsl_matrix_complex_get (branchEigenSubCount[cpt][node], k, l), Ubasis[l])), weight / norm)); // LogThisAt(9,"colLogLike=" << colLogLike << " logF[cpt][node]=" << logF[cpt][node] << " logG[cpt][parent]=" << logG[cpt][parent] << " logE[cpt][sibling]=" << logE[cpt][sibling] << " maxU0=" << maxU0 << " maxD0=" << maxD0 << endl); // LogThisAt(8,"Component #" << cpt << " eigencounts matrix (norm=" << norm << "):" << endl << complexMatrixToString(eigenCounts[cpt]) << endl); } } }