void LinearSolver::eigen_system( DENS_MAT & eigenvalues, DENS_MAT & eigenvectors, const DENS_MAT * M) /* const */
{
initialize_matrix(); // no inverse needed
const DENS_MAT * Kp = NULL;
const DENS_MAT * Mp =M;
DENS_MAT MM;
DENS_MAT KM;
if (constraintHandlerType_ == CONDENSE_CONSTRAINTS) {
Kp = &matrixFreeFree_;
if (M) {
DENS_MAT MfreeFixed; // not used
M->row_partition(freeSet_,MM,MfreeFixed);
Mp = &MM;
}
}
else {
if (matrixDense_.nRows() == 0) matrixDense_ =matrixSparse_->dense_copy();
Kp = &matrixDense_;
}
if (!M) {
MM.identity(Kp->nRows());
Mp = &MM;
}
DENS_MAT eVecs, eVals;
eVecs = eigensystem(*Kp,*Mp,eVals);
eigenvalues.reset(nVariables_,1);
eigenvectors.reset(nVariables_,nVariables_);
set<int>::const_iterator itr;
for (int i = 0; i < Kp->nRows(); i++) { // ordering is by energy not node
eigenvalues(i,0) = eVals(i,0);
int j = 0;
for (itr = freeSet_.begin(); itr != freeSet_.end(); itr++,j++) {
int jj = *itr;
eigenvectors(jj,i) = eVecs(j,i); // transpose
}
}
}
void p4_setPramsPartTest(p4_tree *aTree, int pNum)
{
int mNum;
int nNum, cNum, rNum;
int dim;
int i, j;
int ret;
double alpha, beta;
p4_modelPart *mp;
//part *dp;
p4_gdasrv *g;
p4_rMatrix *r;
p4_node *n;
p4_comp *c;
p4_bigQAndEig *aQE;
printf("a p4_setPramsPartTest here. pNum=%i\n", pNum);
#if 1
mp = aTree->model->parts[pNum];
if(mp->nCat > 1) {
// Set gdasrv->rates using Yang's DiscreteGamma function.
for(mNum = 0; mNum < mp->nGdasrvs; mNum++) {
g = mp->gdasrvs[mNum];
// All rates were set when the gdasrv was given its
// val, in pf_p4_setGdasrvVal() in pfmodule.c. We
// need only re-do the ones with a free shape.
if(g->free) {
printf("part %i, gdasrv %i, is free, with val %f\n", pNum, mNum, g->val[0]);
// int DiscreteGamma (double freqK[], double rK[], double alfa, double beta, int K, int median)
// Here alfa = beta. The freqK array is used as a
// working space for internal calcs by
// DiscreteGamma, but boringly ends up as all 1.0/K
DiscreteGamma(g->freqs, g->rates, g->val[0], g->val[0], mp->nCat, 0);
if(1) {
printf("gdasrv[%i] = %f, free=%i\n", mNum, g->val[0], g->free);
for(i = 0; i < mp->nCat; i++) {
printf(" %i %f %f\n", i, g->freqs[i], g->rates[i]);
}
}
}
}
}
#endif
#if 1
// Set rMatrix for 2p (ie nst=2) models (k2p and hky)
mp = aTree->model->parts[pNum];
dim = mp->dim;
for(mNum = 0; mNum < mp->nRMatrices; mNum++) {
r = mp->rMatrices[mNum];
if(r->free && (r->spec == RMATRIX_2P)) {
//printf("part %i, rMatrix %i, is free. spec = %i\n", pNum, mNum, r->spec);
alpha = 1.0 / 3.0;
beta = alpha * r->kappa[0];
r->bigR[0][0] = 0.0;
r->bigR[0][1] = alpha;
r->bigR[0][2] = beta;
r->bigR[0][3] = alpha;
r->bigR[1][0] = alpha;
r->bigR[1][1] = 0.0;
r->bigR[1][2] = alpha;
r->bigR[1][3] = beta;
r->bigR[2][0] = beta;
r->bigR[2][1] = alpha;
r->bigR[2][2] = 0.0;
r->bigR[2][3] = alpha;
r->bigR[3][0] = alpha;
r->bigR[3][1] = beta;
r->bigR[3][2] = alpha;
r->bigR[3][3] = 0.0;
}
}
#endif
// bigQ
/*
Each part has a bigQAndEigThing. It is nComps * nRMatrices.
Each slot has a bigQAndEig struct, but at the start of play they
are all empty. They are only filled and the qEig calculated if
needed.
*/
//printf("about to set bigQ.\n");
// Nasty surprise. If a charFreq is zero, eg if the charFreq is
// (0.4, 0.4, 0.2, 0.0), then the bigP's are *wrong* --- way
// wrong. However, its ok if eg the charFreq is (0.4, 0.4,
// 0.199999, 0.000001). In previous versions I used to check and
// adjust, but here it is just prevented, elsewhere. In
// optimizations, the PIVEC_MIN is enforced.
#if 0
// Check comps anyway. This interferes with mcmc when I do verifyModelPrams().
{
double sum=0.0;
mp = aTree->model->parts[pNum];
//printf(" part %i, nComps=%i, nRMatrices=%i, nCat=%i\n", pNum, mp->nComps, mp->nRMatrices, mp->nCat);
for(i = 0; i < mp->nComps; i++) {
for(j = 0; j < mp->dim; j++) {
if(mp->comps[i]->val[j] < (0.5 * PIVEC_MIN)) {
printf("p4_setPramsPart() part %i, comp %i, value %i is %g Bad.\n", pNum, i, j, mp->comps[i]->val[j]);
exit(1);
}
}
// check that the sum is 1.0
sum = 0.0;
for(j = 0; j < mp->dim; j++) {
sum += mp->comps[i]->val[j];
}
// normalize if needed
if(fabs(sum - 1.0) > 1e-15) {
printf(" ** p4_setPramsPart(). normalizing comp. diff=%f (%g)\n", fabs(sum - 1.0), fabs(sum - 1.0));
if(fabs(sum - 1.0) > 0.001) {
printf("p4_setPramsPart() part %i, comp %i, sum of vals = %f. Bad.\n", pNum, i, sum);
}
for(j = 0; j < mp->dim; j++) {
mp->comps[i]->val[j] /= sum;
}
}
// check again
sum = 0.0;
for(j = 0; j < mp->dim; j++) {
sum += mp->comps[i]->val[j];
}
// if its wrong this time, give up (diff of 1e-16 was too small, raised to 1e-14)
if(fabs(sum - 1.0) > 1e-14) {
printf("p4_setPramsPart() part %i, comp %i, values do not sum to 1.0. sum=%g\n", pNum, i, sum);
printf(" sum - 1.0 = %g\n", sum - 1.0);
exit(1);
}
}
}
#endif
#if 1
// Check comps anyway. Same as above, but no normalizing. Its only
// a check -- it doesn't actually do anything.
{
double sum=0.0;
mp = aTree->model->parts[pNum];
//printf(" part %i, nComps=%i, nRMatrices=%i, nCat=%i\n", pNum, mp->nComps, mp->nRMatrices, mp->nCat);
for(i = 0; i < mp->nComps; i++) {
for(j = 0; j < mp->dim; j++) {
if(mp->comps[i]->val[j] < (0.5 * PIVEC_MIN)) {
printf("p4_setPramsPart() part %i, comp %i, value %i is %g Bad.\n", pNum, i, j, mp->comps[i]->val[j]);
exit(1);
}
}
// check
sum = 0.0;
for(j = 0; j < mp->dim; j++) {
sum += mp->comps[i]->val[j];
}
// if its wrong this time, give up (diff of 1e-16 was too small, raised to 1e-14)
if(fabs(sum - 1.0) > 1e-14) {
printf("**p4_setPramsPart() part %i, comp %i, values do not sum to 1.0. sum=%g\n", pNum, i, sum);
printf("** sum - 1.0 = %g\n", sum - 1.0);
exit(1);
}
}
}
#endif
#if 1
// turn on all needsReset
mp = aTree->model->parts[pNum];
//printf(" part %i, nComps=%i, nRMatrices=%i, nCat=%i\n", pNum, mp->nComps, mp->nRMatrices, mp->nCat);
for(i = 0; i < mp->nComps; i++) {
for(j = 0; j < mp->nRMatrices; j++) {
mp->bQETneedsReset[(i * mp->nRMatrices) + j] = 1;
}
}
mp = aTree->model->parts[pNum];
dim = mp->dim;
for(nNum = 0; nNum < aTree->nNodes; nNum++) {
n = aTree->nodes[nNum];
if(n != aTree->root) {
cNum = n->compNums[pNum];
rNum = n->rMatrixNums[pNum];
//printf("cNum = %i, rNum = %i\n", cNum, rNum);
c = mp->comps[cNum];
r = mp->rMatrices[rNum];
aQE = aTree->model->parts[pNum]->bigQAndEigThing[cNum][rNum];
if(mp->bQETneedsReset[(cNum * mp->nRMatrices) + rNum]) {
//printf("Doing comp %i, rMatrix %i\n", cNum, rNum);
if(!aQE->bigQ) {
aQE->bigQ = psdmatrix(dim);
setBigQFromRMatrixDotCharFreq(aQE->bigQ, r->bigR, c->val, dim);
normalizeBigQ(aQE->bigQ, c->val, dim);
aQE->qEig = allocEig(dim, aQE->bigQ);
}
else {
//dump_psdmatrix(r->bigR, dim);
//setBigQFromRMatrixDotCharFreq(m->bigQ, theRMatrix, theCharFreq, m->dim);
setBigQFromRMatrixDotCharFreq(aQE->bigQ, r->bigR, c->val, dim);
//dump_psdmatrix(aQE->bigQ, dim);
//normalizeBigQ(m->bigQ, theCharFreq, m->dim);
normalizeBigQ(aQE->bigQ, c->val, dim);
//dump_psdmatrix(aQE->bigQ, dim);
}
ret = eigensystem(aQE->qEig);
if(ret) {
printf("There is a problem with the eigensystem.\n");
exit(1);
}
mp->bQETneedsReset[(cNum * mp->nRMatrices) + rNum] = 0; // No need to do it again for this
// combination of cNum and rNum on another node.
}
}
}
#endif
#if 0
for(cNum = 0; cNum < aTree->model->parts[pNum]->nComps; cNum++) {
for(rNum = 0; rNum < aTree->model->parts[pNum]->nRMatrices; rNum++) {
aQE = aTree->model->parts[pNum]->bigQAndEigThing[cNum][rNum];
printf("cNum=%i, rNum=%i, mp->needsReset=%i, ", cNum, rNum,
mp->bQETneedsReset[(cNum * mp->nRMatrices) + rNum]);
printf("aQE->bigQ=%li\n", (long int)(aQE->bigQ));
}
}
#endif
#if 0
printf("bigQ\n");
dump_psdmatrix(aTree->model->parts[pNum]->bigQAndEigThing[0][0]->bigQ, aTree->model->parts[pNum]->dim);
#endif
#if 1
//printf("about to set bigP\n");
// for each node except the root, "calculateBigPDecks()"
for(nNum = 0; nNum < aTree->nNodes; nNum++) {
if(nNum != NO_ORDER) {
n = aTree->nodes[nNum];
if(n != aTree->root) {
p4_calculateBigPDecksPart(n, pNum);
}
}
}
#endif
#if 0
printf("bigP for node 1\n");
dump_psdmatrix(aTree->nodes[1]->bigPDecks[0][0], aTree->model->parts[pNum]->dim);
//dump_psdmatrix(aTree->nodes[1]->bigPDecks[0][1], aTree->model->parts[pNum]->dim);
//printf("bigP for node 2\n");
//dump_psdmatrix(aTree->nodes[2]->bigPDecks[0][0], aTree->model->parts[pNum]->dim);
#endif
}
void ContactAngle2::doFrame(int frame) {
StuntDouble* sd;
int i;
// set up the bins for density analysis
Mat3x3d hmat = info_->getSnapshotManager()->getCurrentSnapshot()->getHmat();
RealType len = std::min(hmat(0, 0), hmat(1, 1));
RealType zLen = hmat(2,2);
RealType dr = len / (RealType) nRBins_;
RealType dz = zLen / (RealType) nZBins_;
std::vector<std::vector<RealType> > histo;
histo.resize(nRBins_);
for (unsigned int i = 0; i < histo.size(); ++i){
histo[i].resize(nZBins_);
std::fill(histo[i].begin(), histo[i].end(), 0.0);
}
if (evaluator1_.isDynamic()) {
seleMan1_.setSelectionSet(evaluator1_.evaluate());
}
Vector3d com(centroidX_, centroidY_, solidZ_);
// now that we have the centroid, we can make cylindrical density maps
Vector3d pos;
RealType r;
RealType z;
for (sd = seleMan1_.beginSelected(i); sd != NULL;
sd = seleMan1_.nextSelected(i)) {
pos = sd->getPos() - com;
// r goes from zero upwards
r = sqrt(pow(pos.x(), 2) + pow(pos.y(), 2));
// z is possibly symmetric around 0
z = pos.z();
int whichRBin = int(r / dr);
int whichZBin = int( (zLen/2.0 + z) / dz);
if ((whichRBin < int(nRBins_)) && (whichZBin >= 0)
&& (whichZBin < int(nZBins_))) {
histo[whichRBin][whichZBin] += sd->getMass();
}
}
for(unsigned int i = 0 ; i < histo.size(); ++i){
RealType rL = i * dr;
RealType rU = rL + dr;
RealType volSlice = NumericConstant::PI * dz * (( rU*rU ) - ( rL*rL ));
for (unsigned int j = 0; j < histo[i].size(); ++j) {
histo[i][j] *= PhysicalConstants::densityConvert / volSlice;
}
}
std::vector<Vector<RealType, 2> > points;
points.clear();
for (unsigned int j = 0; j < nZBins_; ++j) {
// The z coordinates were measured relative to the selection
// center of mass. However, we're interested in the elevation
// above the solid surface. Also, the binning was done around
// zero with enough bins to cover the zLength of the box:
RealType thez = com.z() - solidZ_ - zLen/2.0 + dz * (j + 0.5);
bool aboveThresh = false;
bool foundThresh = false;
int rloc = 0;
for (std::size_t i = 0; i < nRBins_; ++i) {
if (histo[i][j] >= threshDens_) aboveThresh = true;
if (aboveThresh && (histo[i][j] <= threshDens_)) {
rloc = i;
foundThresh = true;
aboveThresh = false;
}
}
if (foundThresh) {
Vector<RealType,2> point;
point[0] = dr*(rloc+0.5);
point[1] = thez;
if (thez > bufferLength_) {
points.push_back( point );
}
}
}
int numPoints = points.size();
// Compute the average of the data points.
Vector<RealType, 2> average = points[0];
int i0;
for (i0 = 1; i0 < numPoints; ++i0) {
average += points[i0];
}
RealType invNumPoints = ((RealType)1)/(RealType)numPoints;
average *= invNumPoints;
DynamicRectMatrix<RealType> mat(4, 4);
int row, col;
for (row = 0; row < 4; ++row) {
for (col = 0; col < 4; ++col){
mat(row,col) = 0.0;
}
}
for (int i = 0; i < numPoints; ++i) {
RealType x = points[i][0];
RealType y = points[i][1];
RealType x2 = x*x;
RealType y2 = y*y;
RealType xy = x*y;
RealType r2 = x2+y2;
RealType xr2 = x*r2;
RealType yr2 = y*r2;
RealType r4 = r2*r2;
mat(0,1) += x;
mat(0,2) += y;
mat(0,3) += r2;
mat(1,1) += x2;
mat(1,2) += xy;
mat(1,3) += xr2;
mat(2,2) += y2;
mat(2,3) += yr2;
mat(3,3) += r4;
}
mat(0,0) = (RealType)numPoints;
for (row = 0; row < 4; ++row) {
for (col = 0; col < row; ++col) {
mat(row,col) = mat(col,row);
}
}
for (row = 0; row < 4; ++row) {
for (col = 0; col < 4; ++col) {
mat(row,col) *= invNumPoints;
}
}
JAMA::Eigenvalue<RealType> eigensystem(mat);
DynamicRectMatrix<RealType> evects(4, 4);
DynamicVector<RealType> evals(4);
eigensystem.getRealEigenvalues(evals);
eigensystem.getV(evects);
DynamicVector<RealType> evector = evects.getColumn(0);
RealType inv = ((RealType)1)/evector[3]; // beware zero divide
RealType coeff[3];
for (row = 0; row < 3; ++row) {
coeff[row] = inv*evector[row];
}
Vector<RealType, 2> center;
center[0] = -((RealType)0.5)*coeff[1];
center[1] = -((RealType)0.5)*coeff[2];
RealType radius = sqrt(fabs(center[0]*center[0] + center[1]*center[1]
- coeff[0]));
int i1;
for (i1 = 0; i1 < 100; ++i1) {
// Update the iterates.
Vector<RealType, 2> current = center;
// Compute average L, dL/da, dL/db.
RealType lenAverage = (RealType)0;
Vector<RealType, 2> derLenAverage = Vector<RealType, 2>(0.0);
for (i0 = 0; i0 < numPoints; ++i0) {
Vector<RealType, 2> diff = points[i0] - center;
RealType length = diff.length();
if (length > 1e-6) {
lenAverage += length;
RealType invLength = ((RealType)1)/length;
derLenAverage -= invLength*diff;
}
}
lenAverage *= invNumPoints;
derLenAverage *= invNumPoints;
center = average + lenAverage*derLenAverage;
radius = lenAverage;
Vector<RealType, 2> diff = center - current;
if (fabs(diff[0]) <= 1e-6 && fabs(diff[1]) <= 1e-6) {
break;
}
}
RealType zCen = center[1];
RealType rDrop = radius;
RealType ca;
if (fabs(zCen) > rDrop) {
ca = 180.0;
} else {
ca = 90.0 + asin(zCen/rDrop)*(180.0/M_PI);
}
values_.push_back( ca );
}
