void COrthogonalArray::oarand(int is, int js, int ks, int ls)
{
m_randomClass.seed(is, js, ks, ls);
std::vector<int> pi = std::vector<int>(m_q);
for (int j = 0; j < m_ncol; j++)
{
rutils::unifperm(pi, m_q, m_randomClass);
for (int i = 0; i < m_nrow; i++)
{
m_A(i,j) = pi[ m_A(i,j) ];
}
}
}
int COrthogonalArray::oaagree(bool verbose)
{
int agree, maxagr;
int mrow1, mrow2;
maxagr = mrow1 = mrow2 = 0;
for (int i = 0; i < m_nrow; i++)
{
for (int j = i+1; j < m_nrow; j++)
{
agree = 0;
for (int k = 0; k < m_ncol; k++)
{
agree += (m_A(i,k) == m_A(j,k));
}
if (agree > maxagr)
{
maxagr = agree;
mrow1 = i;
mrow2 = j;
if (verbose)
{
PRINT_OUTPUT("New max %d %d %d\n", i, j, agree);
}
}
}
if (i && i % ROWCHECK == 0 && verbose)
{
PRINT_OUTPUT("Checked rows <= %d vs all other rows.\n",i);
}
}
if (verbose)
{
if (maxagr == 0)
{
PRINT_OUTPUT("No two distinct rows agree in any columns.\n");
}
else
{
PRINT_OUTPUT("Maximum number of columns matching for two distinct rows is %d.\n", maxagr);
PRINT_OUTPUT("This is attained by rows %d and %d.\n", mrow1, mrow2);
}
}
return maxagr;
}
int COrthogonalArray::oatriple(bool verbose)
{
/* Count triple agreements among rows of an array */
int a3/*, q*/;
int num3 = 0;
for (int j1 = 0; j1 < m_ncol; j1++)
{
for (int j2 = j1+1; j2 < m_ncol; j2++)
{
for (int j3 = j2+1; j3 < m_ncol; j3++)
{
a3 = 0;
for (int i1 = 0; i1 < m_nrow; i1++)
{
for (int i2 = i1+1; i2 < m_nrow; i2++)
{
a3 += ( m_A(i1,j1)==m_A(i2,j1) )&&( m_A(i1,j2)==m_A(i2,j2) )&&( m_A(i1,j3)==m_A(i2,j3) );
}
if (a3 && verbose)
{
PRINT_OUTPUT("Cols %d %d %d match in %d distinct pairs of rows.\n", j1, j2, j3, a3);
num3++;
}
}
}
}
}
if (verbose)
{
PRINT_OUTPUT("There are %d distinct triples of columns that agree\n", num3);
PRINT_OUTPUT("in at least two distinct rows.\n");
}
return num3;
}
/**
* direction == 1 is used for forward adjustment,
* direction == -1 is used for undoing the last step.
*/
void
Optimizer::adjustConstraints(double direction)
{
size_t const num_dimensions = m_b.size();
for (size_t i = m_numVars; i < num_dimensions; ++i) {
// See setConstraints() for more information
// on the layout of m_A and m_b.
double c = 0;
for (size_t j = 0; j < m_numVars; ++j) {
c += m_A(i, j) * m_x[j];
}
m_b[i] -= c * direction;
}
}
OptimizationResult
Optimizer::optimize(double internal_force_weight)
{
// Note: because we are supposed to reset the forces anyway,
// we re-use m_internalForce to store the cummulative force.
m_internalForce *= internal_force_weight;
m_internalForce += m_externalForce;
// For the layout of m_A and m_b, see setConstraints()
QuadraticFunction::Gradient const grad(m_internalForce.gradient());
for (size_t i = 0; i < m_numVars; ++i) {
m_b[i] = -grad.b[i];
for (size_t j = 0; j < m_numVars; ++j) {
m_A(i, j) = grad.A(i, j);
}
}
double const total_force_before = m_internalForce.c;
DynamicMatrixCalc<double> mc;
try {
mc(m_A).solve(mc(m_b)).write(m_x.data());
} catch (std::runtime_error const&) {
m_externalForce.reset();
m_internalForce.reset();
m_x.fill(0); // To make undoLastStep() work as expected.
return OptimizationResult(total_force_before, total_force_before);
}
double const total_force_after = m_internalForce.evaluate(m_x.data());
m_externalForce.reset(); // Now it's finally safe to reset these.
m_internalForce.reset();
// The last thing remaining is to adjust constraints,
// as they depend on the current variables.
adjustConstraints(1.0);
return OptimizationResult(total_force_before, total_force_after);
}
void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal)
{
int numContactRows = interleaveContactAndFriction ? 3 : 1;
int numConstraintRows = m_allConstraintPtrArray.size();
if (numConstraintRows == 0)
return;
int n = numConstraintRows;
{
BT_PROFILE("init b (rhs)");
m_b.resize(numConstraintRows);
m_bSplit.resize(numConstraintRows);
m_b.setZero();
m_bSplit.setZero();
for (int i = 0; i < numConstraintRows; i++)
{
btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
if (!btFuzzyZero(jacDiag))
{
btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
m_b[i] = rhs / jacDiag;
m_bSplit[i] = rhsPenetration / jacDiag;
}
}
}
// btScalar* w = 0;
// int nub = 0;
m_lo.resize(numConstraintRows);
m_hi.resize(numConstraintRows);
{
BT_PROFILE("init lo/ho");
for (int i = 0; i < numConstraintRows; i++)
{
if (0) //m_limitDependencies[i]>=0)
{
m_lo[i] = -BT_INFINITY;
m_hi[i] = BT_INFINITY;
}
else
{
m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
}
}
}
//
int m = m_allConstraintPtrArray.size();
int numBodies = m_tmpSolverBodyPool.size();
btAlignedObjectArray<int> bodyJointNodeArray;
{
BT_PROFILE("bodyJointNodeArray.resize");
bodyJointNodeArray.resize(numBodies, -1);
}
btAlignedObjectArray<btJointNode> jointNodeArray;
{
BT_PROFILE("jointNodeArray.reserve");
jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
}
btMatrixXu& J3 = m_scratchJ3;
{
BT_PROFILE("J3.resize");
J3.resize(2 * m, 8);
}
btMatrixXu& JinvM3 = m_scratchJInvM3;
{
BT_PROFILE("JinvM3.resize/setZero");
JinvM3.resize(2 * m, 8);
JinvM3.setZero();
J3.setZero();
}
int cur = 0;
int rowOffset = 0;
btAlignedObjectArray<int>& ofs = m_scratchOfs;
{
BT_PROFILE("ofs resize");
ofs.resize(0);
ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
}
{
BT_PROFILE("Compute J and JinvM");
int c = 0;
int numRows = 0;
for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
{
ofs[c] = rowOffset;
int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
if (orgBodyA)
{
{
int slotA = -1;
//find free jointNode slot for sbA
slotA = jointNodeArray.size();
jointNodeArray.expand(); //NonInitializing();
int prevSlot = bodyJointNodeArray[sbA];
bodyJointNodeArray[sbA] = slotA;
jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
jointNodeArray[slotA].jointIndex = c;
jointNodeArray[slotA].constraintRowIndex = i;
jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
}
for (int row = 0; row < numRows; row++, cur++)
{
btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
for (int r = 0; r < 3; r++)
{
J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
JinvM3.setElem(cur, r, normalInvMass[r]);
JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
}
J3.setElem(cur, 3, 0);
JinvM3.setElem(cur, 3, 0);
J3.setElem(cur, 7, 0);
JinvM3.setElem(cur, 7, 0);
}
}
else
{
cur += numRows;
}
if (orgBodyB)
{
{
int slotB = -1;
//find free jointNode slot for sbA
slotB = jointNodeArray.size();
jointNodeArray.expand(); //NonInitializing();
int prevSlot = bodyJointNodeArray[sbB];
bodyJointNodeArray[sbB] = slotB;
jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
jointNodeArray[slotB].jointIndex = c;
jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
jointNodeArray[slotB].constraintRowIndex = i;
}
for (int row = 0; row < numRows; row++, cur++)
{
btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
for (int r = 0; r < 3; r++)
{
J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
JinvM3.setElem(cur, r, normalInvMassB[r]);
JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
}
J3.setElem(cur, 3, 0);
JinvM3.setElem(cur, 3, 0);
J3.setElem(cur, 7, 0);
JinvM3.setElem(cur, 7, 0);
}
}
else
{
cur += numRows;
}
rowOffset += numRows;
}
}
//compute JinvM = J*invM.
const btScalar* JinvM = JinvM3.getBufferPointer();
const btScalar* Jptr = J3.getBufferPointer();
{
BT_PROFILE("m_A.resize");
m_A.resize(n, n);
}
{
BT_PROFILE("m_A.setZero");
m_A.setZero();
}
int c = 0;
{
int numRows = 0;
BT_PROFILE("Compute A");
for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
{
int row__ = ofs[c];
int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
// btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
// btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
{
int startJointNodeA = bodyJointNodeArray[sbA];
while (startJointNodeA >= 0)
{
int j0 = jointNodeArray[startJointNodeA].jointIndex;
int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
if (j0 < c)
{
int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
//printf("%d joint i %d and j0: %d: ",count++,i,j0);
m_A.multiplyAdd2_p8r(JinvMrow,
Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
}
startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
}
}
{
int startJointNodeB = bodyJointNodeArray[sbB];
while (startJointNodeB >= 0)
{
int j1 = jointNodeArray[startJointNodeB].jointIndex;
int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
if (j1 < c)
{
int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
}
startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
}
}
}
{
BT_PROFILE("compute diagonal");
// compute diagonal blocks of m_A
int row__ = 0;
int numJointRows = m_allConstraintPtrArray.size();
int jj = 0;
for (; row__ < numJointRows;)
{
//int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
// btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
if (orgBodyB)
{
m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
}
row__ += infom;
jj++;
}
}
}
if (1)
{
// add cfm to the diagonal of m_A
for (int i = 0; i < m_A.rows(); ++i)
{
m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
}
}
///fill the upper triangle of the matrix, to make it symmetric
{
BT_PROFILE("fill the upper triangle ");
m_A.copyLowerToUpperTriangle();
}
{
BT_PROFILE("resize/init x");
m_x.resize(numConstraintRows);
m_xSplit.resize(numConstraintRows);
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
{
const btSolverConstraint& c = *m_allConstraintPtrArray[i];
m_x[i] = c.m_appliedImpulse;
m_xSplit[i] = c.m_appliedPushImpulse;
}
}
else
{
m_x.setZero();
m_xSplit.setZero();
}
}
}
/*
*
* Solve for the diffusional velocities in the Stefan-Maxwell equations
*
*/
void LiquidTransport::stefan_maxwell_solve() {
doublereal tmp;
size_t VIM = m_nDim;
m_B.resize(m_nsp, VIM);
//! grab a local copy of the molecular weights
const vector_fp& M = m_thermo->molecularWeights();
/*
* Update the concentrations in the mixture.
*/
update_conc();
double T = m_thermo->temperature();
m_thermo->getStandardVolumes(DATA_PTR(volume_specPM_));
m_thermo->getActivityCoefficients(DATA_PTR(actCoeffMolar_));
/*
* Calculate the electrochemical potential gradient. This is the
* driving force for relative diffusional transport.
*
* Here we calculate
*
* c_i * (grad (mu_i) + S_i grad T - M_i / dens * grad P
*
* This is Eqn. 13-1 p. 318 Newman. The original equation is from
* Hershfeld, Curtis, and Bird.
*
* S_i is the partial molar entropy of species i. This term will cancel
* out a lot of the grad T terms in grad (mu_i), therefore simplifying
* the expression.
*
* Ok I think there may be many ways to do this. One way is to do it via basis
* functions, at the nodes, as a function of the variables in the problem.
*
* For calculation of molality based thermo systems, we current get
* the molar based values. This may change.
*
* Note, we have broken the symmetry of the matrix here, due to
* consideratins involving species concentrations going to zero.
*
*/
for (size_t i = 0; i < m_nsp; i++) {
double xi_denom = m_molefracs_tran[i];
for (size_t a = 0; a < VIM; a++) {
m_ck_Grad_mu[a*m_nsp + i] =
m_chargeSpecies[i] * concTot_ * Faraday * m_Grad_V[a]
+ concTot_ * (volume_specPM_[i] - M[i]/dens_) * m_Grad_P[a]
+ concTot_ * GasConstant * T * m_Grad_lnAC[a*m_nsp+i] / actCoeffMolar_[i]
+ concTot_ * GasConstant * T * m_Grad_X[a*m_nsp+i] / xi_denom;
}
}
if (m_thermo->activityConvention() == cAC_CONVENTION_MOLALITY) {
int iSolvent = 0;
double mwSolvent = m_thermo->molecularWeight(iSolvent);
double mnaught = mwSolvent/ 1000.;
double lnmnaught = log(mnaught);
for (size_t i = 1; i < m_nsp; i++) {
for (size_t a = 0; a < VIM; a++) {
m_ck_Grad_mu[a*m_nsp + i] -=
m_concentrations[i] * GasConstant * m_Grad_T[a] * lnmnaught;
}
}
}
/*
* Just for Note, m_A(i,j) refers to the ith row and jth column.
* They are still fortran ordered, so that i varies fastest.
*/
switch (VIM) {
case 1: /* 1-D approximation */
m_B(0,0) = 0.0;
for (size_t j = 0; j < m_nsp; j++) {
m_A(0,j) = M[j] * m_concentrations[j];
}
for (size_t i = 1; i < m_nsp; i++){
m_B(i,0) = m_ck_Grad_mu[i] / (GasConstant * T);
m_A(i,i) = 0.0;
for (size_t j = 0; j < m_nsp; j++){
if (j != i) {
tmp = m_concentrations[j] / m_DiffCoeff_StefMax(i,j);
m_A(i,i) += tmp;
m_A(i,j) = - tmp;
}
}
}
//! invert and solve the system Ax = b. Answer is in m_B
solve(m_A, m_B);
break;
case 2: /* 2-D approximation */
m_B(0,0) = 0.0;
m_B(0,1) = 0.0;
for (size_t j = 0; j < m_nsp; j++) {
m_A(0,j) = M[j] * m_concentrations[j];
}
for (size_t i = 1; i < m_nsp; i++){
m_B(i,0) = m_ck_Grad_mu[i] / (GasConstant * T);
m_B(i,1) = m_ck_Grad_mu[m_nsp + i] / (GasConstant * T);
m_A(i,i) = 0.0;
for (size_t j = 0; j < m_nsp; j++) {
if (j != i) {
tmp = m_concentrations[j] / m_DiffCoeff_StefMax(i,j);
m_A(i,i) += tmp;
m_A(i,j) = - tmp;
}
}
}
//! invert and solve the system Ax = b. Answer is in m_B
solve(m_A, m_B);
break;
case 3: /* 3-D approximation */
m_B(0,0) = 0.0;
m_B(0,1) = 0.0;
m_B(0,2) = 0.0;
for (size_t j = 0; j < m_nsp; j++) {
m_A(0,j) = M[j] * m_concentrations[j];
}
for (size_t i = 1; i < m_nsp; i++){
m_B(i,0) = m_ck_Grad_mu[i] / (GasConstant * T);
m_B(i,1) = m_ck_Grad_mu[m_nsp + i] / (GasConstant * T);
m_B(i,2) = m_ck_Grad_mu[2*m_nsp + i] / (GasConstant * T);
m_A(i,i) = 0.0;
for (size_t j = 0; j < m_nsp; j++) {
if (j != i) {
tmp = m_concentrations[j] / m_DiffCoeff_StefMax(i,j);
m_A(i,i) += tmp;
m_A(i,j) = - tmp;
}
}
}
//! invert and solve the system Ax = b. Answer is in m_B
solve(m_A, m_B);
break;
default:
printf("uninmplemetnd\n");
throw CanteraError("routine", "not done");
break;
}
for (size_t a = 0; a < VIM; a++) {
for (size_t j = 0; j < m_nsp; j++) {
m_flux(j,a) = M[j] * m_concentrations[j] * m_B(j,a);
}
}
}
bool PointPlaneICP::align( const std::vector< Vector3f >& srcPoints,
const std::vector< Vector3f >& dstPoints, const std::vector< Vector3f >& dstNormals,
const Matrix4f& initialGuess,
Matrix4f& outputSrcToDestination )
{
outputSrcToDestination = initialGuess;
// make a copy of the source points that actually move
std::vector< Vector3f > srcPoints2( srcPoints );
int itr = 0;
bool succeeded = true;
// TODO: could in theory pass in srcPoints, update it in one shot
// one possibility is to multiply by the full every time, instead of each increment
float energy = updateSourcePointsAndEvaluateEnergy( initialGuess,
dstPoints, dstNormals,
srcPoints2 );
while( succeeded && // abort on singular configuration
itr < m_maxNumIterations && // also exit if we
energy > m_epsilon ) // exit loop if we found an acceptable minimum
{
m_A.fill( 0 );
m_b.fill( 0 );
for( size_t i = 0; i < srcPoints2.size(); ++i )
{
Vector3f p = srcPoints2[ i ];
Vector3f q = dstPoints[ i ];
Vector3f n = dstNormals[ i ];
Vector3f c = Vector3f::cross( p, n );
float d = Vector3f::dot( p - q, n );
m_A( 0, 0 ) += c.x * c.x;
m_A( 1, 0 ) += c.y * c.x;
m_A( 2, 0 ) += c.z * c.x;
m_A( 3, 0 ) += n.x * c.x;
m_A( 4, 0 ) += n.y * c.x;
m_A( 5, 0 ) += n.z * c.x;
m_A( 1, 1 ) += c.y * c.y;
m_A( 2, 1 ) += c.z * c.y;
m_A( 3, 1 ) += n.x * c.y;
m_A( 4, 1 ) += n.y * c.y;
m_A( 5, 1 ) += n.z * c.y;
m_A( 2, 2 ) += c.z * c.z;
m_A( 3, 2 ) += n.x * c.z;
m_A( 4, 2 ) += n.y * c.z;
m_A( 5, 2 ) += n.z * c.z;
m_A( 3, 3 ) += n.x * n.x;
m_A( 4, 3 ) += n.y * n.x;
m_A( 5, 3 ) += n.z * n.x;
m_A( 4, 4 ) += n.y * n.y;
m_A( 5, 4 ) += n.z * n.y;
m_A( 5, 5 ) += n.z * n.z;
m_b[ 0 ] -= c.x * d;
m_b[ 1 ] -= c.y * d;
m_b[ 2 ] -= c.z * d;
m_b[ 3 ] -= n.x * d;
m_b[ 4 ] -= n.y * d;
m_b[ 5 ] -= n.z * d;
}
FloatMatrix x = m_A.solveSPD( m_b, succeeded );
if( succeeded )
{
float alpha = x[0];
float beta = x[1];
float gamma = x[2];
float tx = x[3];
float ty = x[4];
float tz = x[5];
Matrix4f incremental =
Matrix4f::translation( tx, ty, tz ) *
Matrix4f::rotateZ( gamma ) *
Matrix4f::rotateY( beta ) *
Matrix4f::rotateX( alpha );
energy = updateSourcePointsAndEvaluateEnergy( incremental,
dstPoints, dstNormals,
srcPoints2 );
// accumulate incremental transformation
outputSrcToDestination = incremental * outputSrcToDestination;
}
printf( "At the end of iteration %d, energy = %f\n", itr, energy );
++itr;
}
return succeeded;
}
double& A(const size_t& i, const size_t& j) { return m_A(i,j); }
void AqueousTransport::stefan_maxwell_solve()
{
size_t VIM = 2;
m_B.resize(m_nsp, VIM);
// grab a local copy of the molecular weights
const vector_fp& M = m_thermo->molecularWeights();
// get the mean molecular weight of the mixture
//double M_mix = m_thermo->meanMolecularWeight();
// get the concentration of the mixture
//double rho = m_thermo->density();
//double c = rho/M_mix;
m_thermo->getMoleFractions(DATA_PTR(m_molefracs));
double T = m_thermo->temperature();
/* electrochemical potential gradient */
for (size_t i = 0; i < m_nsp; i++) {
for (size_t a = 0; a < VIM; a++) {
m_Grad_mu[a*m_nsp + i] = m_chargeSpecies[i] * Faraday * m_Grad_V[a]
+ (GasConstant*T/m_molefracs[i]) * m_Grad_X[a*m_nsp+i];
}
}
/*
* Just for Note, m_A(i,j) refers to the ith row and jth column.
* They are still fortran ordered, so that i varies fastest.
*/
switch (VIM) {
case 1: /* 1-D approximation */
m_B(0,0) = 0.0;
for (size_t j = 0; j < m_nsp; j++) {
m_A(0,j) = 1.0;
}
for (size_t i = 1; i < m_nsp; i++) {
m_B(i,0) = m_concentrations[i] * m_Grad_mu[i] / (GasConstant * T);
for (size_t j = 0; j < m_nsp; j++) {
if (j != i) {
m_A(i,j) = m_molefracs[i] / (M[j] * m_DiffCoeff_StefMax(i,j));
m_A(i,i) -= m_molefracs[j] / (M[i] * m_DiffCoeff_StefMax(i,j));
} else if (j == i) {
m_A(i,i) = 0.0;
}
}
}
//! invert and solve the system Ax = b. Answer is in m_B
solve(m_A, m_B.ptrColumn(0));
m_flux = m_B;
break;
case 2: /* 2-D approximation */
m_B(0,0) = 0.0;
m_B(0,1) = 0.0;
for (size_t j = 0; j < m_nsp; j++) {
m_A(0,j) = 1.0;
}
for (size_t i = 1; i < m_nsp; i++) {
m_B(i,0) = m_concentrations[i] * m_Grad_mu[i] / (GasConstant * T);
m_B(i,1) = m_concentrations[i] * m_Grad_mu[m_nsp + i] / (GasConstant * T);
for (size_t j = 0; j < m_nsp; j++) {
if (j != i) {
m_A(i,j) = m_molefracs[i] / (M[j] * m_DiffCoeff_StefMax(i,j));
m_A(i,i) -= m_molefracs[j] / (M[i] * m_DiffCoeff_StefMax(i,j));
} else if (j == i) {
m_A(i,i) = 0.0;
}
}
}
//! invert and solve the system Ax = b. Answer is in m_B
//solve(m_A, m_B);
m_flux = m_B;
break;
case 3: /* 3-D approximation */
m_B(0,0) = 0.0;
m_B(0,1) = 0.0;
m_B(0,2) = 0.0;
for (size_t j = 0; j < m_nsp; j++) {
m_A(0,j) = 1.0;
}
for (size_t i = 1; i < m_nsp; i++) {
m_B(i,0) = m_concentrations[i] * m_Grad_mu[i] / (GasConstant * T);
m_B(i,1) = m_concentrations[i] * m_Grad_mu[m_nsp + i] / (GasConstant * T);
m_B(i,2) = m_concentrations[i] * m_Grad_mu[2*m_nsp + i] / (GasConstant * T);
for (size_t j = 0; j < m_nsp; j++) {
if (j != i) {
m_A(i,j) = m_molefracs[i] / (M[j] * m_DiffCoeff_StefMax(i,j));
m_A(i,i) -= m_molefracs[j] / (M[i] * m_DiffCoeff_StefMax(i,j));
} else if (j == i) {
m_A(i,i) = 0.0;
}
}
}
//! invert and solve the system Ax = b. Answer is in m_B
//solve(m_A, m_B);
m_flux = m_B;
break;
default:
printf("unimplemented\n");
throw CanteraError("routine", "not done");
break;
}
}
void LiquidTransport::stefan_maxwell_solve()
{
doublereal tmp;
m_B.resize(m_nsp, m_nDim, 0.0);
m_A.resize(m_nsp, m_nsp, 0.0);
//! grab a local copy of the molecular weights
const vector_fp& M = m_thermo->molecularWeights();
//! grad a local copy of the ion molar volume (inverse total ion concentration)
const doublereal vol = m_thermo->molarVolume();
/*
* Update the temperature, concentrations and diffusion coefficients in the mixture.
*/
update_T();
update_C();
if (!m_diff_temp_ok) {
updateDiff_T();
}
double T = m_thermo->temperature();
update_Grad_lnAC();
m_thermo->getActivityCoefficients(DATA_PTR(m_actCoeff));
/*
* Calculate the electrochemical potential gradient. This is the
* driving force for relative diffusional transport.
*
* Here we calculate
*
* X_i * (grad (mu_i) + S_i grad T - M_i / dens * grad P
*
* This is Eqn. 13-1 p. 318 Newman. The original equation is from
* Hershfeld, Curtis, and Bird.
*
* S_i is the partial molar entropy of species i. This term will cancel
* out a lot of the grad T terms in grad (mu_i), therefore simplifying
* the expression.
*
* Ok I think there may be many ways to do this. One way is to do it via basis
* functions, at the nodes, as a function of the variables in the problem.
*
* For calculation of molality based thermo systems, we current get
* the molar based values. This may change.
*
* Note, we have broken the symmetry of the matrix here, due to
* considerations involving species concentrations going to zero.
*/
for (size_t a = 0; a < m_nDim; a++) {
for (size_t i = 0; i < m_nsp; i++) {
m_Grad_mu[a*m_nsp + i] =
m_chargeSpecies[i] * Faraday * m_Grad_V[a]
+ GasConstant * T * m_Grad_lnAC[a*m_nsp+i];
}
}
if (m_thermo->activityConvention() == cAC_CONVENTION_MOLALITY) {
int iSolvent = 0;
double mwSolvent = m_thermo->molecularWeight(iSolvent);
double mnaught = mwSolvent/ 1000.;
double lnmnaught = log(mnaught);
for (size_t a = 0; a < m_nDim; a++) {
for (size_t i = 1; i < m_nsp; i++) {
m_Grad_mu[a*m_nsp + i] -=
m_molefracs[i] * GasConstant * m_Grad_T[a] * lnmnaught;
}
}
}
/*
* Just for Note, m_A(i,j) refers to the ith row and jth column.
* They are still fortran ordered, so that i varies fastest.
*/
double condSum1;
const doublereal invRT = 1.0 / (GasConstant * T);
switch (m_nDim) {
case 1: /* 1-D approximation */
m_B(0,0) = 0.0;
//equation for the reference velocity
for (size_t j = 0; j < m_nsp; j++) {
if (m_velocityBasis == VB_MOLEAVG) {
m_A(0,j) = m_molefracs_tran[j];
} else if (m_velocityBasis == VB_MASSAVG) {
m_A(0,j) = m_massfracs_tran[j];
} else if ((m_velocityBasis >= 0)
&& (m_velocityBasis < static_cast<int>(m_nsp))) {
// use species number m_velocityBasis as reference velocity
if (m_velocityBasis == static_cast<int>(j)) {
m_A(0,j) = 1.0;
} else {
m_A(0,j) = 0.0;
}
} else {
throw CanteraError("LiquidTransport::stefan_maxwell_solve",
"Unknown reference velocity provided.");
}
}
for (size_t i = 1; i < m_nsp; i++) {
m_B(i,0) = m_Grad_mu[i] * invRT;
m_A(i,i) = 0.0;
for (size_t j = 0; j < m_nsp; j++) {
if (j != i) {
tmp = m_molefracs_tran[j] * m_bdiff(i,j);
m_A(i,i) -= tmp;
m_A(i,j) = tmp;
}
}
}
//! invert and solve the system Ax = b. Answer is in m_B
solve(m_A, m_B);
condSum1 = 0;
for (size_t i = 0; i < m_nsp; i++) {
condSum1 -= Faraday*m_chargeSpecies[i]*m_B(i,0)*m_molefracs_tran[i]/vol;
}
break;
case 2: /* 2-D approximation */
m_B(0,0) = 0.0;
m_B(0,1) = 0.0;
//equation for the reference velocity
for (size_t j = 0; j < m_nsp; j++) {
if (m_velocityBasis == VB_MOLEAVG) {
m_A(0,j) = m_molefracs_tran[j];
} else if (m_velocityBasis == VB_MASSAVG) {
m_A(0,j) = m_massfracs_tran[j];
} else if ((m_velocityBasis >= 0)
&& (m_velocityBasis < static_cast<int>(m_nsp))) {
// use species number m_velocityBasis as reference velocity
if (m_velocityBasis == static_cast<int>(j)) {
m_A(0,j) = 1.0;
} else {
m_A(0,j) = 0.0;
}
} else {
throw CanteraError("LiquidTransport::stefan_maxwell_solve",
"Unknown reference velocity provided.");
}
}
for (size_t i = 1; i < m_nsp; i++) {
m_B(i,0) = m_Grad_mu[i] * invRT;
m_B(i,1) = m_Grad_mu[m_nsp + i] * invRT;
m_A(i,i) = 0.0;
for (size_t j = 0; j < m_nsp; j++) {
if (j != i) {
tmp = m_molefracs_tran[j] * m_bdiff(i,j);
m_A(i,i) -= tmp;
m_A(i,j) = tmp;
}
}
}
//! invert and solve the system Ax = b. Answer is in m_B
solve(m_A, m_B);
break;
case 3: /* 3-D approximation */
m_B(0,0) = 0.0;
m_B(0,1) = 0.0;
m_B(0,2) = 0.0;
//equation for the reference velocity
for (size_t j = 0; j < m_nsp; j++) {
if (m_velocityBasis == VB_MOLEAVG) {
m_A(0,j) = m_molefracs_tran[j];
} else if (m_velocityBasis == VB_MASSAVG) {
m_A(0,j) = m_massfracs_tran[j];
} else if ((m_velocityBasis >= 0)
&& (m_velocityBasis < static_cast<int>(m_nsp))) {
// use species number m_velocityBasis as reference velocity
if (m_velocityBasis == static_cast<int>(j)) {
m_A(0,j) = 1.0;
} else {
m_A(0,j) = 0.0;
}
} else {
throw CanteraError("LiquidTransport::stefan_maxwell_solve",
"Unknown reference velocity provided.");
}
}
for (size_t i = 1; i < m_nsp; i++) {
m_B(i,0) = m_Grad_mu[i] * invRT;
m_B(i,1) = m_Grad_mu[m_nsp + i] * invRT;
m_B(i,2) = m_Grad_mu[2*m_nsp + i] * invRT;
m_A(i,i) = 0.0;
for (size_t j = 0; j < m_nsp; j++) {
if (j != i) {
tmp = m_molefracs_tran[j] * m_bdiff(i,j);
m_A(i,i) -= tmp;
m_A(i,j) = tmp;
}
}
}
//! invert and solve the system Ax = b. Answer is in m_B
solve(m_A, m_B);
break;
default:
printf("unimplemented\n");
throw CanteraError("routine", "not done");
break;
}
for (size_t a = 0; a < m_nDim; a++) {
for (size_t j = 0; j < m_nsp; j++) {
m_Vdiff(j,a) = m_B(j,a);
m_flux(j,a) = concTot_ * M[j] * m_molefracs_tran[j] * m_B(j,a);
}
}
}
void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
{
int numBodies = this->m_tmpSolverBodyPool.size();
int numConstraintRows = m_allConstraintArray.size();
m_b.resize(numConstraintRows);
if (infoGlobal.m_splitImpulse)
m_bSplit.resize(numConstraintRows);
m_bSplit.setZero();
m_b.setZero();
for (int i=0;i<numConstraintRows ;i++)
{
if (m_allConstraintArray[i].m_jacDiagABInv)
{
m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
if (infoGlobal.m_splitImpulse)
m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
}
}
static btMatrixXu Minv;
Minv.resize(6*numBodies,6*numBodies);
Minv.setZero();
for (int i=0;i<numBodies;i++)
{
const btSolverBody& rb = m_tmpSolverBodyPool[i];
const btVector3& invMass = rb.m_invMass;
setElem(Minv,i*6+0,i*6+0,invMass[0]);
setElem(Minv,i*6+1,i*6+1,invMass[1]);
setElem(Minv,i*6+2,i*6+2,invMass[2]);
btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody;
for (int r=0;r<3;r++)
for (int c=0;c<3;c++)
setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
}
static btMatrixXu J;
J.resize(numConstraintRows,6*numBodies);
J.setZero();
m_lo.resize(numConstraintRows);
m_hi.resize(numConstraintRows);
for (int i=0;i<numConstraintRows;i++)
{
m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
m_hi[i] = m_allConstraintArray[i].m_upperLimit;
int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA;
int bodyIndex1 = m_allConstraintArray[i].m_solverBodyIdB;
if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody)
{
setElem(J,i,6*bodyIndex0+0,m_allConstraintArray[i].m_contactNormal1[0]);
setElem(J,i,6*bodyIndex0+1,m_allConstraintArray[i].m_contactNormal1[1]);
setElem(J,i,6*bodyIndex0+2,m_allConstraintArray[i].m_contactNormal1[2]);
setElem(J,i,6*bodyIndex0+3,m_allConstraintArray[i].m_relpos1CrossNormal[0]);
setElem(J,i,6*bodyIndex0+4,m_allConstraintArray[i].m_relpos1CrossNormal[1]);
setElem(J,i,6*bodyIndex0+5,m_allConstraintArray[i].m_relpos1CrossNormal[2]);
}
if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody)
{
setElem(J,i,6*bodyIndex1+0,m_allConstraintArray[i].m_contactNormal2[0]);
setElem(J,i,6*bodyIndex1+1,m_allConstraintArray[i].m_contactNormal2[1]);
setElem(J,i,6*bodyIndex1+2,m_allConstraintArray[i].m_contactNormal2[2]);
setElem(J,i,6*bodyIndex1+3,m_allConstraintArray[i].m_relpos2CrossNormal[0]);
setElem(J,i,6*bodyIndex1+4,m_allConstraintArray[i].m_relpos2CrossNormal[1]);
setElem(J,i,6*bodyIndex1+5,m_allConstraintArray[i].m_relpos2CrossNormal[2]);
}
}
static btMatrixXu J_transpose;
J_transpose= J.transpose();
static btMatrixXu tmp;
{
{
BT_PROFILE("J*Minv");
tmp = J*Minv;
}
{
BT_PROFILE("J*tmp");
m_A = tmp*J_transpose;
}
}
if (1)
{
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
}
}
m_x.resize(numConstraintRows);
if (infoGlobal.m_splitImpulse)
m_xSplit.resize(numConstraintRows);
// m_x.setZero();
for (int i=0;i<m_allConstraintArray.size();i++)
{
const btSolverConstraint& c = m_allConstraintArray[i];
m_x[i]=c.m_appliedImpulse;
if (infoGlobal.m_splitImpulse)
m_xSplit[i] = c.m_appliedPushImpulse;
}
}
