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);
}
}
}
