Пример #1
0
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) ];
		}
	}
}
Пример #2
0
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;
}
Пример #3
0
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;
}
Пример #4
0
/**
 * 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;
	}
}
Пример #5
0
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);
}
Пример #6
0
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();
		}
	}
}
Пример #7
0
  /*
   *
   *    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);
      }
    }
  }
Пример #8
0
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;
}
Пример #9
0
 double& A(const size_t& i, const size_t& j)       { return m_A(i,j); }
Пример #10
0
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;
    }
}
Пример #11
0
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);
        }
    }
}
Пример #12
0
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;
	}

}