Esempio n. 1
0
/* State decision algorithm 9.3.3 Fig 26 */
static int bmc_state_decision(struct pp_instance *ppi,
							  struct pp_frgn_master *m)
{
	int cmpres;
	struct pp_frgn_master myself;

	if (ppi->master_only)
		goto master;

	if (ppi->slave_only)
		goto slave;

	if ((!ppi->frgn_rec_num) && (ppi->state == PPS_LISTENING))
		return PPS_LISTENING;

	/* copy local information to a foreign_master structure */
	copy_d0(ppi, &myself);

	/* dataset_cmp is "a - b" but lower values win */
	cmpres = bmc_dataset_cmp(ppi, &myself, m);

	if (DSDEF(ppi)->clockQuality.clockClass < 128) {
		if (cmpres < 0)
			goto master;
		if (cmpres > 0)
			goto passive;
	}
	if (cmpres < 0)
		goto master;
	if (cmpres > 0) {
		if (DSDEF(ppi)->numberPorts == 1)
			goto slave; /* directly skip to ordinary clock handling */
		else
			goto check_boundary_clk;
	}

	pp_diag(ppi, bmc, 1,"%s: error\n", __func__);

	/*  MB: Is this the return code below correct? */
	/*  Anyway, it's a valid return code. */
	return PPS_FAULTY;

check_boundary_clk:
	if (ppi->port_idx == GLBS(ppi)->ebest_idx) /* This port is the Ebest */
		goto slave;

	/* If idcmp returns 0, it means that this port is not the best because
		* Ebest is better by topology than Erbest */
	if (!idcmp(&myself.ann.grandmasterIdentity,
			&m->ann.grandmasterIdentity))
		goto passive;
	else
		goto master;

passive:
	p1(ppi, &m->hdr, &m->ann);
	pp_diag(ppi, bmc, 1,"%s: passive\n", __func__);
	return PPS_PASSIVE;

master:
	m1(ppi);
	pp_diag(ppi, bmc, 1,"%s: master\n", __func__);
	return PPS_MASTER;

slave:
	s1(ppi, &m->hdr, &m->ann);
	pp_diag(ppi, bmc, 1,"%s: slave\n", __func__);
	return PPS_SLAVE;

}
Esempio n. 2
0
void RegionalTerrain_3r::SigPopEdge(QuadEdge::Edge* e) {
    Segment_3r s1(e->Org()->pos, e->Dest()->pos);
    Segment_3r s2(e->Dest()->pos, e->Org()->pos);
    SigPopVisualSegment_3r(s1);
    SigPopVisualSegment_3r(s2);
}
NOX::Abstract::Group::ReturnType
LOCA::TurningPoint::MinimallyAugmented::Constraint::
computeConstraints()
{
  if (isValidConstraints)
    return NOX::Abstract::Group::Ok;

  std::string callingFunction = 
    "LOCA::TurningPoint::MinimallyAugmented::Constraint::computeConstraints()";
  NOX::Abstract::Group::ReturnType status;
  NOX::Abstract::Group::ReturnType finalStatus = NOX::Abstract::Group::Ok;

  // Compute J
  status = grpPtr->computeJacobian();
  finalStatus = 
    globalData->locaErrorCheck->combineAndCheckReturnTypes(status, 
							   finalStatus,
							   callingFunction);

  // Set up bordered systems
  Teuchos::RCP<const LOCA::BorderedSolver::JacobianOperator> op =
    Teuchos::rcp(new  LOCA::BorderedSolver::JacobianOperator(grpPtr));
  borderedSolver->setMatrixBlocksMultiVecConstraint(op, 
						    a_vector, 
						    b_vector, 
						    Teuchos::null);

  // Create RHS
  NOX::Abstract::MultiVector::DenseMatrix one(1,1);
  if (nullVecScaling == NVS_OrderN)
    one(0,0) = dn;
  else
    one(0,0) = 1.0;

  // Get linear solver parameters
  Teuchos::RCP<Teuchos::ParameterList> linear_solver_params =
    parsedParams->getSublist("Linear Solver");

  // Compute sigma_1 and right null vector v
  NOX::Abstract::MultiVector::DenseMatrix s1(1,1);
  status = borderedSolver->initForSolve();
  finalStatus = 
    globalData->locaErrorCheck->combineAndCheckReturnTypes(status, 
							   finalStatus,
							   callingFunction);
  status = borderedSolver->applyInverse(*linear_solver_params, 
					NULL, 
					&one, 
					*v_vector, 
					s1);
  finalStatus = 
    globalData->locaErrorCheck->combineAndCheckReturnTypes(status, 
							   finalStatus,
							   callingFunction);

  // Compute sigma_2 and left null vector w
  NOX::Abstract::MultiVector::DenseMatrix s2(1,1);
  if (!isSymmetric) {
    status = borderedSolver->initForTransposeSolve();
    finalStatus = 
      globalData->locaErrorCheck->combineAndCheckReturnTypes(status, 
							     finalStatus,
							     callingFunction);
    status = borderedSolver->applyInverseTranspose(*linear_solver_params, 
						   NULL, 
						   &one, 
						   *w_vector, 
						   s2);
    finalStatus = 
      globalData->locaErrorCheck->combineAndCheckReturnTypes(status, 
							     finalStatus,
							     callingFunction);

  }
  else {
    *w_vector = *v_vector;
    s2.assign(s1);
  }
  
  // Compute sigma = -w^T*J*v
  status = grpPtr->applyJacobianMultiVector(*v_vector, *Jv_vector);
  finalStatus = 
    globalData->locaErrorCheck->combineAndCheckReturnTypes(status, 
							   finalStatus,
							   callingFunction);
  if (!isSymmetric) {
    status = grpPtr->applyJacobianTransposeMultiVector(*w_vector, *Jtw_vector);
    finalStatus = 
      globalData->locaErrorCheck->combineAndCheckReturnTypes(status, 
							     finalStatus,
							     callingFunction);
  }
  else
    *Jtw_vector = *Jv_vector;
  Jv_vector->multiply(-1.0, *w_vector, constraints);

  // Scale sigma
  double w_norm = (*w_vector)[0].norm();
  double v_norm = (*v_vector)[0].norm();
  double Jv_norm = (*Jv_vector)[0].norm();
  double Jtw_norm = (*Jtw_vector)[0].norm();
  if (nullVecScaling == NVS_OrderN)
    sigma_scale = dn;
  else
    sigma_scale = 1.0;
  constraints.scale(1.0/sigma_scale);

  if (globalData->locaUtils->isPrintType(NOX::Utils::OuterIteration)) {
    globalData->locaUtils->out() << "\n\t||Right null vector v|| = " 
				 << globalData->locaUtils->sciformat(v_norm);
    globalData->locaUtils->out() << "\n\t||Left null vector w|| = " 
				 << globalData->locaUtils->sciformat(w_norm);
    globalData->locaUtils->out() << "\n\t||Jv|| = " 
				 << globalData->locaUtils->sciformat(Jv_norm);
    globalData->locaUtils->out() << "\n\t||J^T*w|| = " 
				 << globalData->locaUtils->sciformat(Jtw_norm);
    globalData->locaUtils->out() << 
      "\n\tRight estimate for singularity of Jacobian (sigma1) = " << 
      globalData->locaUtils->sciformat(s1(0,0));
    globalData->locaUtils->out() << 
      "\n\tLeft estimate for singularity of Jacobian (sigma2) = " << 
      globalData->locaUtils->sciformat(s2(0,0));
    globalData->locaUtils->out() << 
      "\n\tFinal Estimate for singularity of Jacobian (sigma) = " << 
      globalData->locaUtils->sciformat(constraints(0,0)) << std::endl;
  }

  isValidConstraints = true;

  // Update a and b if requested
  if (updateVectorsEveryIteration) {
    if (globalData->locaUtils->isPrintType(NOX::Utils::OuterIteration)) {
      globalData->locaUtils->out() << 
	"\n\tUpdating null vectors for the next nonlinear iteration" << 
	std::endl;
    }
    *a_vector = *w_vector;
    *b_vector = *v_vector;

    scaleNullVectors((*a_vector)[0],(*b_vector)[0]);
  }

  return finalStatus;
}
Esempio n. 4
0
String AirDC::OutputSerial(int mode)
{
    String StreamOut;
    switch(mode)
    {
    case 1: //Measurements output
    {
//_p,_T,_RH,_qc,AOA,AOS
        String s1(_p, 6);
        String s2(_T, 6);
        String s3(_RH, 6);
        String s4(_qc, 6);
        String s5(_AOA, 6);
        String s6(_AOS, 6);
        StreamOut="$TMO,"+s1+','+s2+','+s3+','+s4+','+s5+','+s6;
//To read string on the other side
        /*
          if (Serial.find("$TMO,")) {
            _p = Serial.parseFloat(); //
            _T = Serial.parseFloat();//
            _RH = Serial.parseFloat();//
            _qc = Serial.parseFloat();//
        */
        break;
    }
    case 2: //Air data output
        //_Rho,_IAS,_CAS,_TAS,_TASPCorrected,_M,_TAT,_h,_mu,_Re
    {
        String s1(_Rho, 6);
        String s2(_IAS, 6);
        String s3(_CAS, 6);
        String s4(_TAS, 6);
        String s5(_TASPCorrected, 6);
        String s6(_M, 6);
        String s7(_TAT, 6);
        String s8(_h, 6);
        String s9(_mu, 8);
        String s10(_Re, 6);
        StreamOut="$TAD,"+s1+','+s2+','+s3+','+s4+','+s5+','+s6+','+s7+','+s8+','+s9+','+s10;
        break;
    }
    case 3: //Measurements uncertainty output
        //_up,_uT,_uRH,_uqc
    {
        String s1(_up, 6);
        String s2(_uT, 6);
        String s3(_uRH, 6);
        String s4(_uqc, 6);
        StreamOut="$TMU,"+s1+','+s2+','+s3+','+s4;
        break;
    }
    case 4: //Air data uncertainty output
        //_uRho,_uIAS,_uCAS,_uTAS,_uTAT,_uh;
    {
        String s1(_uRho, 6);
        String s2(_uIAS, 6);
        String s3(_uCAS, 6);
        String s4(_uTAS, 6);
        String s5(_uTAT, 6);
        String s6(_uh, 6);
        StreamOut="$TAU,"+s1+','+s2+','+s3+','+s4+','+s5+','+s6;
        break;
    }
    case 51: //Output for Temperature Logger Example
    {
        String s1(_Rho, 6);
        String s2(_TAT, 2);
        String s3(_TAT-273.15, 2);
        String s4(_uTAT, 2);
        String s5(_p, 2);
        String s6(_mu, 6);
        String s7(hour());
        String s8(minute());
        String s9(second());
        String s10(month());
        String s11(day());
        String s12(year());
        String s13(millis());
        StreamOut="$TEX,"+s1+','+s2+','+s3+','+s4+','+s5+','+s6+','+s7+','+s8+','+s9+','+s10+','+s11+','+s12+','+s13;
        break;
    }
    return StreamOut;
    }
}
Esempio n. 5
0
File: tstore3.cpp Progetto: aosm/tcl
void TestStores3() {
  B(s20, View outlives storage, 0)W(s20a);
   {
    c4_IntProp p1("p1");
    c4_View v1;

     {
      c4_Storage s1("s20a", 1);
      v1 = s1.GetAs("a[p1:I,p2:S]");
      v1.Add(p1[123]);
    }

    // 19990916 - semantics changed, rows kept but no properties
    //A(p1 (v1[0]) == 123);
    A(v1.GetSize() == 1);
    A(v1.NumProperties() == 0);

  }
  D(s20a);
  R(s20a);
  E;

  B(s21, Test demo scenario, 0)W(s21a);
   {
    c4_StringProp p1("p1"), p2("p2");
     {
      c4_Storage storage("s21a", 1);
      storage.SetStructure("a[p1:S,p2:S]");
      c4_View v1;
      c4_Row r1;

      p1(r1) = "One";
      p2(r1) = "Un";
      v1.Add(r1);
      A(v1.GetSize() == 1);

      p1(r1) = "Two";
      p2(r1) = "Deux";
      v1.Add(r1);
      A(v1.GetSize() == 2);

      // changed 2000-03-15: Store is gone
      //v1 = storage.Store("a", v1);
      v1 = storage.View("a") = v1;

      A(v1.GetSize() == 2);
      A(p1(v1[1]) == (c4_String)"Two");
      A(p2(v1[1]) == (c4_String)"Deux");
      A(p1(v1[0]) == (c4_String)"One");
      A(p2(v1[0]) == (c4_String)"Un");

      storage.Commit();
      A(v1.GetSize() == 2);
      A(p1(v1[1]) == (c4_String)"Two");
      A(p2(v1[1]) == (c4_String)"Deux");
      A(p1(v1[0]) == (c4_String)"One");
      A(p2(v1[0]) == (c4_String)"Un");

      c4_String s1(p1(v1[1]));
      c4_String s2(p2(v1[1]));
      A(s1 == "Two");
      A(s2 == "Deux");

      storage.Commit();

      v1.Add(p1["Three"] + p2["Trois"]);

      storage.Commit();
      A(v1.GetSize() == 3);
      A(p2(v1[2]) == (c4_String)"Trois");

      v1 = storage.GetAs("a[p1:S,p2:S,p3:I]");
      A(v1.GetSize() == 3);
      A(p2(v1[2]) == (c4_String)"Trois");

      c4_IntProp p3("p3");
      p3(v1[1]) = 123;

      storage.Commit();
      A(v1.GetSize() == 3);
      A(p2(v1[2]) == (c4_String)"Trois");

      c4_View v2 = storage.GetAs("b[p4:I]");

      c4_IntProp p4("p4");
      v2.Add(p4[234]);

      storage.Commit();
      A(v1.GetSize() == 3);
      A(p2(v1[2]) == (c4_String)"Trois");

      c4_IntProp p4a("p4");
      v1.InsertAt(2, p1["Four"] + p4a[345]);

      storage.Commit();
      A(v1.GetSize() == 4);
      A(p1(v1[0]) == (c4_String)"One");
      A(p1(v1[1]) == (c4_String)"Two");
      A(p1(v1[2]) == (c4_String)"Four");
      A(p1(v1[3]) == (c4_String)"Three");
      A(p2(v1[3]) == (c4_String)"Trois");
      A(v2.GetSize() == 1);
      A(p4(v2[0]) == 234);
    }
     {
      c4_Storage storage("s21a", 0);
      c4_View v1 = storage.View("a");
      A(v1.GetSize() == 4);
      A(p1(v1[0]) == (c4_String)"One");
      A(p1(v1[1]) == (c4_String)"Two");
      A(p1(v1[2]) == (c4_String)"Four");
      A(p1(v1[3]) == (c4_String)"Three");
      c4_View v2 = storage.View("b");
      c4_IntProp p4("p4");
      A(v2.GetSize() == 1);
      A(p4(v2[0]) == 234);
    }
  }
  D(s21a);
  R(s21a);
  E;

#if !q4_TINY
  B(s22, Double storage, 0)W(s22a);
   {
    c4_DoubleProp p1("p1");
    c4_Storage s1("s22a", 1);
    s1.SetStructure("a[p1:D]");
    c4_View v1 = s1.View("a");
    v1.Add(p1[1234.5678]);
    v1.Add(p1[2345.6789]);
    v1.InsertAt(1, p1[3456.7890]);
    s1.Commit();
  }
  D(s22a);
  R(s22a);
  E;
#endif 

  B(s23, Find absent record, 0)W(s23a);
   {
    c4_Storage s1("s23a", 1);
    s1.SetStructure("v[h:S,p:I,a:I,b:I,c:I,d:I,e:I,f:I,g:I,x:I]");
    c4_View view = s1.View("v");

    c4_StringProp H("h");
    c4_IntProp P("p");

    c4_Row row;
    H(row) = "someString";
    P(row) = 99;

    int x = view.Find(row);
    A(x ==  - 1);

  }
  D(s23a);
  R(s23a);
  E;

  B(s24, Bitwise storage, 0)W(s24a);
   {
    c4_IntProp p1("p1");

    int m = 9;

    // insert values in front, but check fractional sizes at each step
    for (int n = 0; n < m; ++n) {
       {
        c4_Storage s1("s24a", 1);
        s1.SetStructure("a1[p1:I],a2[p1:I],a3[p1:I],a4[p1:I]");
        s1.AutoCommit(); // new feature in 1.6

        c4_View v1 = s1.View("a1");
        c4_View v2 = s1.View("a2");
        c4_View v3 = s1.View("a3");
        c4_View v4 = s1.View("a4");

        c4_Row row;
        int k = ~n;

        p1(row) = k &0x01;
        v1.InsertAt(0, row);

        p1(row) = k &0x03;
        v2.InsertAt(0, row);

        p1(row) = k &0x0F;
        v3.InsertAt(0, row);

        p1(row) = k &0x7F;
        v4.InsertAt(0, row);
      }
      // the following checks that all tiny size combinations work
       {
        c4_Storage s1("s24a", 0);

        c4_View v1 = s1.View("a1");
        c4_View v2 = s1.View("a2");
        c4_View v3 = s1.View("a3");
        c4_View v4 = s1.View("a4");

        A(v1.GetSize() == n + 1);
        A(v2.GetSize() == n + 1);
        A(v3.GetSize() == n + 1);
        A(v4.GetSize() == n + 1);
      }
    }

    c4_Storage s1("s24a", 0);

    c4_View v1 = s1.View("a1");
    c4_View v2 = s1.View("a2");
    c4_View v3 = s1.View("a3");
    c4_View v4 = s1.View("a4");

    A(v1.GetSize() == m);
    A(v2.GetSize() == m);
    A(v3.GetSize() == m);
    A(v4.GetSize() == m);

    // now check that the inserted values are correct
    for (int i = 0; i < m; ++i) {
      int j = m - i - 1;
      int k = ~i;

      A(p1(v1[j]) == (k &0x01));
      A(p1(v2[j]) == (k &0x03));
      A(p1(v3[j]) == (k &0x0F));
      A(p1(v4[j]) == (k &0x7F));
    }

  }
  D(s24a);
  R(s24a);
  E;

  B(s25, Bytes storage, 0)W(s25a);
   {
    c4_Bytes hi("hi", 2);
    c4_Bytes gday("gday", 4);
    c4_Bytes hello("hello", 5);

    c4_BytesProp p1("p1");
    c4_Storage s1("s25a", 1);
    s1.SetStructure("a[p1:B]");
    c4_View v1 = s1.View("a");

    v1.Add(p1[hi]);
    A(p1(v1[0]) == hi);
    v1.Add(p1[hello]);
    A(p1(v1[0]) == hi);
    A(p1(v1[1]) == hello);
    v1.InsertAt(1, p1[gday]);
    A(p1(v1[0]) == hi);
    A(p1(v1[1]) == gday);
    A(p1(v1[2]) == hello);
    s1.Commit();
    A(p1(v1[0]) == hi);
    A(p1(v1[1]) == gday);
    A(p1(v1[2]) == hello);

  }
  D(s25a);
  R(s25a);
  E;

  B(s26, Bitwise autosizing, 0)W(s26a);
   {
    c4_IntProp p1("p1"), p2("p2"), p3("p3"), p4("p4");
    c4_Storage s1("s26a", 1);
    s1.SetStructure("a[p1:I,p2:I,p3:I,p4:I]");
    c4_View v1 = s1.View("a");

    v1.Add(p1[1] + p2[3] + p3[15] + p4[127]);
    A(p1(v1[0]) == 1);
    A(p2(v1[0]) == 3);
    A(p3(v1[0]) == 15);
    A(p4(v1[0]) == 127);

    p1(v1[0]) = 100000L;
    p2(v1[0]) = 100000L;
    p3(v1[0]) = 100000L;
    p4(v1[0]) = 100000L;

    // these failed in 1.61
    A(p1(v1[0]) == 100000L);
    A(p2(v1[0]) == 100000L);
    A(p3(v1[0]) == 100000L);
    A(p4(v1[0]) == 100000L);

    s1.Commit();

  }
  D(s26a);
  R(s26a);
  E;

  B(s27, Bytes restructuring, 0)W(s27a);
   {
    c4_Bytes test("test", 4);

    c4_BytesProp p1("p1");
    c4_Storage s1("s27a", 1);

    c4_Row row;
    p1(row) = test;

    c4_View v1;
    v1.Add(row);

    // changed 2000-03-15: Store is gone
    //s1.Store("a", v1); // asserts in 1.61
    c4_View v2 = s1.GetAs("a[p1:B]");
    v2.InsertAt(0, v1);

    s1.Commit();

  }
  D(s27a);
  R(s27a);
  E;

#if !q4_TINY
  B(s28, Doubles added later, 0)W(s28a);
   {
    c4_FloatProp p1("p1");
    c4_DoubleProp p2("p2");
    c4_ViewProp p3("p3");

    c4_Storage s1("s28a", 1);
    s1.SetStructure("a[p1:F,p2:D,p3[p1:F,p2:D]]");
    c4_View v1 = s1.View("a");

    c4_Row r1;

    p1(r1) = 123;
    p2(r1) = 123;

    c4_View v2;
    v2.Add(p1[234] + p2[234]);
    p3(r1) = v2;

    v1.Add(r1);
    double x1 = p1(v1[0]);
    A(x1 == p2(v1[0]));

    v2 = p3(v1[0]);
    double x2 = p1(v2[0]);
    A(x2 == p2(v2[0])); // fails in 1.6

    s1.Commit();

  }
  D(s28a);
  R(s28a);
  E;
#endif 

  B(s29, Delete bytes property, 0)W(s29a);
   {
     {
      c4_BytesProp p1("p1");

      c4_Storage s1("s29a", 1);
      s1.SetStructure("a[p1:B]");
      c4_View v1 = s1.View("a");

      int data = 99;
      v1.Add(p1[c4_Bytes(&data, sizeof data)]);

      s1.Commit();
    }
     {
      c4_Storage s1("s29a", 1);
      c4_View v1 = s1.View("a");

      v1.RemoveAt(0); // asserts in 1.7

      s1.Commit();
    }

  }
  D(s29a);
  R(s29a);
  E;
}
Esempio n. 6
0
int main(int argc, char* argv[])
{
  // Load the mesh.
  Mesh basemesh;
  H2DReader mloader;
  mloader.load("GAMM-channel.mesh", &basemesh);

  // Initialize the meshes.
  Mesh mesh_flow, mesh_concentration;
  mesh_flow.copy(&basemesh);
  mesh_concentration.copy(&basemesh);

  for(unsigned int i = 0; i < INIT_REF_NUM_CONCENTRATION; i++)
    mesh_concentration.refine_all_elements();

  mesh_concentration.refine_towards_boundary(BDY_DIRICHLET_CONCENTRATION, INIT_REF_NUM_CONCENTRATION_BDY);
  mesh_flow.refine_towards_boundary(BDY_DIRICHLET_CONCENTRATION, INIT_REF_NUM_CONCENTRATION_BDY);

  for(unsigned int i = 0; i < INIT_REF_NUM_FLOW; i++)
    mesh_flow.refine_all_elements();

  // Initialize boundary condition types and spaces with default shapesets.
  // For the concentration.
  EssentialBCs bcs_concentration;
  
  bcs_concentration.add_boundary_condition(new ConcentrationTimedepEssentialBC(BDY_DIRICHLET_CONCENTRATION, CONCENTRATION_EXT, CONCENTRATION_EXT_STARTUP_TIME));
  bcs_concentration.add_boundary_condition(new ConcentrationTimedepEssentialBC(BDY_SOLID_WALL_TOP, 0.0, CONCENTRATION_EXT_STARTUP_TIME));
  
  L2Space space_rho(&mesh_flow, P_INIT_FLOW);
  L2Space space_rho_v_x(&mesh_flow, P_INIT_FLOW);
  L2Space space_rho_v_y(&mesh_flow, P_INIT_FLOW);
  L2Space space_e(&mesh_flow, P_INIT_FLOW);
  // Space for concentration.
  H1Space space_c(&mesh_concentration, &bcs_concentration, P_INIT_CONCENTRATION);

  int ndof = Space::get_num_dofs(Hermes::vector<Space*>(&space_rho, &space_rho_v_x, &space_rho_v_y, &space_e, &space_c));
  info("ndof: %d", ndof);

  // Initialize solutions, set initial conditions.
  InitialSolutionEulerDensity prev_rho(&mesh_flow, RHO_EXT);
  InitialSolutionEulerDensityVelX prev_rho_v_x(&mesh_flow, RHO_EXT * V1_EXT);
  InitialSolutionEulerDensityVelY prev_rho_v_y(&mesh_flow, RHO_EXT * V2_EXT);
  InitialSolutionEulerDensityEnergy prev_e(&mesh_flow, QuantityCalculator::calc_energy(RHO_EXT, RHO_EXT * V1_EXT, RHO_EXT * V2_EXT, P_EXT, KAPPA));
  InitialSolutionConcentration prev_c(&mesh_concentration, 0.0);

  // Numerical flux.
  OsherSolomonNumericalFlux num_flux(KAPPA);

  // Initialize weak formulation.
  EulerEquationsWeakFormSemiImplicitCoupled wf(&num_flux, KAPPA, RHO_EXT, V1_EXT, V2_EXT, P_EXT, BDY_SOLID_WALL_BOTTOM,
    BDY_SOLID_WALL_TOP, BDY_INLET, BDY_OUTLET, BDY_NATURAL_CONCENTRATION, &prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e, &prev_c, EPSILON, (P_INIT_FLOW == 0));
  
  wf.set_time_step(time_step);

  // Initialize the FE problem.
  DiscreteProblem dp(&wf, Hermes::vector<Space*>(&space_rho, &space_rho_v_x, &space_rho_v_y, &space_e, &space_c));

  // If the FE problem is in fact a FV problem.
  //if(P_INIT == 0) dp.set_fvm();  

  // Filters for visualization of Mach number, pressure and entropy.
  MachNumberFilter Mach_number(Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e), KAPPA);
  PressureFilter pressure(Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e), KAPPA);
  EntropyFilter entropy(Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e), KAPPA, RHO_EXT, P_EXT);

  /*
  ScalarView pressure_view("Pressure", new WinGeom(0, 0, 600, 300));
  ScalarView Mach_number_view("Mach number", new WinGeom(700, 0, 600, 300));
  ScalarView entropy_production_view("Entropy estimate", new WinGeom(0, 400, 600, 300));
  ScalarView s5("Concentration", new WinGeom(700, 400, 600, 300));
  */
  
  ScalarView s1("1", new WinGeom(0, 0, 600, 300));
  ScalarView s2("2", new WinGeom(700, 0, 600, 300));
  ScalarView s3("3", new WinGeom(0, 400, 600, 300));
  ScalarView s4("4", new WinGeom(700, 400, 600, 300));
  ScalarView s5("Concentration", new WinGeom(350, 200, 600, 300));

  // Set up the solver, matrix, and rhs according to the solver selection.
  SparseMatrix* matrix = create_matrix(matrix_solver);
  Vector* rhs = create_vector(matrix_solver);
  Solver* solver = create_linear_solver(matrix_solver, matrix, rhs);

  // Set up CFL calculation class.
  CFLCalculation CFL(CFL_NUMBER, KAPPA);

  // Set up Advection-Diffusion-Equation stability calculation class.
  ADEStabilityCalculation ADES(ADVECTION_STABILITY_CONSTANT, DIFFUSION_STABILITY_CONSTANT, EPSILON);

  int iteration = 0; double t = 0;
  for(t = 0.0; t < 100.0; t += time_step) {
    info("---- Time step %d, time %3.5f.", iteration++, t);

    // Set the current time step.
    wf.set_time_step(time_step);
    Space::update_essential_bc_values(&space_c, t);

    // Assemble stiffness matrix and rhs.
    info("Assembling the stiffness matrix and right-hand side vector.");
    dp.assemble(matrix, rhs);

    // Solve the matrix problem.
    info("Solving the matrix problem.");
    scalar* solution_vector = NULL;
    if(solver->solve()) {
      solution_vector = solver->get_solution();
      Solution::vector_to_solutions(solution_vector, Hermes::vector<Space *>(&space_rho, &space_rho_v_x, 
      &space_rho_v_y, &space_e, &space_c), Hermes::vector<Solution *>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e, &prev_c));
    }
    else
    error ("Matrix solver failed.\n");

    if(SHOCK_CAPTURING) {
      DiscontinuityDetector discontinuity_detector(Hermes::vector<Space *>(&space_rho, &space_rho_v_x, 
        &space_rho_v_y, &space_e), Hermes::vector<Solution *>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));

      std::set<int> discontinuous_elements = discontinuity_detector.get_discontinuous_element_ids(DISCONTINUITY_DETECTOR_PARAM);

      FluxLimiter flux_limiter(solution_vector, Hermes::vector<Space *>(&space_rho, &space_rho_v_x, 
        &space_rho_v_y, &space_e), Hermes::vector<Solution *>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));

      flux_limiter.limit_according_to_detector(discontinuous_elements);
    }

    util_time_step = time_step;

    CFL.calculate_semi_implicit(Hermes::vector<Solution *>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e), &mesh_flow, util_time_step);

    time_step = util_time_step;

    ADES.calculate(Hermes::vector<Solution *>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y), &mesh_concentration, util_time_step);

    if(util_time_step < time_step)
      time_step = util_time_step;

    // Visualization.
    if((iteration - 1) % EVERY_NTH_STEP == 0) {
      // Hermes visualization.
      if(HERMES_VISUALIZATION) {
        /*
        Mach_number.reinit();
        pressure.reinit();
        entropy.reinit();
        pressure_view.show(&pressure);
        entropy_production_view.show(&entropy);
        Mach_number_view.show(&Mach_number);
        s5.show(&prev_c);
        */
        s1.show(&prev_rho);
        s2.show(&prev_rho_v_x);
        s3.show(&prev_rho_v_y);
        s4.show(&prev_e);
        s5.show(&prev_c);
        /*
        s1.save_numbered_screenshot("density%i.bmp", iteration, true);
        s2.save_numbered_screenshot("density_v_x%i.bmp", iteration, true);
        s3.save_numbered_screenshot("density_v_y%i.bmp", iteration, true);
        s4.save_numbered_screenshot("energy%i.bmp", iteration, true);
        s5.save_numbered_screenshot("concentration%i.bmp", iteration, true);
        */
        //s5.wait_for_close();
        
      }
      // Output solution in VTK format.
      if(VTK_VISUALIZATION) {
        pressure.reinit();
        Mach_number.reinit();
        Linearizer lin;
        char filename[40];
        sprintf(filename, "pressure-%i.vtk", iteration - 1);
        lin.save_solution_vtk(&pressure, filename, "Pressure", false);
        sprintf(filename, "pressure-3D-%i.vtk", iteration - 1);
        lin.save_solution_vtk(&pressure, filename, "Pressure", true);
        sprintf(filename, "Mach number-%i.vtk", iteration - 1);
        lin.save_solution_vtk(&Mach_number, filename, "MachNumber", false);
        sprintf(filename, "Mach number-3D-%i.vtk", iteration - 1);
        lin.save_solution_vtk(&Mach_number, filename, "MachNumber", true);
        sprintf(filename, "Concentration-%i.vtk", iteration - 1);
        lin.save_solution_vtk(&prev_c, filename, "Concentration", true);
        sprintf(filename, "Concentration-3D-%i.vtk", iteration - 1);
        lin.save_solution_vtk(&prev_c, filename, "Concentration", true);
 
      }
    }
  }
  
  /*
  pressure_view.close();
  entropy_production_view.close();
  Mach_number_view.close();
  s5.close();
  */
  
  s1.close();
  s2.close();
  s3.close();
  s4.close();
  s5.close();

  return 0;
}
Esempio n. 7
0
void TestStores1()
{
  B(s00, Simple storage, 0) W(s00a);
  {
    c4_Storage s1 ("s00a", 1);
    s1.SetStructure("a[p1:I]");
    s1.Commit();
  } D(s00a); R(s00a); E;

  B(s01, Integer storage, 0) W(s01a);
  {
    c4_IntProp p1 ("p1");
    c4_Storage s1 ("s01a", 1);
    s1.SetStructure("a[p1:I]");
      c4_View v1 = s1.View("a");
      v1.Add(p1 [123]);
      v1.Add(p1 [456]);
      v1.InsertAt(1, p1 [789]);
        A(v1.GetSize() == 3);
    s1.Commit();
        A(v1.GetSize() == 3);
  } D(s01a); R(s01a); E;

#if !q4_TINY
  B(s02, Float storage, 0) W(s02a);
  {
    c4_FloatProp p1 ("p1");
    c4_Storage s1 ("s02a", 1);
    s1.SetStructure("a[p1:F]");
      c4_View v1 = s1.View("a");
      v1.Add(p1 [12.3]);
      v1.Add(p1 [45.6]);
      v1.InsertAt(1, p1 [78.9]);
    s1.Commit();
  } D(s02a); R(s02a); E;
#endif

  B(s03, String storage, 0) W(s03a);
  {
    c4_StringProp p1 ("p1");
    c4_Storage s1 ("s03a", 1);
    s1.SetStructure("a[p1:S]");
      c4_View v1 = s1.View("a");
      v1.Add(p1 ["one"]);
      v1.Add(p1 ["two"]);
      v1.InsertAt(1, p1 ["three"]);
    s1.Commit();
  } D(s03a); R(s03a); E;

  B(s04, View storage, 0) W(s04a);
  {
    c4_StringProp p1 ("p1");
    c4_ViewProp p2 ("p2");
    c4_IntProp p3 ("p3");
    c4_Storage s1 ("s04a", 1);
    s1.SetStructure("a[p1:S,p2[p3:I]]");
      c4_View v1 = s1.View("a");
      v1.Add(p1 ["one"]);
      v1.Add(p1 ["two"]);
      c4_View v2 = p2 (v1[0]);
      v2.Add(p3 [1]);
      v2 = p2 (v1[1]);
      v2.Add(p3 [11]);
      v2.Add(p3 [22]);
      v1.InsertAt(1, p1 ["three"]);
      v2 = p2 (v1[1]);
      v2.Add(p3 [111]);
      v2.Add(p3 [222]);
      v2.Add(p3 [333]);
      s1.Commit();
  } D(s04a); R(s04a); E;

  B(s05, Store and reload, 0) W(s05a);
  {
    c4_IntProp p1 ("p1");
    {
      c4_Storage s1 ("s05a", 1);
      s1.SetStructure("a[p1:I]");
      c4_View v1 = s1.View("a");
      v1.Add(p1 [123]);
      s1.Commit();
    }
    {
      c4_Storage s1 ("s05a", 0);
      c4_View v1 = s1.View("a");
        A(v1.GetSize() == 1);
        A(p1 (v1[0]) == 123);
    }
  } D(s05a); R(s05a); E;

  B(s06, Commit twice, 0) W(s06a);
  {
    c4_IntProp p1 ("p1");
    {
      c4_Storage s1 ("s06a", 1);
      s1.SetStructure("a[p1:I]");
      c4_View v1 = s1.View("a");
      v1.Add(p1 [123]);
      s1.Commit();
      v1.Add(p1 [234]);
      s1.Commit();
    }
    {
      c4_Storage s1 ("s06a", 0);
      c4_View v1 = s1.View("a");
        A(v1.GetSize() == 2);
        A(p1 (v1[0]) == 123);
        A(p1 (v1[1]) == 234);
    }
  } D(s06a); R(s06a); E;

  B(s07, Commit modified, 0) W(s07a);
  {
    c4_IntProp p1 ("p1");
    {
      c4_Storage s1 ("s07a", 1);
      s1.SetStructure("a[p1:I]");
      c4_View v1 = s1.View("a");
      v1.Add(p1 [123]);
      s1.Commit();
      p1 (v1[0]) = 234;
      s1.Commit();
    }
    {
      c4_Storage s1 ("s07a", 0);
      c4_View v1 = s1.View("a");
        A(v1.GetSize() == 1);
        A(p1 (v1[0]) == 234);
    }
  } D(s07a); R(s07a); E;

  B(s08, View after storage, 0) W(s08a);
  {
    c4_IntProp p1 ("p1");
    {
      c4_Storage s1 ("s08a", 1);
      s1.SetStructure("a[p1:I]");
      c4_View v1 = s1.View("a");
      v1.Add(p1 [123]);
      s1.Commit();
    }
    c4_View v1;
    {
      c4_Storage s1 ("s08a", 0);
      v1 = s1.View("a");
    }
        // 19990916 - semantics changed, view now 1 row, but 0 props
      A(v1.GetSize() == 1);
      A(v1.NumProperties() == 0);
    v1.InsertAt(0, p1 [234]);
      A(v1.GetSize() == 2);
      A(p1 (v1[0]) == 234);
      A(p1 (v1[1]) == 0); // the original value is gone
  } D(s08a); R(s08a); E;

  B(s09, Copy storage, 0) W(s09a); W(s09b);
  {
    c4_IntProp p1 ("p1");
    {
      c4_Storage s1 ("s09a", 1);
      s1.SetStructure("a[p1:I]");
      c4_View v1 = s1.View("a");
      v1.Add(p1 [123]);
      s1.Commit();
    }
    {
      c4_Storage s1 ("s09a", 0);
      c4_Storage s2 ("s09b", 1);
      s2.SetStructure("a[p1:I]");
      s2.View("a") = s1.View("a");
      s2.Commit();
    }
  } D(s09a); D(s09b); R(s09a); R(s09b); E;
}
Esempio n. 8
0
bool QgsPgTableModel::setData( const QModelIndex &idx, const QVariant &value, int role )
{
  if ( !QStandardItemModel::setData( idx, value, role ) )
    return false;

  if ( idx.column() == DbtmType || idx.column() == DbtmSrid || idx.column() == DbtmPkCol )
  {
    QgsWkbTypes::Type wkbType = ( QgsWkbTypes::Type ) idx.sibling( idx.row(), DbtmType ).data( Qt::UserRole + 2 ).toInt();

    QString tip;
    if ( wkbType == QgsWkbTypes::Unknown )
    {
      tip = tr( "Specify a geometry type in the '%1' column" ).arg( tr( "Data Type" ) );
    }
    else if ( wkbType != QgsWkbTypes::NoGeometry )
    {
      bool ok;
      int srid = idx.sibling( idx.row(), DbtmSrid ).data().toInt( &ok );

      if ( !ok || srid == std::numeric_limits<int>::min() )
        tip = tr( "Enter a SRID into the '%1' column" ).arg( tr( "SRID" ) );
    }

    QStringList pkCols = idx.sibling( idx.row(), DbtmPkCol ).data( Qt::UserRole + 1 ).toStringList();
    if ( tip.isEmpty() && !pkCols.isEmpty() )
    {
      QSet<QString> s0( idx.sibling( idx.row(), DbtmPkCol ).data( Qt::UserRole + 2 ).toStringList().toSet() );
      QSet<QString> s1( pkCols.toSet() );
      if ( !s0.intersects( s1 ) )
        tip = tr( "Select columns in the '%1' column that uniquely identify features of this layer" ).arg( tr( "Feature id" ) );
    }

    for ( int i = 0; i < DbtmColumns; i++ )
    {
      QStandardItem *item = itemFromIndex( idx.sibling( idx.row(), i ) );
      if ( tip.isEmpty() )
      {
        if ( i == DbtmSchema )
        {
          item->setData( QVariant(), Qt::DecorationRole );
        }

        item->setFlags( item->flags() | Qt::ItemIsSelectable );
        item->setToolTip( QString() );
      }
      else
      {
        item->setFlags( item->flags() & ~Qt::ItemIsSelectable );

        if ( i == DbtmSchema )
          item->setData( QgsApplication::getThemeIcon( QStringLiteral( "/mIconWarning.svg" ) ), Qt::DecorationRole );

        if ( i == DbtmSchema || i == DbtmTable || i == DbtmGeomCol )
        {
          item->setFlags( item->flags() );
          item->setToolTip( tip );
        }
      }
    }
  }

  return true;
}
Esempio n. 9
0
QString QgsPgTableModel::layerURI( const QModelIndex &index, const QString &connInfo, bool useEstimatedMetadata )
{
  if ( !index.isValid() )
  {
    QgsDebugMsg( QStringLiteral( "invalid index" ) );
    return QString();
  }

  QgsWkbTypes::Type wkbType = ( QgsWkbTypes::Type ) itemFromIndex( index.sibling( index.row(), DbtmType ) )->data( Qt::UserRole + 2 ).toInt();
  if ( wkbType == QgsWkbTypes::Unknown )
  {
    QgsDebugMsg( QStringLiteral( "unknown geometry type" ) );
    // no geometry type selected
    return QString();
  }

  QStandardItem *pkItem = itemFromIndex( index.sibling( index.row(), DbtmPkCol ) );
  QSet<QString> s0( pkItem->data( Qt::UserRole + 1 ).toStringList().toSet() );
  QSet<QString> s1( pkItem->data( Qt::UserRole + 2 ).toStringList().toSet() );
  if ( !s0.isEmpty() && !s0.intersects( s1 ) )
  {
    // no valid primary candidate selected
    QgsDebugMsg( QStringLiteral( "no pk candidate selected" ) );
    return QString();
  }

  QString schemaName = index.sibling( index.row(), DbtmSchema ).data( Qt::DisplayRole ).toString();
  QString tableName = index.sibling( index.row(), DbtmTable ).data( Qt::DisplayRole ).toString();

  QString geomColumnName;
  QString srid;
  if ( wkbType != QgsWkbTypes::NoGeometry )
  {
    geomColumnName = index.sibling( index.row(), DbtmGeomCol ).data( Qt::DisplayRole ).toString();

    srid = index.sibling( index.row(), DbtmSrid ).data( Qt::DisplayRole ).toString();
    bool ok;
    srid.toInt( &ok );
    if ( !ok )
    {
      QgsDebugMsg( QStringLiteral( "srid not numeric" ) );
      return QString();
    }
  }

  bool selectAtId = itemFromIndex( index.sibling( index.row(), DbtmSelectAtId ) )->checkState() == Qt::Checked;
  QString sql = index.sibling( index.row(), DbtmSql ).data( Qt::DisplayRole ).toString();
  bool checkPrimaryKeyUnicity = itemFromIndex( index.sibling( index.row(), DbtmCheckPkUnicity ) )->checkState() == Qt::Checked;

  QgsDataSourceUri uri( connInfo );

  QStringList cols;
  const auto constS1 = s1;
  for ( const QString &col : constS1 )
  {
    cols << QgsPostgresConn::quotedIdentifier( col );
  }

  QgsSettings().setValue( QStringLiteral( "/PostgreSQL/connections/%1/keys/%2/%3" ).arg( mConnName, schemaName, tableName ), QVariant( s1.toList() ) );

  uri.setDataSource( schemaName, tableName, geomColumnName, sql, cols.join( ',' ) );
  uri.setUseEstimatedMetadata( useEstimatedMetadata );
  uri.setWkbType( wkbType );
  uri.setSrid( srid );
  uri.disableSelectAtId( !selectAtId );
  uri.setParam( QStringLiteral( "checkPrimaryKeyUnicity" ), checkPrimaryKeyUnicity ? QLatin1Literal( "1" ) : QLatin1Literal( "0" ) );

  QgsDebugMsg( QStringLiteral( "returning uri %1" ).arg( uri.uri( false ) ) );
  return uri.uri( false );
}
Esempio n. 10
0
int setupAndSolveQP(NewQPControllerData *pdata, std::shared_ptr<drake::lcmt_qp_controller_input> qp_input, double t, Map<VectorXd> &q, Map<VectorXd> &qd, const Ref<Matrix<bool, Dynamic, 1>> &b_contact_force, QPControllerOutput *qp_output, std::shared_ptr<QPControllerDebugData> debug) {
  // The primary solve loop for our controller. This constructs and solves a Quadratic Program and produces the instantaneous desired torques, along with reference positions, velocities, and accelerations. It mirrors the Matlab implementation in atlasControllers.InstantaneousQPController.setupAndSolveQP(), and more documentation can be found there. 
  // Note: argument `debug` MAY be set to NULL, which signals that no debug information is requested.

  // look up the param set by name
  AtlasParams *params; 
  std::map<string,AtlasParams>::iterator it;
  it = pdata->param_sets.find(qp_input->param_set_name);
  if (it == pdata->param_sets.end()) {
    mexWarnMsgTxt("Got a param set I don't recognize! Using standing params instead");
    it = pdata->param_sets.find("standing");
    if (it == pdata->param_sets.end()) {
      mexErrMsgTxt("Could not fall back to standing parameters either. I have to give up here.");
    }
  }
  // cout << "using params set: " + it->first + ", ";
  params = &(it->second);
  // mexPrintf("Kp_accel: %f, ", params->Kp_accel);

  int nu = pdata->B.cols();
  int nq = pdata->r->num_positions;

  // zmp_data
  Map<Matrix<double, 4, 4, RowMajor>> A_ls(&qp_input->zmp_data.A[0][0]);
  Map<Matrix<double, 4, 2, RowMajor>> B_ls(&qp_input->zmp_data.B[0][0]);
  Map<Matrix<double, 2, 4, RowMajor>> C_ls(&qp_input->zmp_data.C[0][0]);
  Map<Matrix<double, 2, 2, RowMajor>> D_ls(&qp_input->zmp_data.D[0][0]);
  Map<Matrix<double, 4, 1>> x0(&qp_input->zmp_data.x0[0][0]);
  Map<Matrix<double, 2, 1>> y0(&qp_input->zmp_data.y0[0][0]);
  Map<Matrix<double, 2, 1>> u0(&qp_input->zmp_data.u0[0][0]);
  Map<Matrix<double, 2, 2, RowMajor>> R_ls(&qp_input->zmp_data.R[0][0]);
  Map<Matrix<double, 2, 2, RowMajor>> Qy(&qp_input->zmp_data.Qy[0][0]);
  Map<Matrix<double, 4, 4, RowMajor>> S(&qp_input->zmp_data.S[0][0]);
  Map<Matrix<double, 4, 1>> s1(&qp_input->zmp_data.s1[0][0]);
  Map<Matrix<double, 4, 1>> s1dot(&qp_input->zmp_data.s1dot[0][0]);

  // // whole_body_data
  if (qp_input->whole_body_data.num_positions != nq) mexErrMsgTxt("number of positions doesn't match num_dof for this robot");
  Map<VectorXd> q_des(qp_input->whole_body_data.q_des.data(), nq);
  Map<VectorXd> condof(qp_input->whole_body_data.constrained_dofs.data(), qp_input->whole_body_data.num_constrained_dofs);
  PIDOutput pid_out = wholeBodyPID(pdata, t, q, qd, q_des, &params->whole_body);
  qp_output->q_ref = pid_out.q_ref;

  // mu
  // NOTE: we're using the same mu for all supports
  double mu;
  if (qp_input->num_support_data == 0) {
    mu = 1.0;
  } else {
    mu = qp_input->support_data[0].mu;
    for (int i=1; i < qp_input->num_support_data; i++) {
      if (qp_input->support_data[i].mu != mu) {
        mexWarnMsgTxt("Currently, we assume that all supports have the same value of mu");
      }
    }
  }

  const int dim = 3, // 3D
  nd = 2*m_surface_tangents; // for friction cone approx, hard coded for now
  
  assert(nu+6 == nq);

  vector<DesiredBodyAcceleration> desired_body_accelerations;
  desired_body_accelerations.resize(qp_input->num_tracked_bodies);
  Vector6d body_pose_des, body_v_des, body_vdot_des;
  Vector6d body_vdot;

  for (int i=0; i < qp_input->num_tracked_bodies; i++) {
    int body_id0 = qp_input->body_motion_data[i].body_id - 1;
    double weight = params->body_motion[body_id0].weight;
    desired_body_accelerations[i].body_id0 = body_id0;
    Map<Matrix<double, 6, 4,RowMajor>>coefs_rowmaj(&qp_input->body_motion_data[i].coefs[0][0]);
    Matrix<double, 6, 4> coefs = coefs_rowmaj;
    evaluateCubicSplineSegment(t - qp_input->body_motion_data[i].ts[0], coefs, body_pose_des, body_v_des, body_vdot_des);
    desired_body_accelerations[i].body_vdot = bodyMotionPD(pdata->r, q, qd, body_id0, body_pose_des, body_v_des, body_vdot_des, params->body_motion[body_id0].Kp, params->body_motion[body_id0].Kd);
    desired_body_accelerations[i].weight = weight;
    desired_body_accelerations[i].accel_bounds = params->body_motion[body_id0].accel_bounds;
    // mexPrintf("body: %d, vdot: %f %f %f %f %f %f weight: %f\n", body_id0, 
    //           desired_body_accelerations[i].body_vdot(0), 
    //           desired_body_accelerations[i].body_vdot(1), 
    //           desired_body_accelerations[i].body_vdot(2), 
    //           desired_body_accelerations[i].body_vdot(3), 
    //           desired_body_accelerations[i].body_vdot(4), 
    //           desired_body_accelerations[i].body_vdot(5),
    //           weight);
      // mexPrintf("tracking body: %d, coefs[:,0]: %f %f %f %f %f %f coefs(", body_id0,
  }

  int n_body_accel_eq_constraints = 0;
  for (int i=0; i < desired_body_accelerations.size(); i++) {
    if (desired_body_accelerations[i].weight < 0)
      n_body_accel_eq_constraints++;
  }

  MatrixXd R_DQyD_ls = R_ls + D_ls.transpose()*Qy*D_ls;

  pdata->r->doKinematics(q,false,qd);

  //---------------------------------------------------------------------

  vector<SupportStateElement> available_supports = loadAvailableSupports(qp_input);
  vector<SupportStateElement> active_supports = getActiveSupports(pdata->r, pdata->map_ptr, q, qd, available_supports, b_contact_force, params->contact_threshold, pdata->default_terrain_height);

  int num_active_contact_pts=0;
  for (vector<SupportStateElement>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) {
    num_active_contact_pts += iter->contact_pts.size();
  }

  pdata->r->HandC(q,qd,(MatrixXd*)nullptr,pdata->H,pdata->C,(MatrixXd*)nullptr,(MatrixXd*)nullptr,(MatrixXd*)nullptr);

  pdata->H_float = pdata->H.topRows(6);
  pdata->H_act = pdata->H.bottomRows(nu);
  pdata->C_float = pdata->C.head(6);
  pdata->C_act = pdata->C.tail(nu);

  bool include_angular_momentum = (params->W_kdot.array().maxCoeff() > 1e-10);

  if (include_angular_momentum) {
    pdata->r->getCMM(q,qd,pdata->Ag,pdata->Agdot);
    pdata->Ak = pdata->Ag.topRows(3);
    pdata->Akdot = pdata->Agdot.topRows(3);
  }
  Vector3d xcom;
  // consider making all J's into row-major
  
  pdata->r->getCOM(xcom);
  pdata->r->getCOMJac(pdata->J);
  pdata->r->getCOMJacDot(pdata->Jdot);
  pdata->J_xy = pdata->J.topRows(2);
  pdata->Jdot_xy = pdata->Jdot.topRows(2);

  MatrixXd Jcom,Jcomdot;

  if (x0.size()==6) {
    Jcom = pdata->J;
    Jcomdot = pdata->Jdot;
  }
  else {
    Jcom = pdata->J_xy;
    Jcomdot = pdata->Jdot_xy;
  }
  
  MatrixXd B,JB,Jp,Jpdot,normals;
  int nc = contactConstraintsBV(pdata->r,num_active_contact_pts,mu,active_supports,pdata->map_ptr,B,JB,Jp,Jpdot,normals,pdata->default_terrain_height);
  int neps = nc*dim;

  VectorXd x_bar,xlimp;
  MatrixXd D_float(6,JB.cols()), D_act(nu,JB.cols());
  if (nc>0) {
    if (x0.size()==6) {
      // x,y,z com 
      xlimp.resize(6); 
      xlimp.topRows(3) = xcom;
      xlimp.bottomRows(3) = Jcom*qd;
    }
    else {
      xlimp.resize(4); 
      xlimp.topRows(2) = xcom.topRows(2);
      xlimp.bottomRows(2) = Jcom*qd;
    }
    x_bar = xlimp-x0;

    D_float = JB.topRows(6);
    D_act = JB.bottomRows(nu);
  }

  int nf = nc*nd; // number of contact force variables
  int nparams = nq+nf+neps;

  Vector3d kdot_des; 
  if (include_angular_momentum) {
    VectorXd k = pdata->Ak*qd;
    kdot_des = -params->Kp_ang*k; // TODO: parameterize
  }
  
  //----------------------------------------------------------------------
  // QP cost function ----------------------------------------------------
  //
  //  min: ybar*Qy*ybar + ubar*R*ubar + (2*S*xbar + s1)*(A*x + B*u) +
  //    w_qdd*quad(qddot_ref - qdd) + w_eps*quad(epsilon) +
  //    w_grf*quad(beta) + quad(kdot_des - (A*qdd + Adot*qd))  
  VectorXd f(nparams);
  {      
    if (nc > 0) {
      // NOTE: moved Hqp calcs below, because I compute the inverse directly for FastQP (and sparse Hqp for gurobi)
      VectorXd tmp = C_ls*xlimp;
      VectorXd tmp1 = Jcomdot*qd;
      MatrixXd tmp2 = R_DQyD_ls*Jcom;

      pdata->fqp = tmp.transpose()*Qy*D_ls*Jcom;
      // mexPrintf("fqp head: %f %f %f\n", pdata->fqp(0), pdata->fqp(1), pdata->fqp(2));
      pdata->fqp += tmp1.transpose()*tmp2;
      pdata->fqp += (S*x_bar + 0.5*s1).transpose()*B_ls*Jcom;
      pdata->fqp -= u0.transpose()*tmp2;
      pdata->fqp -= y0.transpose()*Qy*D_ls*Jcom;
      pdata->fqp -= (params->whole_body.w_qdd.array()*pid_out.qddot_des.array()).matrix().transpose();
      if (include_angular_momentum) {
        pdata->fqp += qd.transpose()*pdata->Akdot.transpose()*params->W_kdot*pdata->Ak;
        pdata->fqp -= kdot_des.transpose()*params->W_kdot*pdata->Ak;
      }
      f.head(nq) = pdata->fqp.transpose();
     } else {
      f.head(nq) = -pid_out.qddot_des;
    } 
  }
  f.tail(nf+neps) = VectorXd::Zero(nf+neps);
  
  int neq = 6+neps+6*n_body_accel_eq_constraints+qp_input->whole_body_data.num_constrained_dofs;
  MatrixXd Aeq = MatrixXd::Zero(neq,nparams);
  VectorXd beq = VectorXd::Zero(neq);
  
  // constrained floating base dynamics
  //  H_float*qdd - J_float'*lambda - Dbar_float*beta = -C_float
  Aeq.topLeftCorner(6,nq) = pdata->H_float;
  beq.topRows(6) = -pdata->C_float;
    
  if (nc>0) {
    Aeq.block(0,nq,6,nc*nd) = -D_float;
  }
  
  if (nc > 0) {
    // relative acceleration constraint
    Aeq.block(6,0,neps,nq) = Jp;
    Aeq.block(6,nq,neps,nf) = MatrixXd::Zero(neps,nf);  // note: obvious sparsity here
    Aeq.block(6,nq+nf,neps,neps) = MatrixXd::Identity(neps,neps);             // note: obvious sparsity here
    beq.segment(6,neps) = (-Jpdot -params->Kp_accel*Jp)*qd; 
  }    
  
  // add in body spatial equality constraints
  // VectorXd body_vdot;
  MatrixXd orig = MatrixXd::Zero(4,1);
  orig(3,0) = 1;
  int equality_ind = 6+neps;
  MatrixXd Jb(6,nq);
  MatrixXd Jbdot(6,nq);
  for (int i=0; i<desired_body_accelerations.size(); i++) {
    if (desired_body_accelerations[i].weight < 0) { // negative implies constraint
      if (!inSupport(active_supports,desired_body_accelerations[i].body_id0)) {
        pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb);
        pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot);

        for (int j=0; j<6; j++) {
          if (!std::isnan(desired_body_accelerations[i].body_vdot(j))) {
            Aeq.block(equality_ind,0,1,nq) = Jb.row(j);
            beq[equality_ind++] = -Jbdot.row(j)*qd + desired_body_accelerations[i].body_vdot(j);
          }
        }
      }
    }
  }

  if (qp_input->whole_body_data.num_constrained_dofs>0) {
    // add joint acceleration constraints
    for (int i=0; i<qp_input->whole_body_data.num_constrained_dofs; i++) {
      Aeq(equality_ind,(int)condof[i]-1) = 1;
      beq[equality_ind++] = pid_out.qddot_des[(int)condof[i]-1];
    }
  }  
  
  int n_ineq = 2*nu+2*6*desired_body_accelerations.size();
  MatrixXd Ain = MatrixXd::Zero(n_ineq,nparams);  // note: obvious sparsity here
  VectorXd bin = VectorXd::Zero(n_ineq);

  // linear input saturation constraints
  // u=B_act'*(H_act*qdd + C_act - Jz_act'*z - Dbar_act*beta)
  // using transpose instead of inverse because B is orthogonal
  Ain.topLeftCorner(nu,nq) = pdata->B_act.transpose()*pdata->H_act;
  Ain.block(0,nq,nu,nc*nd) = -pdata->B_act.transpose()*D_act;
  bin.head(nu) = -pdata->B_act.transpose()*pdata->C_act + pdata->umax;

  Ain.block(nu,0,nu,nparams) = -1*Ain.block(0,0,nu,nparams);
  bin.segment(nu,nu) = pdata->B_act.transpose()*pdata->C_act - pdata->umin;

  int constraint_start_index = 2*nu;
  for (int i=0; i<desired_body_accelerations.size(); i++) {
    pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb);
    pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot);
    Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = Jb;
    bin.segment(constraint_start_index,6) = -Jbdot*qd + desired_body_accelerations[i].accel_bounds.max;
    constraint_start_index += 6;
    Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = -Jb;
    bin.segment(constraint_start_index,6) = Jbdot*qd - desired_body_accelerations[i].accel_bounds.min;
    constraint_start_index += 6;
  }
       
  for (int i=0; i<n_ineq; i++) {
    // remove inf constraints---needed by gurobi
    if (std::isinf(double(bin(i)))) {
      Ain.row(i) = 0*Ain.row(i);
      bin(i)=0;
    }  
  }

  GRBmodel * model = nullptr;
  int info=-1;
  
  // set obj,lb,up
  VectorXd lb(nparams), ub(nparams);
  lb.head(nq) = pdata->qdd_lb;
  ub.head(nq) = pdata->qdd_ub;
  lb.segment(nq,nf) = VectorXd::Zero(nf);
  ub.segment(nq,nf) = 1e3*VectorXd::Ones(nf);
  lb.tail(neps) = -params->slack_limit*VectorXd::Ones(neps);
  ub.tail(neps) = params->slack_limit*VectorXd::Ones(neps);

  VectorXd alpha(nparams);

  MatrixXd Qnfdiag(nf,1), Qneps(neps,1);
  vector<MatrixXd*> QBlkDiag( nc>0 ? 3 : 1 );  // nq, nf, neps   // this one is for gurobi
  
  VectorXd w = (params->whole_body.w_qdd.array() + REG).matrix();
  #ifdef USE_MATRIX_INVERSION_LEMMA
  double max_body_accel_weight = -numeric_limits<double>::infinity();
  for (int i=0; i < desired_body_accelerations.size(); i++) {
    max_body_accel_weight = max(max_body_accel_weight, desired_body_accelerations[i].weight);
  }
  bool include_body_accel_cost_terms = desired_body_accelerations.size() > 0 && max_body_accel_weight > 1e-10;
  if (pdata->use_fast_qp > 0 && !include_angular_momentum && !include_body_accel_cost_terms)
  { 
    // TODO: update to include angular momentum, body accel objectives.

    //    We want Hqp inverse, which I can compute efficiently using the
    //    matrix inversion lemma (see wikipedia):
    //    inv(A + U'CV) = inv(A) - inv(A)*U* inv([ inv(C)+ V*inv(A)*U ]) V inv(A)
    if (nc>0) {
      MatrixXd Wi = ((1/(params->whole_body.w_qdd.array() + REG)).matrix()).asDiagonal();
      if (R_DQyD_ls.trace()>1e-15) { // R_DQyD_ls is not zero
        pdata->Hqp = Wi - Wi*Jcom.transpose()*(R_DQyD_ls.inverse() + Jcom*Wi*Jcom.transpose()).inverse()*Jcom*Wi;
      }
    } 
    else {
      pdata->Hqp = MatrixXd::Constant(nq,1,1/(1+REG));
    }

    #ifdef TEST_FAST_QP
      if (nc>0) {
        MatrixXd Hqp_test(nq,nq);
        MatrixXd W = w.asDiagonal();
        Hqp_test = (Jcom.transpose()*R_DQyD_ls*Jcom + W).inverse();
        if (((Hqp_test-pdata->Hqp).array().abs()).maxCoeff() > 1e-6) {
          mexErrMsgTxt("Q submatrix inverse from matrix inversion lemma does not match direct Q inverse.");
        }
      }
    #endif

    Qnfdiag = MatrixXd::Constant(nf,1,1/REG);
    Qneps = MatrixXd::Constant(neps,1,1/(.001+REG));

    QBlkDiag[0] = &pdata->Hqp;
    if (nc>0) {
      QBlkDiag[1] = &Qnfdiag;
      QBlkDiag[2] = &Qneps;     // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
    }

    MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
    VectorXd bin_lb_ub(n_ineq+2*nparams);
    Ain_lb_ub << Ain,            // note: obvious sparsity here
        -MatrixXd::Identity(nparams,nparams),
        MatrixXd::Identity(nparams,nparams);
    bin_lb_ub << bin, -lb, ub;

    info = fastQPThatTakesQinv(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->state.active, alpha);

    //if (info<0)   mexPrintf("fastQP info = %d.  Calling gurobi.\n", info);
  }
  else {
  #endif

    if (nc>0) {
      pdata->Hqp = Jcom.transpose()*R_DQyD_ls*Jcom;
      if (include_angular_momentum) {
        pdata->Hqp += pdata->Ak.transpose()*params->W_kdot*pdata->Ak;
      }
      pdata->Hqp += params->whole_body.w_qdd.asDiagonal();
      pdata->Hqp += REG*MatrixXd::Identity(nq,nq);
    } else {
      pdata->Hqp = (1+REG)*MatrixXd::Identity(nq,nq);
    }

    // add in body spatial acceleration cost terms
    for (int i=0; i<desired_body_accelerations.size(); i++) {
      if (desired_body_accelerations[i].weight > 0) {
        if (!inSupport(active_supports,desired_body_accelerations[i].body_id0)) {
          pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb);
          pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot);

          for (int j=0; j<6; j++) {
            if (!std::isnan(desired_body_accelerations[i].body_vdot[j])) {
              pdata->Hqp += desired_body_accelerations[i].weight*(Jb.row(j)).transpose()*Jb.row(j);
              f.head(nq) += desired_body_accelerations[i].weight*(qd.transpose()*Jbdot.row(j).transpose() - desired_body_accelerations[i].body_vdot[j])*Jb.row(j).transpose();
            }
          }
        }
      }
    }

    Qnfdiag = MatrixXd::Constant(nf,1,params->w_grf+REG);
    Qneps = MatrixXd::Constant(neps,1,params->w_slack+REG);

    QBlkDiag[0] = &pdata->Hqp;
    if (nc>0) {
      QBlkDiag[1] = &Qnfdiag;
      QBlkDiag[2] = &Qneps;     // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
    }


    MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
    VectorXd bin_lb_ub(n_ineq+2*nparams);
    Ain_lb_ub << Ain,            // note: obvious sparsity here
        -MatrixXd::Identity(nparams,nparams),
        MatrixXd::Identity(nparams,nparams);
    bin_lb_ub << bin, -lb, ub;


    if (pdata->use_fast_qp > 0)
    { // set up and call fastqp
      info = fastQP(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->state.active, alpha);
      //if (info<0)    mexPrintf("fastQP info=%d... calling Gurobi.\n", info);
    }
    else {
      // use gurobi active set 
      model = gurobiActiveSetQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->state.vbasis,pdata->state.vbasis_len,pdata->state.cbasis,pdata->state.cbasis_len,alpha);
      CGE(GRBgetintattr(model,"NumVars",&(pdata->state.vbasis_len)), pdata->env);
      CGE(GRBgetintattr(model,"NumConstrs",&(pdata->state.cbasis_len)), pdata->env);
      info=66;
      //info = -1;
    }

    if (info<0) {
      model = gurobiQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->state.active,alpha);
      int status; CGE(GRBgetintattr(model, "Status", &status), pdata->env);
      //if (status!=2) mexPrintf("Gurobi reports non-optimal status = %d\n", status);
    }
  #ifdef USE_MATRIX_INVERSION_LEMMA
  }
  #endif

  //----------------------------------------------------------------------
  // Solve for inputs ----------------------------------------------------
  qp_output->qdd = alpha.head(nq);
  VectorXd beta = alpha.segment(nq,nc*nd);

  // use transpose because B_act is orthogonal
  qp_output->u = pdata->B_act.transpose()*(pdata->H_act*qp_output->qdd + pdata->C_act - D_act*beta);
  //y = pdata->B_act.jacobiSvd(ComputeThinU|ComputeThinV).solve(pdata->H_act*qdd + pdata->C_act - Jz_act.transpose()*lambda - D_act*beta);

  bool foot_contact[2];
  foot_contact[0] = b_contact_force(pdata->rpc.body_ids.r_foot) == 1;
  foot_contact[1] = b_contact_force(pdata->rpc.body_ids.l_foot) == 1;
  qp_output->qd_ref = velocityReference(pdata, t, q, qd, qp_output->qdd, foot_contact, &(params->vref_integrator), &(pdata->rpc));

  // Remember t for next time around
  pdata->state.t_prev = t;

  // If a debug pointer was passed in, fill it with useful data
  if (debug) {
    debug->active_supports.resize(active_supports.size());
    for (int i=0; i < active_supports.size(); i++) {
      debug->active_supports[i] = active_supports[i];
    }
    debug->nc = nc;
    debug->normals = normals;
    debug->B = B;
    debug->alpha = alpha;
    debug->f = f;
    debug->Aeq = Aeq;
    debug->beq = beq;
    debug->Ain_lb_ub = Ain_lb_ub;
    debug->bin_lb_ub = bin_lb_ub;
    debug->Qnfdiag = Qnfdiag;
    debug->Qneps = Qneps;
    debug->x_bar = x_bar;
    debug->S = S;
    debug->s1 = s1;
    debug->s1dot = s1dot;
    debug->s2dot = qp_input->zmp_data.s2dot;
    debug->A_ls = A_ls;
    debug->B_ls = B_ls;
    debug->Jcom = Jcom;
    debug->Jcomdot = Jcomdot;
    debug->beta = beta;
  }

  // if we used gurobi, clean up
  if (model) { 
    GRBfreemodel(model); 
  } 
  //  GRBfreeenv(env);

  return info;
}
Esempio n. 11
0
int main() {
    S1 s1( f1 );
}
Esempio n. 12
0
KTYPE
deseval (
	const KTYPE	*p,
	const KTYPE	*c,
	const KTYPE	*k
) {
	aligned register KTYPE	result = KCONST_1;
	aligned register KTYPE	l0 = p[6];
	aligned register KTYPE	l1 = p[14];
	aligned register KTYPE	l2 = p[22];
	aligned register KTYPE	l3 = p[30];
	aligned register KTYPE	l4 = p[38];
	aligned register KTYPE	l5 = p[46];
	aligned register KTYPE	l6 = p[54];
	aligned register KTYPE	l7 = p[62];
	aligned register KTYPE	l8 = p[4];
	aligned register KTYPE	l9 = p[12];
	aligned register KTYPE	l10 = p[20];
	aligned register KTYPE	l11 = p[28];
	aligned register KTYPE	l12 = p[36];
	aligned register KTYPE	l13 = p[44];
	aligned register KTYPE	l14 = p[52];
	aligned register KTYPE	l15 = p[60];
	aligned register KTYPE	l16 = p[2];
	aligned register KTYPE	l17 = p[10];
	aligned register KTYPE	l18 = p[18];
	aligned register KTYPE	l19 = p[26];
	aligned register KTYPE	l20 = p[34];
	aligned register KTYPE	l21 = p[42];
	aligned register KTYPE	l22 = p[50];
	aligned register KTYPE	l23 = p[58];
	aligned register KTYPE	l24 = p[0];
	aligned register KTYPE	l25 = p[8];
	aligned register KTYPE	l26 = p[16];
	aligned register KTYPE	l27 = p[24];
	aligned register KTYPE	l28 = p[32];
	aligned register KTYPE	l29 = p[40];
	aligned register KTYPE	l30 = p[48];
	aligned register KTYPE	l31 = p[56];
	aligned register KTYPE	r0 = p[7];
	aligned register KTYPE	r1 = p[15];
	aligned register KTYPE	r2 = p[23];
	aligned register KTYPE	r3 = p[31];
	aligned register KTYPE	r4 = p[39];
	aligned register KTYPE	r5 = p[47];
	aligned register KTYPE	r6 = p[55];
	aligned register KTYPE	r7 = p[63];
	aligned register KTYPE	r8 = p[5];
	aligned register KTYPE	r9 = p[13];
	aligned register KTYPE	r10 = p[21];
	aligned register KTYPE	r11 = p[29];
	aligned register KTYPE	r12 = p[37];
	aligned register KTYPE	r13 = p[45];
	aligned register KTYPE	r14 = p[53];
	aligned register KTYPE	r15 = p[61];
	aligned register KTYPE	r16 = p[3];
	aligned register KTYPE	r17 = p[11];
	aligned register KTYPE	r18 = p[19];
	aligned register KTYPE	r19 = p[27];
	aligned register KTYPE	r20 = p[35];
	aligned register KTYPE	r21 = p[43];
	aligned register KTYPE	r22 = p[51];
	aligned register KTYPE	r23 = p[59];
	aligned register KTYPE	r24 = p[1];
	aligned register KTYPE	r25 = p[9];
	aligned register KTYPE	r26 = p[17];
	aligned register KTYPE	r27 = p[25];
	aligned register KTYPE	r28 = p[33];
	aligned register KTYPE	r29 = p[41];
	aligned register KTYPE	r30 = p[49];
	aligned register KTYPE	r31 = p[57];

	s1 (r31 ^ k[47], r0 ^ k[11], r1 ^ k[26], r2 ^ k[3], r3 ^ k[13],
		r4 ^ k[41], &l8, &l16, &l22, &l30);
	s2 (r3 ^ k[27], r4 ^ k[6], r5 ^ k[54], r6 ^ k[48], r7 ^ k[39],
		r8 ^ k[19], &l12, &l27, &l1, &l17);
	s3 (r7 ^ k[53], r8 ^ k[25], r9 ^ k[33], r10 ^ k[34], r11 ^ k[17],
		r12 ^ k[5], &l23, &l15, &l29, &l5);
	s4 (r11 ^ k[4], r12 ^ k[55], r13 ^ k[24], r14 ^ k[32], r15 ^ k[40],
		r16 ^ k[20], &l25, &l19, &l9, &l0);
	s5 (r15 ^ k[36], r16 ^ k[31], r17 ^ k[21], r18 ^ k[8], r19 ^ k[23],
		r20 ^ k[52], &l7, &l13, &l24, &l2);
	s6 (r19 ^ k[14], r20 ^ k[29], r21 ^ k[51], r22 ^ k[9], r23 ^ k[35],
		r24 ^ k[30], &l3, &l28, &l10, &l18);
	s7 (r23 ^ k[2], r24 ^ k[37], r25 ^ k[22], r26 ^ k[0], r27 ^ k[42],
		r28 ^ k[38], &l31, &l11, &l21, &l6);
	s8 (r27 ^ k[16], r28 ^ k[43], r29 ^ k[44], r30 ^ k[1], r31 ^ k[7],
		r0 ^ k[28], &l4, &l26, &l14, &l20);
	s1 (l31 ^ k[54], l0 ^ k[18], l1 ^ k[33], l2 ^ k[10], l3 ^ k[20],
		l4 ^ k[48], &r8, &r16, &r22, &r30);
	s2 (l3 ^ k[34], l4 ^ k[13], l5 ^ k[4], l6 ^ k[55], l7 ^ k[46],
		l8 ^ k[26], &r12, &r27, &r1, &r17);
	s3 (l7 ^ k[3], l8 ^ k[32], l9 ^ k[40], l10 ^ k[41], l11 ^ k[24],
		l12 ^ k[12], &r23, &r15, &r29, &r5);
	s4 (l11 ^ k[11], l12 ^ k[5], l13 ^ k[6], l14 ^ k[39], l15 ^ k[47],
		l16 ^ k[27], &r25, &r19, &r9, &r0);
	s5 (l15 ^ k[43], l16 ^ k[38], l17 ^ k[28], l18 ^ k[15], l19 ^ k[30],
		l20 ^ k[0], &r7, &r13, &r24, &r2);
	s6 (l19 ^ k[21], l20 ^ k[36], l21 ^ k[31], l22 ^ k[16], l23 ^ k[42],
		l24 ^ k[37], &r3, &r28, &r10, &r18);
	s7 (l23 ^ k[9], l24 ^ k[44], l25 ^ k[29], l26 ^ k[7], l27 ^ k[49],
		l28 ^ k[45], &r31, &r11, &r21, &r6);
	s8 (l27 ^ k[23], l28 ^ k[50], l29 ^ k[51], l30 ^ k[8], l31 ^ k[14],
		l0 ^ k[35], &r4, &r26, &r14, &r20);
	s1 (r31 ^ k[11], r0 ^ k[32], r1 ^ k[47], r2 ^ k[24], r3 ^ k[34],
		r4 ^ k[5], &l8, &l16, &l22, &l30);
	s2 (r3 ^ k[48], r4 ^ k[27], r5 ^ k[18], r6 ^ k[12], r7 ^ k[3],
		r8 ^ k[40], &l12, &l27, &l1, &l17);
	s3 (r7 ^ k[17], r8 ^ k[46], r9 ^ k[54], r10 ^ k[55], r11 ^ k[13],
		r12 ^ k[26], &l23, &l15, &l29, &l5);
	s4 (r11 ^ k[25], r12 ^ k[19], r13 ^ k[20], r14 ^ k[53], r15 ^ k[4],
		r16 ^ k[41], &l25, &l19, &l9, &l0);
	s5 (r15 ^ k[2], r16 ^ k[52], r17 ^ k[42], r18 ^ k[29], r19 ^ k[44],
		r20 ^ k[14], &l7, &l13, &l24, &l2);
	s6 (r19 ^ k[35], r20 ^ k[50], r21 ^ k[45], r22 ^ k[30], r23 ^ k[1],
		r24 ^ k[51], &l3, &l28, &l10, &l18);
	s7 (r23 ^ k[23], r24 ^ k[31], r25 ^ k[43], r26 ^ k[21], r27 ^ k[8],
		r28 ^ k[0], &l31, &l11, &l21, &l6);
	s8 (r27 ^ k[37], r28 ^ k[9], r29 ^ k[38], r30 ^ k[22], r31 ^ k[28],
		r0 ^ k[49], &l4, &l26, &l14, &l20);
	s1 (l31 ^ k[25], l0 ^ k[46], l1 ^ k[4], l2 ^ k[13], l3 ^ k[48],
		l4 ^ k[19], &r8, &r16, &r22, &r30);
	s2 (l3 ^ k[5], l4 ^ k[41], l5 ^ k[32], l6 ^ k[26], l7 ^ k[17],
		l8 ^ k[54], &r12, &r27, &r1, &r17);
	s3 (l7 ^ k[6], l8 ^ k[3], l9 ^ k[11], l10 ^ k[12], l11 ^ k[27],
		l12 ^ k[40], &r23, &r15, &r29, &r5);
	s4 (l11 ^ k[39], l12 ^ k[33], l13 ^ k[34], l14 ^ k[10], l15 ^ k[18],
		l16 ^ k[55], &r25, &r19, &r9, &r0);
	s5 (l15 ^ k[16], l16 ^ k[7], l17 ^ k[1], l18 ^ k[43], l19 ^ k[31],
		l20 ^ k[28], &r7, &r13, &r24, &r2);
	s6 (l19 ^ k[49], l20 ^ k[9], l21 ^ k[0], l22 ^ k[44], l23 ^ k[15],
		l24 ^ k[38], &r3, &r28, &r10, &r18);
	s7 (l23 ^ k[37], l24 ^ k[45], l25 ^ k[2], l26 ^ k[35], l27 ^ k[22],
		l28 ^ k[14], &r31, &r11, &r21, &r6);
	s8 (l27 ^ k[51], l28 ^ k[23], l29 ^ k[52], l30 ^ k[36], l31 ^ k[42],
		l0 ^ k[8], &r4, &r26, &r14, &r20);
	s1 (r31 ^ k[39], r0 ^ k[3], r1 ^ k[18], r2 ^ k[27], r3 ^ k[5],
		r4 ^ k[33], &l8, &l16, &l22, &l30);
	s2 (r3 ^ k[19], r4 ^ k[55], r5 ^ k[46], r6 ^ k[40], r7 ^ k[6],
		r8 ^ k[11], &l12, &l27, &l1, &l17);
	s3 (r7 ^ k[20], r8 ^ k[17], r9 ^ k[25], r10 ^ k[26], r11 ^ k[41],
		r12 ^ k[54], &l23, &l15, &l29, &l5);
	s4 (r11 ^ k[53], r12 ^ k[47], r13 ^ k[48], r14 ^ k[24], r15 ^ k[32],
		r16 ^ k[12], &l25, &l19, &l9, &l0);
	s5 (r15 ^ k[30], r16 ^ k[21], r17 ^ k[15], r18 ^ k[2], r19 ^ k[45],
		r20 ^ k[42], &l7, &l13, &l24, &l2);
	s6 (r19 ^ k[8], r20 ^ k[23], r21 ^ k[14], r22 ^ k[31], r23 ^ k[29],
		r24 ^ k[52], &l3, &l28, &l10, &l18);
	s7 (r23 ^ k[51], r24 ^ k[0], r25 ^ k[16], r26 ^ k[49], r27 ^ k[36],
		r28 ^ k[28], &l31, &l11, &l21, &l6);
	s8 (r27 ^ k[38], r28 ^ k[37], r29 ^ k[7], r30 ^ k[50], r31 ^ k[1],
		r0 ^ k[22], &l4, &l26, &l14, &l20);
	s1 (l31 ^ k[53], l0 ^ k[17], l1 ^ k[32], l2 ^ k[41], l3 ^ k[19],
		l4 ^ k[47], &r8, &r16, &r22, &r30);
	s2 (l3 ^ k[33], l4 ^ k[12], l5 ^ k[3], l6 ^ k[54], l7 ^ k[20],
		l8 ^ k[25], &r12, &r27, &r1, &r17);
	s3 (l7 ^ k[34], l8 ^ k[6], l9 ^ k[39], l10 ^ k[40], l11 ^ k[55],
		l12 ^ k[11], &r23, &r15, &r29, &r5);
	s4 (l11 ^ k[10], l12 ^ k[4], l13 ^ k[5], l14 ^ k[13], l15 ^ k[46],
		l16 ^ k[26], &r25, &r19, &r9, &r0);
	s5 (l15 ^ k[44], l16 ^ k[35], l17 ^ k[29], l18 ^ k[16], l19 ^ k[0],
		l20 ^ k[1], &r7, &r13, &r24, &r2);
	s6 (l19 ^ k[22], l20 ^ k[37], l21 ^ k[28], l22 ^ k[45], l23 ^ k[43],
		l24 ^ k[7], &r3, &r28, &r10, &r18);
	s7 (l23 ^ k[38], l24 ^ k[14], l25 ^ k[30], l26 ^ k[8], l27 ^ k[50],
		l28 ^ k[42], &r31, &r11, &r21, &r6);
	s8 (l27 ^ k[52], l28 ^ k[51], l29 ^ k[21], l30 ^ k[9], l31 ^ k[15],
		l0 ^ k[36], &r4, &r26, &r14, &r20);
	s1 (r31 ^ k[10], r0 ^ k[6], r1 ^ k[46], r2 ^ k[55], r3 ^ k[33],
		r4 ^ k[4], &l8, &l16, &l22, &l30);
	s2 (r3 ^ k[47], r4 ^ k[26], r5 ^ k[17], r6 ^ k[11], r7 ^ k[34],
		r8 ^ k[39], &l12, &l27, &l1, &l17);
	s3 (r7 ^ k[48], r8 ^ k[20], r9 ^ k[53], r10 ^ k[54], r11 ^ k[12],
		r12 ^ k[25], &l23, &l15, &l29, &l5);
	s4 (r11 ^ k[24], r12 ^ k[18], r13 ^ k[19], r14 ^ k[27], r15 ^ k[3],
		r16 ^ k[40], &l25, &l19, &l9, &l0);
	s5 (r15 ^ k[31], r16 ^ k[49], r17 ^ k[43], r18 ^ k[30], r19 ^ k[14],
		r20 ^ k[15], &l7, &l13, &l24, &l2);
	s6 (r19 ^ k[36], r20 ^ k[51], r21 ^ k[42], r22 ^ k[0], r23 ^ k[2],
		r24 ^ k[21], &l3, &l28, &l10, &l18);
	s7 (r23 ^ k[52], r24 ^ k[28], r25 ^ k[44], r26 ^ k[22], r27 ^ k[9],
		r28 ^ k[1], &l31, &l11, &l21, &l6);
	s8 (r27 ^ k[7], r28 ^ k[38], r29 ^ k[35], r30 ^ k[23], r31 ^ k[29],
		r0 ^ k[50], &l4, &l26, &l14, &l20);
	s1 (l31 ^ k[24], l0 ^ k[20], l1 ^ k[3], l2 ^ k[12], l3 ^ k[47],
		l4 ^ k[18], &r8, &r16, &r22, &r30);
	s2 (l3 ^ k[4], l4 ^ k[40], l5 ^ k[6], l6 ^ k[25], l7 ^ k[48],
		l8 ^ k[53], &r12, &r27, &r1, &r17);
	s3 (l7 ^ k[5], l8 ^ k[34], l9 ^ k[10], l10 ^ k[11], l11 ^ k[26],
		l12 ^ k[39], &r23, &r15, &r29, &r5);
	s4 (l11 ^ k[13], l12 ^ k[32], l13 ^ k[33], l14 ^ k[41], l15 ^ k[17],
		l16 ^ k[54], &r25, &r19, &r9, &r0);
	s5 (l15 ^ k[45], l16 ^ k[8], l17 ^ k[2], l18 ^ k[44], l19 ^ k[28],
		l20 ^ k[29], &r7, &r13, &r24, &r2);
	s6 (l19 ^ k[50], l20 ^ k[38], l21 ^ k[1], l22 ^ k[14], l23 ^ k[16],
		l24 ^ k[35], &r3, &r28, &r10, &r18);
	s7 (l23 ^ k[7], l24 ^ k[42], l25 ^ k[31], l26 ^ k[36], l27 ^ k[23],
		l28 ^ k[15], &r31, &r11, &r21, &r6);
	s8 (l27 ^ k[21], l28 ^ k[52], l29 ^ k[49], l30 ^ k[37], l31 ^ k[43],
		l0 ^ k[9], &r4, &r26, &r14, &r20);
	s1 (r31 ^ k[6], r0 ^ k[27], r1 ^ k[10], r2 ^ k[19], r3 ^ k[54],
		r4 ^ k[25], &l8, &l16, &l22, &l30);
	s2 (r3 ^ k[11], r4 ^ k[47], r5 ^ k[13], r6 ^ k[32], r7 ^ k[55],
		r8 ^ k[3], &l12, &l27, &l1, &l17);
	s3 (r7 ^ k[12], r8 ^ k[41], r9 ^ k[17], r10 ^ k[18], r11 ^ k[33],
		r12 ^ k[46], &l23, &l15, &l29, &l5);
	s4 (r11 ^ k[20], r12 ^ k[39], r13 ^ k[40], r14 ^ k[48], r15 ^ k[24],
		r16 ^ k[4], &l25, &l19, &l9, &l0);
	s5 (r15 ^ k[52], r16 ^ k[15], r17 ^ k[9], r18 ^ k[51], r19 ^ k[35],
		r20 ^ k[36], &l7, &l13, &l24, &l2);
	s6 (r19 ^ k[2], r20 ^ k[45], r21 ^ k[8], r22 ^ k[21], r23 ^ k[23],
		r24 ^ k[42], &l3, &l28, &l10, &l18);
	s7 (r23 ^ k[14], r24 ^ k[49], r25 ^ k[38], r26 ^ k[43], r27 ^ k[30],
		r28 ^ k[22], &l31, &l11, &l21, &l6);
	s8 (r27 ^ k[28], r28 ^ k[0], r29 ^ k[1], r30 ^ k[44], r31 ^ k[50],
		r0 ^ k[16], &l4, &l26, &l14, &l20);
	s1 (l31 ^ k[20], l0 ^ k[41], l1 ^ k[24], l2 ^ k[33], l3 ^ k[11],
		l4 ^ k[39], &r8, &r16, &r22, &r30);
	s2 (l3 ^ k[25], l4 ^ k[4], l5 ^ k[27], l6 ^ k[46], l7 ^ k[12],
		l8 ^ k[17], &r12, &r27, &r1, &r17);
	s3 (l7 ^ k[26], l8 ^ k[55], l9 ^ k[6], l10 ^ k[32], l11 ^ k[47],
		l12 ^ k[3], &r23, &r15, &r29, &r5);
	s4 (l11 ^ k[34], l12 ^ k[53], l13 ^ k[54], l14 ^ k[5], l15 ^ k[13],
		l16 ^ k[18], &r25, &r19, &r9, &r0);
	s5 (l15 ^ k[7], l16 ^ k[29], l17 ^ k[23], l18 ^ k[38], l19 ^ k[49],
		l20 ^ k[50], &r7, &r13, &r24, &r2);
	s6 (l19 ^ k[16], l20 ^ k[0], l21 ^ k[22], l22 ^ k[35], l23 ^ k[37],
		l24 ^ k[1], &r3, &r28, &r10, &r18);
	s7 (l23 ^ k[28], l24 ^ k[8], l25 ^ k[52], l26 ^ k[2], l27 ^ k[44],
		l28 ^ k[36], &r31, &r11, &r21, &r6);
	s8 (l27 ^ k[42], l28 ^ k[14], l29 ^ k[15], l30 ^ k[31], l31 ^ k[9],
		l0 ^ k[30], &r4, &r26, &r14, &r20);
	s1 (r31 ^ k[34], r0 ^ k[55], r1 ^ k[13], r2 ^ k[47], r3 ^ k[25],
		r4 ^ k[53], &l8, &l16, &l22, &l30);
	s2 (r3 ^ k[39], r4 ^ k[18], r5 ^ k[41], r6 ^ k[3], r7 ^ k[26],
		r8 ^ k[6], &l12, &l27, &l1, &l17);
	s3 (r7 ^ k[40], r8 ^ k[12], r9 ^ k[20], r10 ^ k[46], r11 ^ k[4],
		r12 ^ k[17], &l23, &l15, &l29, &l5);
	s4 (r11 ^ k[48], r12 ^ k[10], r13 ^ k[11], r14 ^ k[19], r15 ^ k[27],
		r16 ^ k[32], &l25, &l19, &l9, &l0);
	s5 (r15 ^ k[21], r16 ^ k[43], r17 ^ k[37], r18 ^ k[52], r19 ^ k[8],
		r20 ^ k[9], &l7, &l13, &l24, &l2);
	s6 (r19 ^ k[30], r20 ^ k[14], r21 ^ k[36], r22 ^ k[49], r23 ^ k[51],
		r24 ^ k[15], &l3, &l28, &l10, &l18);
	s7 (r23 ^ k[42], r24 ^ k[22], r25 ^ k[7], r26 ^ k[16], r27 ^ k[31],
		r28 ^ k[50], &l31, &l11, &l21, &l6);
	s8 (r27 ^ k[1], r28 ^ k[28], r29 ^ k[29], r30 ^ k[45], r31 ^ k[23],
		r0 ^ k[44], &l4, &l26, &l14, &l20);
	s1 (l31 ^ k[48], l0 ^ k[12], l1 ^ k[27], l2 ^ k[4], l3 ^ k[39],
		l4 ^ k[10], &r8, &r16, &r22, &r30);
	s2 (l3 ^ k[53], l4 ^ k[32], l5 ^ k[55], l6 ^ k[17], l7 ^ k[40],
		l8 ^ k[20], &r12, &r27, &r1, &r17);
	s3 (l7 ^ k[54], l8 ^ k[26], l9 ^ k[34], l10 ^ k[3], l11 ^ k[18],
		l12 ^ k[6], &r23, &r15, &r29, &r5);
	s4 (l11 ^ k[5], l12 ^ k[24], l13 ^ k[25], l14 ^ k[33], l15 ^ k[41],
		l16 ^ k[46], &r25, &r19, &r9, &r0);
	s5 (l15 ^ k[35], l16 ^ k[2], l17 ^ k[51], l18 ^ k[7], l19 ^ k[22],
		l20 ^ k[23], &r7, &r13, &r24, &r2);
	s6 (l19 ^ k[44], l20 ^ k[28], l21 ^ k[50], l22 ^ k[8], l23 ^ k[38],
		l24 ^ k[29], &r3, &r28, &r10, &r18);
	s7 (l23 ^ k[1], l24 ^ k[36], l25 ^ k[21], l26 ^ k[30], l27 ^ k[45],
		l28 ^ k[9], &r31, &r11, &r21, &r6);
	s8 (l27 ^ k[15], l28 ^ k[42], l29 ^ k[43], l30 ^ k[0], l31 ^ k[37],
		l0 ^ k[31], &r4, &r26, &r14, &r20);
	s1 (r31 ^ k[5], r0 ^ k[26], r1 ^ k[41], r2 ^ k[18], r3 ^ k[53],
		r4 ^ k[24], &l8, &l16, &l22, &l30);
	s2 (r3 ^ k[10], r4 ^ k[46], r5 ^ k[12], r6 ^ k[6], r7 ^ k[54],
		r8 ^ k[34], &l12, &l27, &l1, &l17);
	s3 (r7 ^ k[11], r8 ^ k[40], r9 ^ k[48], r10 ^ k[17], r11 ^ k[32],
		r12 ^ k[20], &l23, &l15, &l29, &l5);
	s4 (r11 ^ k[19], r12 ^ k[13], r13 ^ k[39], r14 ^ k[47], r15 ^ k[55],
		r16 ^ k[3], &l25, &l19, &l9, &l0);
	s5 (r15 ^ k[49], r16 ^ k[16], r17 ^ k[38], r18 ^ k[21], r19 ^ k[36],
		r20 ^ k[37], &l7, &l13, &l24, &l2);
	s6 (r19 ^ k[31], r20 ^ k[42], r21 ^ k[9], r22 ^ k[22], r23 ^ k[52],
		r24 ^ k[43], &l3, &l28, &l10, &l18);
	s7 (r23 ^ k[15], r24 ^ k[50], r25 ^ k[35], r26 ^ k[44], r27 ^ k[0],
		r28 ^ k[23], &l31, &l11, &l21, &l6);
	s8 (r27 ^ k[29], r28 ^ k[1], r29 ^ k[2], r30 ^ k[14], r31 ^ k[51],
		r0 ^ k[45], &l4, &l26, &l14, &l20);
	s1 (l31 ^ k[19], l0 ^ k[40], l1 ^ k[55], l2 ^ k[32], l3 ^ k[10],
		l4 ^ k[13], &r8, &r16, &r22, &r30);
	s2 (l3 ^ k[24], l4 ^ k[3], l5 ^ k[26], l6 ^ k[20], l7 ^ k[11],
		l8 ^ k[48], &r12, &r27, &r1, &r17);
	s3 (l7 ^ k[25], l8 ^ k[54], l9 ^ k[5], l10 ^ k[6], l11 ^ k[46],
		l12 ^ k[34], &r23, &r15, &r29, &r5);
	s4 (l11 ^ k[33], l12 ^ k[27], l13 ^ k[53], l14 ^ k[4], l15 ^ k[12],
		l16 ^ k[17], &r25, &r19, &r9, &r0);
	s5 (l15 ^ k[8], l16 ^ k[30], l17 ^ k[52], l18 ^ k[35], l19 ^ k[50],
		l20 ^ k[51], &r7, &r13, &r24, &r2);
	s6 (l19 ^ k[45], l20 ^ k[1], l21 ^ k[23], l22 ^ k[36], l23 ^ k[7],
		l24 ^ k[2], &r3, &r28, &r10, &r18);
	s7 (l23 ^ k[29], l24 ^ k[9], l25 ^ k[49], l26 ^ k[31], l27 ^ k[14],
		l28 ^ k[37], &r31, &r11, &r21, &r6);
	s8 (l27 ^ k[43], l28 ^ k[15], l29 ^ k[16], l30 ^ k[28], l31 ^ k[38],
		l0 ^ k[0], &r4, &r26, &r14, &r20);
	s1 (r31 ^ k[33], r0 ^ k[54], r1 ^ k[12], r2 ^ k[46], r3 ^ k[24],
		r4 ^ k[27], &l8, &l16, &l22, &l30);
	result &= _mm_andnot_si128((l8 ^ c[5]), KCONST_1);
	result &= _mm_andnot_si128((l16 ^ c[3]), KCONST_1);
	result &= _mm_andnot_si128((l22 ^ c[51]), KCONST_1);
	result &= _mm_andnot_si128((l30 ^ c[49]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s2 (r3 ^ k[13], r4 ^ k[17], r5 ^ k[40], r6 ^ k[34], r7 ^ k[25],
		r8 ^ k[5], &l12, &l27, &l1, &l17);
	result &= _mm_andnot_si128((l12 ^ c[37]), KCONST_1);
	result &= _mm_andnot_si128((l27 ^ c[25]), KCONST_1);
	result &= _mm_andnot_si128((l1 ^ c[15]), KCONST_1);
	result &= _mm_andnot_si128((l17 ^ c[11]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s3 (r7 ^ k[39], r8 ^ k[11], r9 ^ k[19], r10 ^ k[20], r11 ^ k[3],
		r12 ^ k[48], &l23, &l15, &l29, &l5);
	result &= _mm_andnot_si128((l23 ^ c[59]), KCONST_1);
	result &= _mm_andnot_si128((l15 ^ c[61]), KCONST_1);
	result &= _mm_andnot_si128((l29 ^ c[41]), KCONST_1);
	result &= _mm_andnot_si128((l5 ^ c[47]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s4 (r11 ^ k[47], r12 ^ k[41], r13 ^ k[10], r14 ^ k[18], r15 ^ k[26],
		r16 ^ k[6], &l25, &l19, &l9, &l0);
	result &= _mm_andnot_si128((l25 ^ c[9]), KCONST_1);
	result &= _mm_andnot_si128((l19 ^ c[27]), KCONST_1);
	result &= _mm_andnot_si128((l9 ^ c[13]), KCONST_1);
	result &= _mm_andnot_si128((l0 ^ c[7]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s5 (r15 ^ k[22], r16 ^ k[44], r17 ^ k[7], r18 ^ k[49], r19 ^ k[9],
		r20 ^ k[38], &l7, &l13, &l24, &l2);
	result &= _mm_andnot_si128((l7 ^ c[63]), KCONST_1);
	result &= _mm_andnot_si128((l13 ^ c[45]), KCONST_1);
	result &= _mm_andnot_si128((l24 ^ c[1]), KCONST_1);
	result &= _mm_andnot_si128((l2 ^ c[23]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s6 (r19 ^ k[0], r20 ^ k[15], r21 ^ k[37], r22 ^ k[50], r23 ^ k[21],
		r24 ^ k[16], &l3, &l28, &l10, &l18);
	result &= _mm_andnot_si128((l3 ^ c[31]), KCONST_1);
	result &= _mm_andnot_si128((l28 ^ c[33]), KCONST_1);
	result &= _mm_andnot_si128((l10 ^ c[21]), KCONST_1);
	result &= _mm_andnot_si128((l18 ^ c[19]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s7 (r23 ^ k[43], r24 ^ k[23], r25 ^ k[8], r26 ^ k[45], r27 ^ k[28],
		r28 ^ k[51], &l31, &l11, &l21, &l6);
	result &= _mm_andnot_si128((l31 ^ c[57]), KCONST_1);
	result &= _mm_andnot_si128((l11 ^ c[29]), KCONST_1);
	result &= _mm_andnot_si128((l21 ^ c[43]), KCONST_1);
	result &= _mm_andnot_si128((l6 ^ c[55]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s8 (r27 ^ k[2], r28 ^ k[29], r29 ^ k[30], r30 ^ k[42], r31 ^ k[52],
		r0 ^ k[14], &l4, &l26, &l14, &l20);
	result &= _mm_andnot_si128((l4 ^ c[39]), KCONST_1);
	result &= _mm_andnot_si128((l26 ^ c[17]), KCONST_1);
	result &= _mm_andnot_si128((l14 ^ c[53]), KCONST_1);
	result &= _mm_andnot_si128((l20 ^ c[35]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s1 (l31 ^ k[40], l0 ^ k[4], l1 ^ k[19], l2 ^ k[53], l3 ^ k[6],
		l4 ^ k[34], &r8, &r16, &r22, &r30);
	result &= _mm_andnot_si128((r8 ^ c[4]), KCONST_1);
	result &= _mm_andnot_si128((r16 ^ c[2]), KCONST_1);
	result &= _mm_andnot_si128((r22 ^ c[50]), KCONST_1);
	result &= _mm_andnot_si128((r30 ^ c[48]), KCONST_1);
	////if (eq(result, KCONST_0))
	//    //return (result);
	s2 (l3 ^ k[20], l4 ^ k[24], l5 ^ k[47], l6 ^ k[41], l7 ^ k[32],
		l8 ^ k[12], &r12, &r27, &r1, &r17);
	result &= _mm_andnot_si128((r12 ^ c[36]), KCONST_1);
	result &= _mm_andnot_si128((r27 ^ c[24]), KCONST_1);
	result &= _mm_andnot_si128((r1 ^ c[14]), KCONST_1);
	result &= _mm_andnot_si128((r17 ^ c[10]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s3 (l7 ^ k[46], l8 ^ k[18], l9 ^ k[26], l10 ^ k[27], l11 ^ k[10],
		l12 ^ k[55], &r23, &r15, &r29, &r5);
	result &= _mm_andnot_si128((r23 ^ c[58]), KCONST_1);
	result &= _mm_andnot_si128((r15 ^ c[60]), KCONST_1);
	result &= _mm_andnot_si128((r29 ^ c[40]), KCONST_1);
	result &= _mm_andnot_si128((r5 ^ c[46]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s4 (l11 ^ k[54], l12 ^ k[48], l13 ^ k[17], l14 ^ k[25], l15 ^ k[33],
		l16 ^ k[13], &r25, &r19, &r9, &r0);
	result &= _mm_andnot_si128((r25 ^ c[8]), KCONST_1);
	result &= _mm_andnot_si128((r19 ^ c[26]), KCONST_1);
	result &= _mm_andnot_si128((r9 ^ c[12]), KCONST_1);
	result &= _mm_andnot_si128((r0 ^ c[6]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s5 (l15 ^ k[29], l16 ^ k[51], l17 ^ k[14], l18 ^ k[1], l19 ^ k[16],
		l20 ^ k[45], &r7, &r13, &r24, &r2);
	result &= _mm_andnot_si128((r7 ^ c[62]), KCONST_1);
	result &= _mm_andnot_si128((r13 ^ c[44]), KCONST_1);
	result &= _mm_andnot_si128((r24 ^ c[0]), KCONST_1);
	result &= _mm_andnot_si128((r2 ^ c[22]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s6 (l19 ^ k[7], l20 ^ k[22], l21 ^ k[44], l22 ^ k[2], l23 ^ k[28],
		l24 ^ k[23], &r3, &r28, &r10, &r18);
	result &= _mm_andnot_si128((r3 ^ c[30]), KCONST_1);
	result &= _mm_andnot_si128((r28 ^ c[32]), KCONST_1);
	result &= _mm_andnot_si128((r10 ^ c[20]), KCONST_1);
	result &= _mm_andnot_si128((r18 ^ c[18]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s7 (l23 ^ k[50], l24 ^ k[30], l25 ^ k[15], l26 ^ k[52], l27 ^ k[35],
		l28 ^ k[31], &r31, &r11, &r21, &r6);
	result &= _mm_andnot_si128((r31 ^ c[56]), KCONST_1);
	result &= _mm_andnot_si128((r11 ^ c[28]), KCONST_1);
	result &= _mm_andnot_si128((r21 ^ c[42]), KCONST_1);
	result &= _mm_andnot_si128((r6 ^ c[54]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	s8 (l27 ^ k[9], l28 ^ k[36], l29 ^ k[37], l30 ^ k[49], l31 ^ k[0],
		l0 ^ k[21], &r4, &r26, &r14, &r20);
	result &= _mm_andnot_si128((r4 ^ c[38]), KCONST_1);
	result &= _mm_andnot_si128((r26 ^ c[16]), KCONST_1);
	result &= _mm_andnot_si128((r14 ^ c[52]), KCONST_1);
	result &= _mm_andnot_si128((r20 ^ c[34]), KCONST_1);
	//if (eq(result, KCONST_0))
	    //return (result);
	return (result);
}
Esempio n. 13
0
int Scene::add_mesh(QString filename, int loadType, typeFuncOpenSave f, Viewer* viewer)
{
	int res = 0;

	m_loadType = loadType;

	PolyhedronPtr polyhedron_ptr(new Polyhedron());

	if (filename == EMPTY_MESH)
	{
		// nothing
	}
	else if (filename == INTERNAL_MESH)
	{
		Point3d p1( -0.5, -0.5, -0.5);
		Point3d q1( 0.5, -0.5, -0.5);
		Point3d r1( 0.0, -0.5, 0.5);
		Point3d s1( 0.0, 0.5, 0.0);

		Halfedge_handle h = polyhedron_ptr->make_tetrahedron(p1, q1, r1, s1);
		if (!polyhedron_ptr->is_tetrahedron(h))
			res = -3;
	}
	else
	{
		if (f != NULL)
			res = f(polyhedron_ptr, filename, viewer);
		else
		{
			QString ext = QFileInfo(filename).suffix();

			if (ext == "off")
				res = polyhedron_ptr->load_mesh_off(filename.toStdString());
			else if (ext == "obj")
				res = polyhedron_ptr->load_mesh_obj(filename.toStdString());
			else if (ext == "smf")
				res = polyhedron_ptr->load_mesh_smf(filename.toStdString());
			else if (ext == "ply")
				res = polyhedron_ptr->load_mesh_ply(filename.toStdString());
			else if (ext == "x3d")
				res = polyhedron_ptr->load_mesh_x3d(filename.toStdString());
			else
				res = 1;
		}
	}

	if (!res)
	{
		if (get_nb_polyhedrons()>0)
		{
			PolyhedronPtr p = get_polyhedron();
			//if (!p->empty())
			{
				viewer->camera()->setPosition(p->pInitialCameraPosition);
				viewer->camera()->setOrientation(p->pInitialCameraOrientation);
			}
		}

		if (!polyhedron_ptr->empty())
		{
			polyhedron_ptr->compute_bounding_box();

			polyhedron_ptr->compute_normals();
			polyhedron_ptr->compute_type();

			(void)polyhedron_ptr->calc_nb_components();
			(void)polyhedron_ptr->calc_nb_boundaries();
		}

		add_polyhedron(polyhedron_ptr);
		set_current_polyhedron(get_nb_polyhedrons()-1);

		setcurrentFile(filename);
		setVisible(true);

		// if mode Space
		todoIfModeSpace(viewer, viewer->getYStep());

		viewer->showAllScene();
	}

	return res;
}
int main()
{
long i;

JSubset s1(10);													// constructor

	cout << "subset s1 created" << endl << endl;

	cout << "s1 itemCount should be 0" << endl;
	cout << "s1 itemCount = " << s1.GetElementCount() << endl << endl;

	s1.AddRange(3,4);
	s1.Add(9);

	cout << "s1 itemCount should be 3" << endl;
	cout << "s1 itemCount = " << s1.GetElementCount() << endl << endl;

	cout << "s1 should contain:         10FFTTFFFFTF" << endl;
	cout << "s1 contains      : " << s1 << endl;
	cout << "using Contains() :           ";

	for (i=1; i<=10; i++)
		{
		cout << s1.Contains(i);
		}
	cout << endl;

JSubset s2 = s1;

	cout << endl;
	cout << "subset s2 created" << endl << endl;

	s2.Remove(1);

	cout << "s1 should equal s2" << endl;
	cout << "s1 equals s2? " << (s1 == s2) << endl;

	s2.Remove(4);
	s2.Add(2);

	cout << "s2 should contain:         10FTTFFFFFTF" << endl;
	cout << "s2 contains      : " << s2 << endl;

	cout << "s1 should not equal s2" << endl;
	cout << "s1 equals s2? " << (s1 == s2) << endl;

	JWaitForReturn();

JSubset s3 = s1 + s2;
JSubset s4 = s1 - s2;
JSubset s5 = s2 - s1;
JSubset s7 = JIntersection(s4, s5);

	cout << "s3 should contain:         10FTTTFFFFTF" << endl;
	cout << "s3 contains      : " << s3 << endl << endl;

	cout << "s4 should contain:         10FFFTFFFFFF" << endl;
	cout << "s4 contains      : " << s4 << endl << endl;

	cout << "s5 should contain:         10FTFFFFFFFF" << endl;
	cout << "s5 contains      : " << s5 << endl << endl;

	cout << "s7 should contain:         10FFFTFFFFFF" << endl;
	cout << "s7 contains      : " << s4 << endl << endl;

	JWaitForReturn();

	s3.RemoveAll();
	s3.AddRange(3,8);
	s3.RemoveRange(4,6);

	cout << "s3 should contain:         10FFTFFFTTFF" << endl;
	cout << "s3 contains      : " << s3 << endl << endl;

JSubsetIterator iter = s3.NewIterator();

	JIndex index;

	cout << "s3 contains: ";
	while (iter.Next(&index))
		{
		cout << ' ' << index;
		}
	cout << endl << endl;

JSubset s6 = s3.Complement();

	cout << "s6 should contain:         10TTFTTTFFTT" << endl;
	cout << "s6 contains      : " << s6 << endl << endl;

	s6 = s3;											// assignment operator

	cout << "s6 assigned from s3" << endl << endl;

	cout << "s6 itemCount should be 3" << endl;
	cout << "s6 itemCount=" << s6.GetElementCount() << endl << endl;

	cout << "s6 should contain:         10FFTFFFTTFF" << endl;
	cout << "s6 contains      : " << s6 << endl << endl;

	JWaitForReturn();

	cout << "Twenty random samples of size 3:" << endl << endl;

	for (i=1; i<=20; i++)
		{
		if (JGetRandomSample(&s1, 3))
			{
			assert( s1.GetElementCount() == 3 );
			cout << s1 << endl;
			}
		else
			{
			cerr << "Unable to generate random subset of that size." << endl;
			}
		}

// JGetRandomSample()

	JHistogram<JSize> h(10);
	for (i=1; i<=5000; i++)
		{
		JGetRandomSample(&s3, 3, 2,8);

		iter.MoveTo(kJIteratorStartAtBeginning, 0);
		while (iter.Next(&index))
			{
			h.IncrementCount(index, 1);
			}
		}

	JProbDistr p = h.ConvertToProbabilities();

	cout << endl;
	cout << "Probability of each element (2-8) being in the subset:" << endl;
	cout << "(averaged over ensemble of 5000 subsets)" << endl << endl;
	cout << p << endl;

// JSubset::GetRandomSample()

	JBoolean ok = JGetRandomSample(&s1, 7);
	assert( ok );

	h.Clear();
	for (i=1; i<=5000; i++)
		{
		s1.GetRandomSample(&s3, 3);

		iter.MoveTo(kJIteratorStartAtBeginning, 0);
		while (iter.Next(&index))
			{
			h.IncrementCount(index, 1);
			}
		}

	p = h.ConvertToProbabilities();

	cout << endl;
	cout << "Probability of each element (sample of 7) being in the subset:" << endl;
	cout << "(averaged over ensemble of 5000 subsets)" << endl << endl;
	cout << p << endl;

// JSubset::GetRandomDisjointSamples()

	JPtrArray<JSubset> sampleList(JPtrArrayT::kDeleteAll);

	JArray<JSize> sampleSizeList;
	sampleSizeList.AppendElement(2);
	sampleSizeList.AppendElement(2);

	JHistogram<JSize> h1(10), h2(10);
	JHistogram<JSize>* hist[] = { &h1, &h2 };
	for (i=1; i<=5000; i++)
		{
		s1.GetRandomDisjointSamples(&sampleList, sampleSizeList);

		for (JIndex j=1; j<=2; j++)
			{
			JSubsetIterator iter = (sampleList.NthElement(j))->NewIterator();
			while (iter.Next(&index))
				{
				hist[j-1]->IncrementCount(index, 1);
				assert( !(sampleList.NthElement(3-j))->Contains(index) );
				}
			}
		}

	cout << endl;
	cout << "Probability of each element (2 from sample of 7) being in the subset:" << endl;
	cout << "(averaged over ensemble of 5000 subsets)" << endl << endl;
	cout << "1) " << h1.ConvertToProbabilities() << endl;
	cout << "2) " << h2.ConvertToProbabilities() << endl;

// JSubset::GetRandomDisjointSamples() -- partitioning

	sampleSizeList.SetElement(1, 3);
	sampleSizeList.SetElement(2, 4);

	h1.Clear();
	h2.Clear();
	for (i=1; i<=5000; i++)
		{
		s1.GetRandomDisjointSamples(&sampleList, sampleSizeList);

		for (JIndex j=1; j<=2; j++)
			{
			JSubsetIterator iter = (sampleList.NthElement(j))->NewIterator();
			while (iter.Next(&index))
				{
				hist[j-1]->IncrementCount(index, 1);
				assert( !(sampleList.NthElement(3-j))->Contains(index) );
				}
			}
		}
	sampleList.DeleteAll();

	cout << endl;
	cout << "Probability of each element (3,4 from sample of 7) being in the subset:" << endl;
	cout << "(averaged over ensemble of 5000 subsets)" << endl << endl;
	cout << "1) " << h1.ConvertToProbabilities() << endl;
	cout << "2) " << h2.ConvertToProbabilities() << endl;

	return 0;
}
Esempio n. 15
0
screen_1::screen_1(void)
{
    // Loads in spritesheet
    texture1.loadFromFile( resourcePath() + "spritesheet.png");
    texture2.loadFromFile( resourcePath() + "vehicles.png");
    texture3.loadFromFile( resourcePath() + "timer.png" );
    hearts.loadFromFile( resourcePath() + "hearts.png");
    
    // *******************************************************************
    // Sets texture to sprites, scale, and pos
    //
    // Create(sf::Sprite &sprite, sf::IntRect &rect, sf::Texture &texture, int left, int top, int width,
    //          int height, double sizeX, double sizeY, double posX, double posY)
    // *******************************************************************
    Create t1(truck, rectSourceSprite, texture2, 0, 24, 130, 73, 1.5, 1.5, 130, 420);
    Create t2(truck2, rectSourceSprite, texture2, 148, 0, 120, 159, 1.5, 1.5, 550, 290);
    Create c1(car, rectSourceSprite, texture2, 310, 0, 146, 149, 1.7, 1.7, -230, 420);
    Create c2(car2, rectSourceSprite, texture2, 460, 0, 146, 149, 1.7, 1.7, 280, 289);
    Create c3(car3, rectSourceSprite, texture2, 460, 0, 146, 149, 1.7, 1.7, 0, 289);
    Create s1(shortLog, rectSourceSprite, texture1, 289, 0, 146, 196, 1, 1, 0, 129);
    Create s2(shortLog2, rectSourceSprite, texture1, 435, 0, 141, 196, 1, 1, 200, 65);
    Create s3(shortLog3, rectSourceSprite, texture1, 289, 0, 146, 196, 1, 1, 300, 8);
    Create l1(longLog, rectSourceSprite, texture1, 577, 0, 205, 196, 1, 1, 799, 8.3);
    Create l2(longLog2, rectSourceSprite, texture1, 577, 0, 205, 196, 1, 1, 500, 138);
    Create leaf(lillypad, rectSourceSprite, texture2, 610, 0, 115, 196, 1, 1, 0, -32);
    Create leaf2(lillypad2, rectSourceSprite, texture2, 610, 0, 115, 196, 1, 1, 167.5, -32);
    Create leaf3(lillypad3, rectSourceSprite, texture2, 610, 0, 115, 196, 1, 1, 335, -32);
    Create leaf4(lillypad4, rectSourceSprite, texture2, 610, 0, 115, 196, 1, 1, 502.5, -32);
    Create leaf5(lillypad5, rectSourceSprite, texture2, 610, 0, 115, 196, 1, 1, 670, -32);

    Create hearts123(life, rectSource, hearts, 0, 0, 116, 30, 1, 1, 10, 600);
    
    //Player ( aka stats )
    mainPlayer.setNumLives(3);
    mainPlayer.setIsHit(false);
    
    // Frogger Sprite
    Create grogger(frogger, rectSourceSprite, texture1, 0, 70, 75, 70, 1, 1, 320, 605);

    // Sets texture to frogs that will go on occupied lilypads
    occupied1.setTexture(texture1);
    occupied1.setTextureRect(rectSourceSprite);
    occupied2.setTexture(texture1);
    occupied2.setTextureRect(rectSourceSprite);
    occupied3.setTexture(texture1);
    occupied3.setTextureRect(rectSourceSprite);
    occupied4.setTexture(texture1);
    occupied4.setTextureRect(rectSourceSprite);
    occupied5.setTexture(texture1);
    occupied5.setTextureRect(rectSourceSprite);
    
    // Sets pos of above frogs
    occupied1.setPosition(-100, -100);
    occupied2.setPosition(-100, -100);
    occupied3.setPosition(-100, -100);
    occupied4.setPosition(-100, -100);
    occupied5.setPosition(-100, -100);
    death.setPosition(-100, -100);
    
    // Timer Rect
    timeRect.left = 297;
    timeRect.top = 43;
    timeRect.width = 143;
    timeRect.height = 14;
    timer.setScale(1.5, 1.5);
    timer.setTexture(texture3, true);
    timer.setTextureRect(timeRect);
    timer.setPosition(10, 648);
    
    // Loads Background Pic
    bg.loadFromFile( resourcePath() + "background.jpg" );
    background.setTexture(bg, true);
    
    // Log Checking
    isOnLog = false;
    isOnLog2 = false;
    isOnLog3 = false;
    isOnLog4 = false;
    isOnLog5 = false;
    
    // Lily Checking
    isOnLily = false;
    isOnLily2 = false;
    isOnLily3 = false;
    isOnLily4 = false;
    isOnLily5 = false;
    
    froggerLanded = false;
    
    // Frogger Theme
    if (!froggerTheme.openFromFile(resourcePath() + "froggerLeche_Remix.ogg"))
        return -1; // error
    froggerTheme.setLoop(true);
    
    // Hop Sound
    if (!hopFile.loadFromFile(resourcePath() + "froggerHop.ogg"))
        return -1;
    
    // Coin Sound
    if (!intro.openFromFile(resourcePath() + "froggerCoin.ogg"))
        return -1;
    
    // Trucked
    if (!truckedFile.loadFromFile(resourcePath() + "froggerTrucked.ogg"))
        return -1;
    
    // Dunked
    if (!dunkedFile.loadFromFile(resourcePath() + "froggerDunked.ogg"))
        return -1;
}
Esempio n. 16
0
int main()
{
  {
    boost::synchronized_value<int> v1;
    *v1=42;
    std::cout<<"v1="<<*v1<<std::endl;
    f(v1);
    int i=*v1;
    std::cout<<"i="<<i<<std::endl;

    {
      boost::strict_lock_ptr<int> u=v1.synchronize();

      *u+=43;
      std::cout<<"v1="<<*u<<std::endl;
      g(u);
    }
    boost::synchronized_value<int> v2(2);
    std::cout<<"v2="<<*v2<<std::endl;
    v2 = 3;
    std::cout<<"v2="<<*v2<<std::endl;

    boost::synchronized_value<int> v3(v2);
    std::cout<<"v3="<<*v3<<std::endl;
    v3 = v1;
    std::cout<<"v3="<<*v3<<std::endl;

    std::cout<<"v2="<<*v3<<std::endl;
    std::cout<<"v3="<<*v3<<std::endl;
    swap(v3,v2);
    v1.swap(v2);
    std::cout<<"v3="<<*v3<<std::endl;
  }
  {
    boost::synchronized_value<std::string> s;
    addTrailingSlashIfMissing(s);
    std::cout<<"s="<<std::string(*s)<<std::endl;
  }
  {
    boost::synchronized_value<std::string> s;
    s->append("foo/");
    s.synchronize()->append("foo");
    addTrailingSlashIfMissing(s);
    std::cout<<"s="<<std::string(*s)<<std::endl;
  }
  {
    boost::synchronized_value<std::string> s;
    s = std::string("foo/");
    std::cout<<"ss="<< s << std::endl;
  }
  {
    boost::synchronized_value<std::string> s;
    s = "foo/";
    std::cout<<"ss="<< s << std::endl;
  }
  {
    boost::synchronized_value<std::string> s1("a");
    boost::synchronized_value<std::string> s2;
    s2=s1;
    std::cout<<"s1="<< s1 << std::endl;
    std::cout<<"s2="<< s2 << std::endl;
  }
  {
    boost::synchronized_value<std::string> s1("a");
    boost::synchronized_value<std::string> s2("b");
    std::cout<<"s1="<< s1 << std::endl;
    std::cout<<"s2="<< s2 << std::endl;
    swap(s1,s2);
    std::cout<<"s1="<< s1 << std::endl;
    std::cout<<"s2="<< s2 << std::endl;
  }
#if ! defined(BOOST_NO_CXX11_EXPLICIT_CONVERSION_OPERATORS)
  {
    boost::synchronized_value<std::string> sts("a");
    std::string s(sts);
    std::cout<<"ssts="<< s << std::endl;
  }
#endif
  {
    boost::synchronized_value<int> s1(1);
    boost::synchronized_value<int> s2(1);
    BOOST_ASSERT(s1==s2);
    BOOST_ASSERT(s1<=s2);
    BOOST_ASSERT(s1>=s2);
    BOOST_ASSERT(s1==1);
    BOOST_ASSERT(s1<=1);
    BOOST_ASSERT(s1>=1);
  }
  {
    boost::synchronized_value<int> s1(1);
    boost::synchronized_value<int> s2(2);
    BOOST_ASSERT(s1!=s2);
    BOOST_ASSERT(s1!=2);
    BOOST_ASSERT(2!=s1);
  }
  {
    boost::synchronized_value<int> s1(1);
    boost::synchronized_value<int> s2(2);
    BOOST_ASSERT(s1<s2);
    BOOST_ASSERT(s1<=s2);
    BOOST_ASSERT(s2>s1);
    BOOST_ASSERT(s2>=s1);
    BOOST_ASSERT(s1<2);
    BOOST_ASSERT(s1<=2);
    BOOST_ASSERT(s2>1);
    BOOST_ASSERT(s2>=1);
  }
  return 0;
}
int main(int argc, char **argv)
{
 PetscInitialize(&argc,&argv,PETSC_NULL,PETSC_NULL);

 double Re = 100;
 double Sc = 1;
 double We = 1;
 double Fr = 1;
 double c1 = 0.0;  // lagrangian
 double c2 = 0.0;  // smooth vel
 double c3 = 0.0;  // smooth coord (fujiwara)
 double d1 = 1.0;  // surface tangent velocity u_n=u-u_t 
 double d2 = 0.0;  // surface smooth cord (fujiwara)
 double alpha = 1;

 double mu_in = 1;
 double mu_out = 0.001;

 double rho_in = 1; 
 double rho_out = 0.01;

 double cfl = 0.8;

 Solver *solverP = new PetscSolver(KSPGMRES,PCILU);
 Solver *solverV = new PetscSolver(KSPCG,PCICC);
 Solver *solverC = new PetscSolver(KSPCG,PCICC);

 const char *vtkFolder  = "./vtk/";
 const char *mshFolder  = "./msh/";
 const char *datFolder  = "./dat/";


 /* meshes */
 vector<const char*> mesh;
 mesh.resize(7);
 mesh[0]  = "../../db/gmsh/3d/hyperboloid/curvature/0.10.msh";
 mesh[1]  = "../../db/gmsh/3d/hyperboloid/curvature/0.09.msh";
 mesh[2]  = "../../db/gmsh/3d/hyperboloid/curvature/0.08.msh";
 mesh[3]  = "../../db/gmsh/3d/hyperboloid/curvature/0.07.msh";
 mesh[4]  = "../../db/gmsh/3d/hyperboloid/curvature/0.06.msh";
 mesh[5]  = "../../db/gmsh/3d/hyperboloid/curvature/0.05.msh";
 mesh[6]  = "../../db/gmsh/3d/hyperboloid/curvature/0.04.msh";

 for( int i=0;i<(int) mesh.size();i++ )
 {
  cout << color(none,magenta,black);
  cout << "____________________________________ Iteration: " 
       << i << endl << endl;
  cout << resetColor();

  Model3D m1;
  Simulator3D s1;

  m1.readMSH(mesh[i]);
  m1.setInterfaceBC();
  m1.setTriEdge();
  m1.mesh2Dto3D();
  m1.setMapping();
#if NUMGLEU == 5
 m1.setMiniElement();
#else
 m1.setQuadElement();
#endif
  m1.setSurfaceConfig();
  m1.setInitSurfaceVolume();
  m1.setInitSurfaceArea();
  m1.setGenericBC();

  s1(m1);

  s1.setRe(Re);
  s1.setSc(Sc);
  s1.setWe(We);
  s1.setFr(Fr);
  s1.setC1(c1);
  s1.setC2(c2);
  s1.setC3(c3);
  s1.setD1(d1);
  s1.setD2(d2);
  s1.setAlpha(alpha);
  s1.setMu(mu_in,mu_out);
  s1.setRho(rho_in,rho_out);
  s1.setCfl(cfl);
  s1.init();
  s1.setSolverPressure(solverP);
  s1.setSolverVelocity(solverV);
  s1.setSolverConcentration(solverC);

  //s1.stepLagrangian();
  s1.stepALE();
  s1.setDtALETwoPhase();
  s1.movePoints();
  s1.assemble();
  s1.matMount();
  s1.setUnCoupledBC();
  s1.setRHS();
  //s1.setInterface();
  s1.setInterfaceGeo();
  s1.unCoupled();

  InOut save(m1,s1); // cria objeto de gravacao
  save.saveMSH(mshFolder,"newMesh",i);
  save.saveVTK(vtkFolder,"sim",i);
  save.saveVTKSurface(vtkFolder,"sim",i);
  save.saveKappaErrorHyperboloid(datFolder);
  save.saveBubbleInfo(datFolder);
  save.chordalPressure(datFolder,"chordalPressure",i);
  save.crossSectionalPlane(datFolder,"XZ",i);

  cout << color(none,magenta,black);
  cout << "________________________________________ END of " 
       << i << endl << endl;;
  cout << resetColor();
 }

 PetscFinalize();
 return 0;
}
Esempio n. 18
0
int main(int argc, char* argv[])
{
  // Load the mesh.
  Mesh mesh;
  H2DReader mloader;
  mloader.load("ffs.mesh", &mesh);

  // Perform initial mesh refinements.
  for (int i = 0; i < INIT_REF_NUM; i++) mesh.refine_all_elements();

  // Boundary condition types;
  BCTypes bc_types;

  // Initialize boundary condition types and spaces with default shapesets.
  bc_types.add_bc_neumann(Hermes::vector<int>(BDY_SOLID_WALL, BDY_INLET_OUTLET));
  L2Space space_rho(&mesh, &bc_types, P_INIT);
  L2Space space_rho_v_x(&mesh, &bc_types, P_INIT);
  L2Space space_rho_v_y(&mesh, &bc_types, P_INIT);
  L2Space space_e(&mesh, &bc_types, P_INIT);

  // Initialize solutions, set initial conditions.
  Solution sln_rho, sln_rho_v_x, sln_rho_v_y, sln_e, prev_rho, prev_rho_v_x, prev_rho_v_y, prev_e;
  sln_rho.set_exact(&mesh, ic_density);
  sln_rho_v_x.set_exact(&mesh, ic_density_vel_x);
  sln_rho_v_y.set_exact(&mesh, ic_density_vel_y);
  sln_e.set_exact(&mesh, ic_energy);
  prev_rho.set_exact(&mesh, ic_density);
  prev_rho_v_x.set_exact(&mesh, ic_density_vel_x);
  prev_rho_v_y.set_exact(&mesh, ic_density_vel_y);
  prev_e.set_exact(&mesh, ic_energy);

  // Initialize weak formulation.
  WeakForm wf(4);

  // Bilinear forms coming from time discretization by explicit Euler's method.
  wf.add_matrix_form(0, 0, callback(bilinear_form_0_0_time));
  wf.add_matrix_form(1, 1, callback(bilinear_form_1_1_time));
  wf.add_matrix_form(2, 2, callback(bilinear_form_2_2_time));
  wf.add_matrix_form(3, 3, callback(bilinear_form_3_3_time));

  // Volumetric linear forms.
  // Linear forms coming from the linearization by taking the Eulerian fluxes' Jacobian matrices 
  // from the previous time step.
  // First flux.
  // Unnecessary for FVM.
  if(P_INIT.order_h > 0 || P_INIT.order_v > 0) {
    wf.add_vector_form(0, callback(linear_form_0_1), HERMES_ANY, Hermes::vector<MeshFunction*>(&prev_rho_v_x));

  wf.add_vector_form(1, callback(linear_form_1_0_first_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y));
  wf.add_vector_form(1, callback(linear_form_1_1_first_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y));
  wf.add_vector_form(1, callback(linear_form_1_2_first_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y));
  wf.add_vector_form(1, callback(linear_form_1_3_first_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form(2, callback(linear_form_2_0_first_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y));
  wf.add_vector_form(2, callback(linear_form_2_1_first_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y));
  wf.add_vector_form(2, callback(linear_form_2_2_first_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y));
  wf.add_vector_form(2, callback(linear_form_2_3_first_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form(3, callback(linear_form_3_0_first_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form(3, callback(linear_form_3_1_first_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form(3, callback(linear_form_3_2_first_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form(3, callback(linear_form_3_3_first_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  // Second flux.
  
    wf.add_vector_form(0, callback(linear_form_0_2), HERMES_ANY, Hermes::vector<MeshFunction*>(&prev_rho_v_y));
  wf.add_vector_form(1, callback(linear_form_1_0_second_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y));
  wf.add_vector_form(1, callback(linear_form_1_1_second_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y));
  wf.add_vector_form(1, callback(linear_form_1_2_second_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y));
  wf.add_vector_form(1, callback(linear_form_1_3_second_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form(2, callback(linear_form_2_0_second_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y));
  wf.add_vector_form(2, callback(linear_form_2_1_second_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y));
  wf.add_vector_form(2, callback(linear_form_2_2_second_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y));
  wf.add_vector_form(2, callback(linear_form_2_3_second_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form(3, callback(linear_form_3_0_second_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form(3, callback(linear_form_3_1_second_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form(3, callback(linear_form_3_2_second_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form(3, callback(linear_form_3_3_second_flux), HERMES_ANY, 
                       Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  }

  wf.add_vector_form(0, linear_form, linear_form_order, HERMES_ANY, &prev_rho);
  wf.add_vector_form(1, linear_form, linear_form_order, HERMES_ANY, &prev_rho_v_x);
  wf.add_vector_form(2, linear_form, linear_form_order, HERMES_ANY, &prev_rho_v_y);
  wf.add_vector_form(3, linear_form, linear_form_order, HERMES_ANY, &prev_e);


  // Surface linear forms - inner edges coming from the DG formulation.
  wf.add_vector_form_surf(0, linear_form_interface_0, linear_form_order, H2D_DG_INNER_EDGE, 
                          Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form_surf(1, linear_form_interface_1, linear_form_order, H2D_DG_INNER_EDGE, 
                          Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form_surf(2, linear_form_interface_2, linear_form_order, H2D_DG_INNER_EDGE, 
                          Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form_surf(3, linear_form_interface_3, linear_form_order, H2D_DG_INNER_EDGE, 
                          Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));


  // Surface linear forms - inlet / outlet edges.
  wf.add_vector_form_surf(0, bdy_flux_inlet_outlet_comp_0, linear_form_order, BDY_INLET_OUTLET, 
                          Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form_surf(1, bdy_flux_inlet_outlet_comp_1, linear_form_order, BDY_INLET_OUTLET, 
                          Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form_surf(2, bdy_flux_inlet_outlet_comp_2, linear_form_order, BDY_INLET_OUTLET, 
                          Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form_surf(3, bdy_flux_inlet_outlet_comp_3, linear_form_order, BDY_INLET_OUTLET, 
                          Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));


  // Surface linear forms - Solid wall edges.
  wf.add_vector_form_surf(0, bdy_flux_solid_wall_comp_0, linear_form_order, BDY_SOLID_WALL, 
                          Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form_surf(1, bdy_flux_solid_wall_comp_1, linear_form_order, BDY_SOLID_WALL, 
                          Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form_surf(2, bdy_flux_solid_wall_comp_2, linear_form_order, BDY_SOLID_WALL, 
                          Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
  wf.add_vector_form_surf(3, bdy_flux_solid_wall_comp_3, linear_form_order, BDY_SOLID_WALL, 
                          Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));

  // Initialize the FE problem.
  bool is_linear = true;
  
  DiscreteProblem dp(&wf, Hermes::vector<Space*>(&space_rho, &space_rho_v_x, &space_rho_v_y, &space_e), is_linear);
  
  // If the FE problem is in fact a FV problem.
  if(P_INIT.order_h == 0 && P_INIT.order_v == 0)
  dp.set_fvm();

  // Filters for visualization of pressure and the two components of velocity.
  SimpleFilter pressure(calc_pressure_func, Hermes::vector<MeshFunction*>(&sln_rho, &sln_rho_v_x, &sln_rho_v_y, &sln_e));
  SimpleFilter u(calc_u_func, Hermes::vector<MeshFunction*>(&sln_rho, &sln_rho_v_x, &sln_rho_v_y, &sln_e));
  SimpleFilter w(calc_w_func, Hermes::vector<MeshFunction*>(&sln_rho, &sln_rho_v_x, &sln_rho_v_y, &sln_e));

  //VectorView vview("Velocity", new WinGeom(0, 0, 600, 300));
  //ScalarView sview("Pressure", new WinGeom(700, 0, 600, 300));

  ScalarView s1("w1", new WinGeom(0, 0, 620, 300));
  s1.fix_scale_width(80);
  ScalarView s2("w2", new WinGeom(625, 0, 600, 300));
  s2.fix_scale_width(50);
  ScalarView s3("w3", new WinGeom(0, 350, 620, 300));
  s3.fix_scale_width(80);
  ScalarView s4("w4", new WinGeom(625, 350, 600, 300));
  s4.fix_scale_width(50);

  // Iteration number.
  int iteration = 0;
  
  // Set up the solver, matrix, and rhs according to the solver selection.
  SparseMatrix* matrix = create_matrix(matrix_solver);
  Vector* rhs = create_vector(matrix_solver);
  Solver* solver = create_linear_solver(matrix_solver, matrix, rhs);

  // Output of the approximate time derivative.
  std::ofstream time_der_out("time_der");

  for(t = 0.0; t < 10; t += TAU)
  {
    info("---- Time step %d, time %3.5f.", iteration, t);

    iteration++;

    bool rhs_only = (iteration == 1 ? false : true);
    // Assemble stiffness matrix and rhs or just rhs.
    if (rhs_only == false) info("Assembling the stiffness matrix and right-hand side vector.");
    else info("Assembling the right-hand side vector (only).");
    dp.assemble(matrix, rhs, rhs_only);

        
    // Solve the matrix problem.
    info("Solving the matrix problem.");
    if(solver->solve())
      Solution::vector_to_solutions(solver->get_solution(), Hermes::vector<Space *>(&space_rho, &space_rho_v_x, 
      &space_rho_v_y, &space_e), Hermes::vector<Solution *>(&sln_rho, &sln_rho_v_x, &sln_rho_v_y, &sln_e));
    else
    error ("Matrix solver failed.\n");

    // Approximate the time derivative of the solution.
    if(CALC_TIME_DER) {
      Adapt *adapt_for_time_der_calc = new Adapt(Hermes::vector<Space *>(&space_rho, &space_rho_v_x, 
        &space_rho_v_y, &space_e));
      bool solutions_for_adapt = false;
      double difference = 
        adapt_for_time_der_calc->calc_err_est(Hermes::vector<Solution *>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e), 
                                              Hermes::vector<Solution *>(&sln_rho, &sln_rho_v_x, &sln_rho_v_y, &sln_e), 
                                              (Hermes::vector<double>*) NULL, solutions_for_adapt, 
                                              HERMES_TOTAL_ERROR_ABS | HERMES_ELEMENT_ERROR_ABS) / TAU;
      delete adapt_for_time_der_calc;

    // Info about the approximate time derivative.
    if(iteration > 1)
    {
      info("Approximate the   norm time derivative : %g.", difference);
      time_der_out << iteration << '\t' << difference << std::endl;
    }
    }
    
    // Determine the time step according to the CFL condition.
    // Only mean values on an element of each solution component are taken into account.
    double *solution_vector = solver->get_solution();
    double min_condition = 0;
    Element *e;
    for (int _id = 0, _max = mesh.get_max_element_id(); _id < _max; _id++) \
          if (((e) = mesh.get_element_fast(_id))->used) \
            if ((e)->active)
    {
      AsmList al;
      space_rho.get_element_assembly_list(e, &al);
      double rho = solution_vector[al.dof[0]];
      space_rho_v_x.get_element_assembly_list(e, &al);
      double v1 = solution_vector[al.dof[0]] / rho;
      space_rho_v_y.get_element_assembly_list(e, &al);
      double v2 = solution_vector[al.dof[0]] / rho;
      space_e.get_element_assembly_list(e, &al);
      double energy = solution_vector[al.dof[0]];
      
      double condition = e->get_area() / (std::sqrt(v1*v1 + v2*v2) + calc_sound_speed(rho, rho*v1, rho*v2, energy));
      
      if(condition < min_condition || min_condition == 0.)
        min_condition = condition;
    }
    if(TAU > min_condition)
      TAU = min_condition;
    if(TAU < min_condition * 0.9)
      TAU = min_condition;

    // Copy the solutions into the previous time level ones.
    prev_rho.copy(&sln_rho);
    prev_rho_v_x.copy(&sln_rho_v_x);
    prev_rho_v_y.copy(&sln_rho_v_y);
    prev_e.copy(&sln_e);

    // Visualization.
    /*
    pressure.reinit();
    u.reinit();
    w.reinit();
    sview.show(&pressure);
    vview.show(&u, &w);
    */

    s1.show(&sln_rho);
    s2.show(&sln_rho_v_x);
    s3.show(&sln_rho_v_y);
    s4.show(&sln_e);
  }
  
  s1.close();
  s2.close();
  s3.close();
  s4.close();

  time_der_out.close();
  return 0;
}
Esempio n. 19
0
    static inline void apply(RingInput const& ring, RingOutput& buffered,
            coordinate_type distance,
            JoinStrategy const& join_strategy
#ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
            , Mapper& mapper
#endif
            )
    {
        typedef model::referring_segment<output_point_type const> segment_type;
        typedef typename boost::range_iterator
            <
                RingInput const
            >::type iterator_type;

        output_point_type previous_p1, previous_p2;
        output_point_type first_p1, first_p2;
        bool first = true;

#ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
        int index = 0;
#endif

        iterator_type it = boost::begin(ring);
        for (iterator_type prev = it++;
            it != boost::end(ring); ++it)
        {
            if (! detail::equals::equals_point_point(*prev, *it))
            {
                bool skip = false;

                // Generate a block along (int most cases to the left of) the segment

                // Simulate a vector d (dx,dy)
                coordinate_type dx = get<0>(*it) - get<0>(*prev);
                coordinate_type dy = get<1>(*it) - get<1>(*prev);


                // For normalization [0,1] (=dot product d.d, sqrt)
                coordinate_type length = sqrt(dx * dx + dy * dy);

                // Because coordinates are not equal, length should not be zero
                BOOST_ASSERT((! geometry::math::equals(length, 0)));

                // Generate the normalized perpendicular p, to the left (ccw)
                coordinate_type px = -dy / length;
                coordinate_type py = dx / length;

                output_point_type p1, p2;

                coordinate_type d = distance;

                set<0>(p2, get<0>(*it) + px * d);
                set<1>(p2, get<1>(*it) + py * d);

                set<0>(p1, get<0>(*prev) + px * d);
                set<1>(p1, get<1>(*prev) + py * d);

                {
                    RingOutput block;
                    block.push_back(*prev);
                    block.push_back(*it);
                    block.push_back(p2);
                    block.push_back(p1);
                    block.push_back(*prev);

    #ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
                    mapper.map(block, "opacity:0.4;fill:rgb(255,128,0);stroke:rgb(0,0,0);stroke-width:1");
    #endif
                }

                if (! first)
                {
                    output_point_type p;
                    segment_type s1(p1, p2);
                    segment_type s2(previous_p1, previous_p2);
                    if (line_line_intersection<output_point_type, segment_type>::apply(s1, s2, p))
                    {
                        join_strategy.apply(p, *prev, previous_p2, p1, distance, buffered);
                        {
#ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
                            mapper.map(p, "fill:rgb(0,0,0);", 3);

                            std::ostringstream out;
                            out << index++;
                            mapper.text(p, out.str(), "fill:rgb(0,0,0);font-family='Arial';", 5, 5);
#endif
                        }
                    }
                    else
                    {
                        skip = false;
                    }
                }
                else
                {
                    first = false;
                    first_p1 = p1;
                    first_p2 = p2;
                }

                if (! skip)
                {
                    previous_p1 = p1;
                    previous_p2 = p2;
                    prev = it;
                }
            }
        }

        // Last one
        {
            output_point_type p;
            segment_type s1(previous_p1, previous_p2);
            segment_type s2(first_p1, first_p2);
            line_line_intersection<output_point_type, segment_type>::apply(s1, s2, p);

            join_strategy.apply(p, *boost::begin(ring), previous_p2, first_p1, distance, buffered);

#ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
            mapper.map(p, "fill:rgb(0,0,0);", 3);
            std::ostringstream out;
            out << index++;
            mapper.text(p, out.str(), "fill:rgb(0,0,0);font-family='Arial';", 5, 5);
#endif
        }

        // Close the generated buffer
        {
            output_point_type p = *boost::begin(buffered);
            buffered.push_back(p);
        }
    }
Esempio n. 20
0
QString QgsStringUtils::longestCommonSubstring( const QString& string1, const QString& string2, bool caseSensitive )
{
  if ( string1.isEmpty() || string2.isEmpty() )
  {
    //empty strings, solution is trivial...
    return QString();
  }

  //handle case sensitive flag (or not)
  QString s1( caseSensitive ? string1 : string1.toLower() );
  QString s2( caseSensitive ? string2 : string2.toLower() );

  if ( s1 == s2 )
  {
    //another trivial case, identical strings
    return s1;
  }

  int* currentScores = new int [ s2.length()];
  int* previousScores = new int [ s2.length()];
  int maxCommonLength = 0;
  int lastMaxBeginIndex = 0;

  const QChar* s1Char = s1.constData();
  const QChar* s2Char = s2.constData();
  const QChar* s2Start = s2Char;

  for ( int i = 0; i < s1.length(); ++i )
  {
    for ( int j = 0; j < s2.length(); ++j )
    {
      if ( *s1Char != *s2Char )
      {
        currentScores[j] = 0;
      }
      else
      {
        if ( i == 0 || j == 0 )
        {
          currentScores[j] = 1;
        }
        else
        {
          currentScores[j] = 1 + previousScores[j - 1];
        }

        if ( maxCommonLength < currentScores[j] )
        {
          maxCommonLength = currentScores[j];
          lastMaxBeginIndex = i;
        }
      }
      s2Char++;
    }
    std::swap( currentScores, previousScores );
    s1Char++;
    s2Char = s2Start;
  }
  delete [] currentScores;
  delete [] previousScores;
  return string1.mid( lastMaxBeginIndex - maxCommonLength + 1, maxCommonLength );
}
Esempio n. 21
0
    string AddBinary(const string &a, const string &b) {
        // Start typing your C/C++ solution below
        // DO NOT write int main() function

        string s1(a), s2(b);
        std::reverse(s1.begin(), s1.end());
        std::reverse(s2.begin(), s2.end());

        char t = '0';
        string r;
        for (size_t i = 0; i < std::min(s1.size(), s2.size()); ++i)
        {
            assert(s1[i] == '0' || s1[i] == '1');
            assert(s2[i] == '0' || s2[i] == '1');

            const size_t sum = s1[i] - 48 + s2[i] - 48 + t - 48;
            switch (sum)
            {
            case 0:
                r += "0";
                t = '0';
                break;
            case 1:
                r += "1";
                t = '0';
                break;
            case 2:
                r += "0";
                t = '1';
                break;
            case 3:
                r += "1";
                t = '1';
                break;
            } 
        }

        for (size_t i = std::min(s1.size(), s2.size()); i < s1.size(); ++i)
        {
            assert(s1[i] == '0' || s1[i] == '1');

            const size_t sum = s1[i] - 48 + t - 48;
            switch (sum)
            {
            case 0:
                r += "0";
                t = '0';
                break;
            case 1:
                r += "1";
                t = '0';
                break;
            case 2:
                r += "0";
                t = '1';
                break;
            } 
        }

        for (size_t i = std::min(s1.size(), s2.size()); i < s2.size(); ++i)
        {
            assert(s2[i] == '0' || s2[i] == '1');

            const size_t sum = s2[i] - 48 + t - 48;
            switch (sum)
            {
            case 0:
                r += "0";
                t = '0';
                break;
            case 1:
                r += "1";
                t = '0';
                break;
            case 2:
                r += "0";
                t = '1';
                break;
            } 
        }

        r += t == '1' ? "1" : "";

        std::reverse(r.begin(), r.end());
        return r;
    }
Esempio n. 22
0
int QgsStringUtils::levenshteinDistance( const QString& string1, const QString& string2, bool caseSensitive )
{
  int length1 = string1.length();
  int length2 = string2.length();

  //empty strings? solution is trivial...
  if ( string1.isEmpty() )
  {
    return length2;
  }
  else if ( string2.isEmpty() )
  {
    return length1;
  }

  //handle case sensitive flag (or not)
  QString s1( caseSensitive ? string1 : string1.toLower() );
  QString s2( caseSensitive ? string2 : string2.toLower() );

  const QChar* s1Char = s1.constData();
  const QChar* s2Char = s2.constData();

  //strip out any common prefix
  int commonPrefixLen = 0;
  while ( length1 > 0 && length2 > 0 && *s1Char == *s2Char )
  {
    commonPrefixLen++;
    length1--;
    length2--;
    s1Char++;
    s2Char++;
  }

  //strip out any common suffix
  while ( length1 > 0 && length2 > 0 && s1.at( commonPrefixLen + length1 - 1 ) == s2.at( commonPrefixLen + length2 - 1 ) )
  {
    length1--;
    length2--;
  }

  //fully checked either string? if so, the answer is easy...
  if ( length1 == 0 )
  {
    return length2;
  }
  else if ( length2 == 0 )
  {
    return length1;
  }

  //ensure the inner loop is longer
  if ( length1 > length2 )
  {
    std::swap( s1, s2 );
    std::swap( length1, length2 );
  }

  //levenshtein algorithm begins here
  QVector< int > col;
  col.fill( 0, length2 + 1 );
  QVector< int > prevCol;
  prevCol.reserve( length2 + 1 );
  for ( int i = 0; i < length2 + 1; ++i )
  {
    prevCol << i;
  }
  const QChar* s2start = s2Char;
  for ( int i = 0; i < length1; ++i )
  {
    col[0] = i + 1;
    s2Char = s2start;
    for ( int j = 0; j < length2; ++j )
    {
      col[j + 1] = qMin( qMin( 1 + col[j], 1 + prevCol[1 + j] ), prevCol[j] + (( *s1Char == *s2Char ) ? 0 : 1 ) );
      s2Char++;
    }
    col.swap( prevCol );
    s1Char++;
  }
  return prevCol[length2];
}
Esempio n. 23
0
void DateTimeTextMatchPlugin::doGetPossibleMatches( const QString& text )
{
    //
    // check for years (numbers between 1900 and 2020 for example)
    // check for stuff like "June 22" or even "June 22-26th" (the latter can be used as start and end time already)
    // check for "2 o'clock"
    // check for "14:23"
    // check for 12.6.2009 and 6/12/2009 and 6/12/09 and 6.12.09
    //

    m_years.clear();
    m_dates.clear();
    m_times.clear();
    m_dateRanges.clear();
    m_text = text;

    // we are english-only here!
    QStringList longMonthNames;
    QStringList shortMonthNames;
    for ( int i = 1; i <= 12; ++i ) {
        longMonthNames << m_enLocale.monthName( i, QLocale::LongFormat );
        shortMonthNames << m_enLocale.monthName( i, QLocale::ShortFormat );
    }

    //
    // most of the dates and times can be checked with QRegExp
    //

    // DD.MM.YYYY
    QRegExp date1( "\\b\\d{1,2}\\.\\d{1,2}\\.\\d{4,4}\\b" );

    // DD.MM.YY
    QRegExp date2( "\\b\\d{1,2}\\.\\d{1,2}\\.\\d{2,2}\\b" );

    // MM/DD/YYYY
    QRegExp date3( "\\b\\d{1,2}/\\d{1,2}/\\d{4,4}\\b" );

    // MM/DD/YY
    QRegExp date4( "\\b\\d{1,2}/\\d{1,2}/\\d{2,2}\\b" );

    // January MM [YYYY] (no word boundry at the end for 'st' or 'nd' or 'th') (also excluding ranges)
    QRegExp date5( QString( "\\b(%1)\\s\\d{1,2}(?!(\\d|\\s?-\\s?\\d))(\\s\\d{4,4})?" ).arg( longMonthNames.join( "|" ) ) );

    // January, MM [YYYY] (no word boundry at the end for 'st' or 'nd' or 'th') (also excluding ranges)
    QRegExp date6( QString( "\\b(%1),\\s?\\d{1,2}(?!(\\d|\\s?-\\s?\\d))(\\s\\d{4,4})?" ).arg( longMonthNames.join( "|" ) ) );

    // FIXME: date ranges 1-4

    QRegExp dateRange5( QString( "(\\b(?:%1)\\s\\d{1,2})\\s?-\\s?(\\d{1,2})(\\s\\d{4,4})?" ).arg( longMonthNames.join( "|" ) ) );

    QRegExp dateRange6( QString( "(\\b(?:%1),\\s?\\d{1,2})\\s?-\\s?(\\d{1,2})(\\s\\d{4,4})?" ).arg( longMonthNames.join( "|" ) ) );

    // YYYY (exclude those in full dates matched by the above)
    QRegExp year( "[^\\./]\\d{4,4}\\b" );

    // hh:mm[pm|am]
    QRegExp time1( "\\b\\d{1,2}\\:\\d{2,2}\\s?(pm|am|AM|PM)?\\b" );

    // hh:mm
    QRegExp time2( "\\b\\d{1,2}\\:\\d{2,2}\\b(?!\\s?(pm|am|AM|PM))\\b" );

    // hh o'clock
//    QRegExp time3( "\\b\\d{1,2}\\so'clock\\b" );

    lookForYears( year );

    lookForDates( date1, "d.M.yyyy" );
    lookForDates( date2, "d.M.yy" );
    lookForDates( date3, "M/d/yyyy" );
    lookForDates( date4, "M/d/yy" );
    lookForDates( date5, "MMMM d", true );
    lookForDates( date5, "MMMM d yyyy" );
    lookForDates( date6, "MMMM, d", true );
    lookForDates( date6, "MMMM,d", true );
    lookForDates( date6, "MMMM, d yyyy" );
    lookForDates( date6, "MMMM,d yyyy" );

    lookForDateRanges( dateRange5, "MMMM d", 1, 2, 3, true );
    lookForDateRanges( dateRange5, "MMMM d yyyy", 1, 2, 3, false );
    lookForDateRanges( dateRange6, "MMMM,d", 1, 2, 3, true );
    lookForDateRanges( dateRange6, "MMMM, d", 1, 2, 3, true );
    lookForDateRanges( dateRange6, "MMMM,d yyyy", 1, 2, 3, false );
    lookForDateRanges( dateRange6, "MMMM, d yyyy", 1, 2, 3, false );

    lookForTimes( time1, "h:map" );
    lookForTimes( time1, "h:m ap" );
    lookForTimes( time2, "h:m" );

    // FIXME: do a date and time proximity search to create combined datetime objects

    //
    // Now use the dates and times to create statements
    //
    for ( QHash<int, QPair<QDate, int> >::const_iterator it = m_dates.constBegin();
          it != m_dates.constEnd(); ++it ) {
        // FIXME: this is not great: 1. dtstart has range dateTime, 2. we do not know that it is a start
        //        better use something else or even create Scribo::Literal as alternative to Entity
        Scribo::Statement s( Nepomuk::Vocabulary::PIMO::dtstart(), Nepomuk::Variant( it.value().first ), Soprano::Graph() );
        Scribo::TextOccurrence oc;
        oc.setStartPos( it.key() );
        oc.setLength( it.value().second );
//        oc.setRelevance( 0.9 ); ?????????
        s.addOccurrence( oc );
        addNewMatch( s );
    }

    for ( QHash<int, QPair<QTime, int> >::const_iterator it = m_times.constBegin();
          it != m_times.constEnd(); ++it ) {
        // FIXME: this is not great: 1. dtstart has range dateTime, 2. we do not know that it is a start
        //        better use something else or even create Scribo::Literal as alternative to Entity
        Scribo::Statement s( Nepomuk::Vocabulary::PIMO::dtstart(), Nepomuk::Variant( it.value().first ), Soprano::Graph() );
        Scribo::TextOccurrence oc;
        oc.setStartPos( it.key() );
        oc.setLength( it.value().second );
//        oc.setRelevance( 0.9 ); ?????????
        s.addOccurrence( oc );
        addNewMatch( s );
    }

    for ( QHash<int, QPair<QPair<QDate, QDate>, int> >::const_iterator it = m_dateRanges.constBegin();
          it != m_dateRanges.constEnd(); ++it ) {
        // FIXME: this is not great: 1. dtstart has range dateTime, 2. we do not know that it is a start
        //        better use something else or even create Scribo::Literal as alternative to Entity
        Scribo::Statement s1( Nepomuk::Vocabulary::PIMO::dtstart(), Nepomuk::Variant( it.value().first.first ), Soprano::Graph() );
        Scribo::TextOccurrence oc1;
        oc1.setStartPos( it.key() );
        oc1.setLength( it.value().second );
//        oc.setRelevance( 0.9 ); ?????????
        s1.addOccurrence( oc1 );
        addNewMatch( s1 );

        Scribo::Statement s2( Nepomuk::Vocabulary::PIMO::dtend(), Nepomuk::Variant( it.value().first.second ), Soprano::Graph() );
        Scribo::TextOccurrence oc2;
        oc2.setStartPos( it.key() );
        oc2.setLength( it.value().second );
//        oc.setRelevance( 0.9 ); ?????????
        s2.addOccurrence( oc2 );
        addNewMatch( s2 );
    }

    emitFinished();
}
Esempio n. 24
0
/*
 * SHA256 block compression function.  The 256-bit state is transformed via
 * the 512-bit input block to produce a new state.
 */
static void
SHA256_Transform(uint32_t * state, const uint32_t block[16], int swap)
{
	uint32_t W[64];
	uint32_t S[8];
	uint32_t t0, t1;
	int i;

	/* 1. Prepare message schedule W. */
	if(swap)
		for (i = 0; i < 16; i++)
			W[i] = htobe32(block[i]);
	else
		memcpy(W, block, 64);
	for (i = 16; i < 64; i += 2) {
		W[i] = s1(W[i - 2]) + W[i - 7] + s0(W[i - 15]) + W[i - 16];
		W[i+1] = s1(W[i - 1]) + W[i - 6] + s0(W[i - 14]) + W[i - 15];
	}

	/* 2. Initialize working variables. */
	memcpy(S, state, 32);

	/* 3. Mix. */
	RNDr(S, W, 0, 0x428a2f98);
	RNDr(S, W, 1, 0x71374491);
	RNDr(S, W, 2, 0xb5c0fbcf);
	RNDr(S, W, 3, 0xe9b5dba5);
	RNDr(S, W, 4, 0x3956c25b);
	RNDr(S, W, 5, 0x59f111f1);
	RNDr(S, W, 6, 0x923f82a4);
	RNDr(S, W, 7, 0xab1c5ed5);
	RNDr(S, W, 8, 0xd807aa98);
	RNDr(S, W, 9, 0x12835b01);
	RNDr(S, W, 10, 0x243185be);
	RNDr(S, W, 11, 0x550c7dc3);
	RNDr(S, W, 12, 0x72be5d74);
	RNDr(S, W, 13, 0x80deb1fe);
	RNDr(S, W, 14, 0x9bdc06a7);
	RNDr(S, W, 15, 0xc19bf174);
	RNDr(S, W, 16, 0xe49b69c1);
	RNDr(S, W, 17, 0xefbe4786);
	RNDr(S, W, 18, 0x0fc19dc6);
	RNDr(S, W, 19, 0x240ca1cc);
	RNDr(S, W, 20, 0x2de92c6f);
	RNDr(S, W, 21, 0x4a7484aa);
	RNDr(S, W, 22, 0x5cb0a9dc);
	RNDr(S, W, 23, 0x76f988da);
	RNDr(S, W, 24, 0x983e5152);
	RNDr(S, W, 25, 0xa831c66d);
	RNDr(S, W, 26, 0xb00327c8);
	RNDr(S, W, 27, 0xbf597fc7);
	RNDr(S, W, 28, 0xc6e00bf3);
	RNDr(S, W, 29, 0xd5a79147);
	RNDr(S, W, 30, 0x06ca6351);
	RNDr(S, W, 31, 0x14292967);
	RNDr(S, W, 32, 0x27b70a85);
	RNDr(S, W, 33, 0x2e1b2138);
	RNDr(S, W, 34, 0x4d2c6dfc);
	RNDr(S, W, 35, 0x53380d13);
	RNDr(S, W, 36, 0x650a7354);
	RNDr(S, W, 37, 0x766a0abb);
	RNDr(S, W, 38, 0x81c2c92e);
	RNDr(S, W, 39, 0x92722c85);
	RNDr(S, W, 40, 0xa2bfe8a1);
	RNDr(S, W, 41, 0xa81a664b);
	RNDr(S, W, 42, 0xc24b8b70);
	RNDr(S, W, 43, 0xc76c51a3);
	RNDr(S, W, 44, 0xd192e819);
	RNDr(S, W, 45, 0xd6990624);
	RNDr(S, W, 46, 0xf40e3585);
	RNDr(S, W, 47, 0x106aa070);
	RNDr(S, W, 48, 0x19a4c116);
	RNDr(S, W, 49, 0x1e376c08);
	RNDr(S, W, 50, 0x2748774c);
	RNDr(S, W, 51, 0x34b0bcb5);
	RNDr(S, W, 52, 0x391c0cb3);
	RNDr(S, W, 53, 0x4ed8aa4a);
	RNDr(S, W, 54, 0x5b9cca4f);
	RNDr(S, W, 55, 0x682e6ff3);
	RNDr(S, W, 56, 0x748f82ee);
	RNDr(S, W, 57, 0x78a5636f);
	RNDr(S, W, 58, 0x84c87814);
	RNDr(S, W, 59, 0x8cc70208);
	RNDr(S, W, 60, 0x90befffa);
	RNDr(S, W, 61, 0xa4506ceb);
	RNDr(S, W, 62, 0xbef9a3f7);
	RNDr(S, W, 63, 0xc67178f2);

	/* 4. Mix local working variables into global state */
	for (i = 0; i < 8; i++)
		state[i] += S[i];
}
Esempio n. 25
0
void RegionalTerrain_3r::SigPushEdge(QuadEdge::Edge* e) {
    Segment_3r s1(e->Org()->pos, e->Dest()->pos);
    Segment_3r s2(e->Dest()->pos, e->Org()->pos);
    SigPushVisualSegment_3r(s1, Visual::Segment(mat_edge_));
    SigPushVisualSegment_3r(s2, Visual::Segment(mat_edge_), 25);
}
Esempio n. 26
0
int main(int argc, char **argv)
{
 PetscInitialize(&argc,&argv,PETSC_NULL,PETSC_NULL);
 //PetscInitializeNoArguments();

 // bogdan's thesis 2010 (Bhaga and Weber, JFM 1980)
 int iter = 1;

 double Re = 100;
 double We = 115.662;
 double Sc = 1;
 double Fr = 1.0;
 double alpha = 1.0;

 double rho_in = 1.225;
 double rho_out = 1350; 

 double mu_out = 1;
 double mu_in = 0.0000178;

 const char* _case = "9";

 // case 1
 if( strcmp( _case,"1") == 0 )
 {
  Re = sqrt(42.895); 
  mu_out = 2.73;
 }

 else if( strcmp( _case,"2") == 0 )
 {
  Re = 13.8487; // case 2
  mu_out = 1.28; 
 }

 else if( strcmp( _case,"3") == 0 )
 {
  Re = 33.0413; // case 3
  mu_out = 0.5396; // case 3
 }

 else if( strcmp( _case,"6") == 0 )
 {
  Re = sqrt(3892.856); // case 6
  mu_out = 0.2857; // case 6
 }

 else if( strcmp( _case,"7") == 0 )
 {
  Re = sqrt(18124.092); // case 7
  mu_out = 0.1324; // case 7
 }

 else if( strcmp( _case,"8") == 0 )
 {
  Re = sqrt(41505.729); // case 8 (extream)
  mu_out = 0.0875134907735; // extream
 }
 else if( strcmp( _case,"9") == 0 )
 {
   double bubbleDiam = 5.2E-3;
   double gravity = 9.8; 
   double sigma = 0.0728; 
   
   rho_in = 1.205; 
   rho_out = 998.0; 
   mu_out = 958.08E-6; 
   mu_in = 18.21E-6; 

   Re = sqrt( CalcArchimedesBuoyancy(gravity,bubbleDiam,rho_out,mu_out) );
   We = CalcEotvos(gravity,bubbleDiam,rho_out,sigma);
 }
 else
 {
  cerr << "test case " << _case << " not available!" << endl;
  exit(1);
 }


 double cfl = 0.5;

 const char* _frame = "fixed";
 //const char* _frame = "moving";

 // fixed
 double c1 = 0.0;      // lagrangian
 double c2 = 1.0;      // smooth vel 
 double c3 = 10.0;     // smooth coord (fujiwara)
 double d1 = 1.0;      // surface tangent vel = (u-ut)
 double d2 = 0.1;      // surface smooth coord (fujiwara)

 // moving
 if( strcmp( _frame,"moving") == 0 )
 {
  c1 = 0.0;      // lagrangian
  c2 = 1.0;      // smooth vel: OBS - different result with c1=0.0
  c3 = 10.0;      // smooth coord (fujiwara)
  d1 = 0.0;      // surface tangent velocity u_n=u-u_t 
  d2 = 0.1;      // surface smooth cord (fujiwara)
 }

 string meshFile = "airWaterSugarPBC-wallNoSlip.msh";
 
 Solver *solverP = new PetscSolver(KSPCG,PCICC);
 Solver *solverV = new PetscSolver(KSPCG,PCILU);
 //Solver *solverV = new PetscSolver(KSPCG,PCJACOBI);
 Solver *solverC = new PetscSolver(KSPCG,PCICC);

 const char *binFolder  = "/work/gcpoliveira/post-processing/3d/rising-compare/bin/";
 const char *mshFolder  = "/work/gcpoliveira/post-processing/3d/rising-compare/msh/";
 const char *vtkFolder  = "/work/gcpoliveira/post-processing/3d/rising-compare/vtk/";
 const char *datFolder  = "/work/gcpoliveira/post-processing/3d/rising-compare/dat/";
 string meshDir = (string) getenv("MESH3D_DIR");
 
 if( strcmp( _frame,"moving") == 0 )
  meshDir += "/rising/movingFrame/" + meshFile;
 else
  meshDir += "/rising/" + meshFile;
 
 const char *mesh = meshDir.c_str();

 Model3D m1;
 Simulator3D s1;

 if( *(argv+1) == NULL )     
 {
  cout << endl;
  cout << "--------------> STARTING FROM 0" << endl;
  cout << endl;

  const char *mesh1 = mesh;

  m1.readMSH(mesh1);
  m1.setInterfaceBC();
  m1.setTriEdge();
  m1.mesh2Dto3D();
  m1.setMapping();
#if NUMGLEU == 5
 m1.setMiniElement();
#else
 m1.setQuadElement();
#endif
  m1.setSurfaceConfig();
  m1.setInitSurfaceVolume();
  m1.setInitSurfaceArea();
  m1.setGenericBC();

  s1(m1);

  s1.setRe(Re);
  s1.setSc(Sc);
  s1.setWe(We);
  s1.setFr(Fr);
  s1.setC1(c1);
  s1.setC2(c2);
  s1.setC3(c3);
  s1.setD1(d1);
  s1.setD2(d2);
  s1.setAlpha(alpha);
  s1.setMu(mu_in,mu_out);
  s1.setRho(rho_in,rho_out);
  s1.setCfl(cfl);
  s1.init();
  s1.setDtALETwoPhase();
  s1.setSolverPressure(solverP);
  s1.setSolverVelocity(solverV);
  s1.setSolverConcentration(solverC);
 }
 else if( strcmp( *(argv+1),"restart") == 0 ) 
 {
  cout << endl;
  cout << "--------------> RE-STARTING..." << endl;
  cout << endl;

  // load surface mesh
  string aux = *(argv+2);
  string file = (string) "/work/gcpoliveira/post-processing/3d/rising/msh/newMesh-" + *(argv+2) + (string) ".msh";
  const char *mesh2 = file.c_str();
  m1.readMSH(mesh2);
  m1.setInterfaceBC();
  m1.setTriEdge();
  m1.mesh2Dto3D();

  s1(m1);

  // load 3D mesh
  file = (string) "/work/gcpoliveira/post-processing/3d/rising/vtk/sim-" + *(argv+2) + (string) ".vtk";
  const char *vtkFile = file.c_str();

  m1.readVTK(vtkFile);
  m1.setMapping();
#if NUMGLEU == 5
  m1.setMiniElement();
#else
  m1.setQuadElement();
#endif
  m1.readVTKHeaviside(vtkFile);
  m1.setSurfaceConfig();
  m1.setInitSurfaceVolume();
  m1.setInitSurfaceArea();
  m1.setGenericBC();

  s1(m1);

  s1.setSolverPressure(solverP);
  s1.setSolverVelocity(solverV);
  s1.setSolverConcentration(solverC);

  iter = s1.loadSolution("/work/gcpoliveira/post-processing/3d/rising/","sim",atoi(*(argv+2)));
 }

 // Point's distribution
 Helmholtz3D h1(m1);
 h1.setBC();
 h1.initRisingBubble();
 h1.assemble();
 h1.setk(0.2);
 h1.matMountC();
 h1.setUnCoupledCBC(); 
 h1.setCRHS();
 h1.unCoupledC();
 h1.setModel3DEdgeSize();

 InOut save(m1,s1); // cria objeto de gravacao
 save.saveVTK(vtkFolder,"geometry");
 save.saveVTKSurface(vtkFolder,"geometry");
 save.saveMeshInfo(datFolder);
 save.saveInfo(datFolder,"info",mesh);

 double vinst=0;
 double vref=0;
 double xref=0;
 double xinit=0;
 double dx=0;
 if( strcmp( _frame,"moving") == 0 )
 {
  // moving
  vref = s1.getURef();
  xref = s1.getXRef();
  s1.setCentroidVelPos();
  xinit = s1.getCentroidPosXAverage();
 }

 int nIter = 30000;
 int nReMesh = 1;
 for( int i=1;i<=nIter;i++ )
 {
  for( int j=0;j<nReMesh;j++ )
  {

   cout << color(none,magenta,black);
   cout << "____________________________________ Iteration: " 
	    << iter << endl << endl;
   cout << resetColor();

   // moving
   if( strcmp( _frame,"moving") == 0 )
   {
	// moving frame

	dx = s1.getCentroidPosXAverage() - xinit;
	vinst = s1.getCentroidVelXAverage() + dx/s1.getDt();
    vref += vinst;
	xref += vref*s1.getDt();
	cout << "vref: " << vref << " xref: " << xref << endl;
	cout << "dx: " << dx << endl;
    s1.setUSol(vinst);
    m1.setGenericBC(vref);
    s1.setURef(vref);
	s1.setXRef(xref);
   }

   //s1.stepLagrangian();
   s1.stepALE();
   s1.setDtALETwoPhase();

   InOut save(m1,s1); // cria objeto de gravacao
   save.printSimulationReport();

   s1.movePoints();
   s1.assemble();
   s1.matMount();
   s1.setUnCoupledBC();
   s1.setRHS();
   s1.setGravity("-X");
   //s1.setInterface();
   s1.setInterfaceGeo();
   s1.unCoupled();

   if ( i%5 == 0 )
   {
   save.saveMSH(mshFolder,"newMesh",iter);
   save.saveVTK(vtkFolder,"sim",iter);
   save.saveVTKSurface(vtkFolder,"sim",iter);
   save.saveSol(binFolder,"sim",iter);
   save.saveBubbleInfo(datFolder);
   //save.crossSectionalVoidFraction(datFolder,"voidFraction",iter);
   }

   s1.saveOldData();

   cout << color(none,magenta,black);
   cout << "________________________________________ END of " 
	    << iter << endl << endl;;
   cout << resetColor();

   s1.timeStep();

   iter++;
  }
  Helmholtz3D h2(m1,h1);
  h2.setBC();
  h2.initRisingBubble();
  h2.assemble();
  h2.setk(0.2);
  h2.matMountC();
  h2.setUnCoupledCBC(); 
  h2.setCRHS();
  h2.unCoupledC();
  
  if ( i%5 == 0 )
  {
  h2.saveVTK(vtkFolder,"edge",iter-1);
  h2.saveChordalEdge(datFolder,"edge",iter-1);
  }

  h2.setModel3DEdgeSize();

  Model3D mOld = m1; 

  /* *********** MESH TREATMENT ************* */
  // set normal and kappa values
  m1.setNormalAndKappa();
  m1.initMeshParameters();

  // 3D operations
  m1.insert3dMeshPointsByDiffusion(6.0);
  m1.remove3dMeshPointsByDiffusion(0.5);
  //m1.removePointByVolume();
  //m1.removePointsByInterfaceDistance();
  //m1.remove3dMeshPointsByDistance();
  m1.remove3dMeshPointsByHeight();
  m1.delete3DPoints();

  // surface operations
  m1.smoothPointsByCurvature();

  m1.insertPointsByLength("flat");
  //m1.insertPointsByCurvature("flat");
  //m1.removePointsByCurvature();
  //m1.insertPointsByInterfaceDistance("flat");
  m1.contractEdgesByLength("flat");
  //m1.removePointsByLength();
  m1.flipTriangleEdges();

  m1.removePointsByNeighbourCheck();
  //m1.checkAngleBetweenPlanes();
  /* **************************************** */

  //m1.mesh2Dto3DOriginal();
  m1.mesh3DPoints();
  m1.setMapping();
#if NUMGLEU == 5
 m1.setMiniElement();
#else
 m1.setQuadElement();
#endif
  m1.setSurfaceConfig();

  if( strcmp( _frame,"moving") == 0 )
   m1.setGenericBC(vref);
  else
  m1.setGenericBC();

  Simulator3D s2(m1,s1);
  s2.applyLinearInterpolation(mOld);
  s1 = s2;
  s1.setSolverPressure(solverP);
  s1.setSolverVelocity(solverV);
  s1.setSolverConcentration(solverC);

  InOut saveEnd(m1,s1); // cria objeto de gravacao
  saveEnd.printMeshReport();
  saveEnd.saveMeshInfo(datFolder);
 }

 PetscFinalize();
 return 0;
}
Esempio n. 27
0
int main(int argc, char **argv)
{
 /* This test case applies a prescribed vortex field in a unit cube to
  * test the re-meshing techinique of the surface mesh. 
  *
  * OBS.: - comment stepSL() on Simulator3D::stepALE
  *       - switch to tetrahedralize( (char*) "QYYAp",&in,&out ) on
  *       Model3D::mesh3DPoints
  *
  * Since the field is prescribed, there is no need of calculating the
  * convection in a Euleurian way (stepSL) and the insertion of nodes on
  * the 3D mesh.
  *
  * */

 PetscInitializeNoArguments();
 
 int iter = 1;
 double d1 = 0.0;   // surface tangent velocity u_n=u-u_t 
 double d2 = 0.0;   // surface smooth cord (fujiwara)

 double dt = 0.02;
 double T = 3.0;
 double time = 0;

 string meshFile = "sphere.msh";

 const char *vtkFolder  = "./vtk/";
 const char *mshFolder  = "./msh/";
 const char *datFolder  = "./dat/";
 string meshDir = (string) getenv("DATA_DIR");
 meshDir += "/gmsh/3d/sphere/vortex/" + meshFile;
 const char *mesh = meshDir.c_str();

 Model3D m1;
 Simulator3D s1;

 const char *mesh1 = mesh;
 m1.readMSH(mesh1);
 m1.setInterfaceBC();
 m1.setTriEdge();
 m1.mesh2Dto3D("QYYAp");
 m1.setMapping();
#if NUMGLEU == 5
 m1.setMiniElement();
#else
 m1.setQuadElement();
#endif
 m1.setSurfaceConfig();
 m1.setInitSurfaceVolume();
 m1.setInitSurfaceArea();

 s1(m1);

 s1.setDt(dt);

 s1.setD1(d1);
 s1.setD2(d2);

 // initial conditions
 s1.stepImposedPeriodicField("3d",T,s1.getTime()); // X,Y and Z --> Sol(n+1)

 int nReMesh = 1;
 while( time < T )
 {
  for( int j=0;j<nReMesh;j++ )
  {
   cout << color(none,magenta,black);
   cout << "____________________________________ Iteration: "
	    << iter << endl << endl;
   cout << resetColor();

   InOut save(m1,s1); // cria objeto de gravacao
   save.printSimulationReport();

   time = s1.getTime();

   // time step: n+1/4
   Simulator3D s20(m1,s1);
   double stepTime = dt/4.0;
   s20.stepImposedPeriodicField("3d",T,time+stepTime,stepTime); // SolOld(n) --> Sol(n+1/2)
   s20.saveOldData();        // Sol(n+1/2) --> SolOld(n+1/2)

   // time step: n+1/2
   Simulator3D s30(m1,s20);
   stepTime = dt/2.0;
   s30.stepImposedPeriodicField("3d",T,time+stepTime,stepTime); // SolOld(n) --> Sol(n+1/2)
   s30.saveOldData();        // Sol(n+1/2) --> SolOld(n+1/2)
   s30.stepALE();         // SolOld(n+1/2) --> ALE(n+1/2)

   // time step: n using ALE(n+1/2)
   s1.setUALE(s30.getUALE());
   s1.setVALE(s30.getVALE());
   s1.setWALE(s30.getWALE());
   s1.movePoints();
   m1.setNormalAndKappa();

   double field = cos(3.14159265358*time/T);
   cout << endl;
   cout << "                             | T:        " << T << endl;
   cout << "                             | dt:       " << dt << endl;
   cout << "                             | time:     " << time << endl;
   cout << "                             | iter:     " << iter << endl;
   cout << "                             | field:    " << field << endl;
   cout << endl;

   save.saveMSH(mshFolder,"newMesh",iter);
   save.saveVTK(vtkFolder,"sim",iter);
   save.saveVTKSurface(vtkFolder,"sim",iter);
   save.saveBubbleInfo(datFolder);

   s1.saveOldData(); // Sol(n+1) --> SolOld(n)

   s1.timeStep();

   cout << color(none,magenta,black);
   cout << "________________________________________ END of "
	<< iter << endl << endl;;
   cout << resetColor();

   iter++;
  }
  Model3D mOld = m1;

  /* *********** MESH TREATMENT ************* */
  // set normal and kappa values
  m1.initMeshParameters();

  // surface operations
  //m1.smoothPointsByCurvature();

  m1.insertPointsByLength("flat");
  m1.contractEdgesByLength("flat");

  if( time > 1.9 )
   m1.contractEdgesByLength("flat",0.65);
  if( time > 2.2 )
   m1.contractEdgesByLength2("flat",0.7);
  if( time > 2.3 )
   m1.contractEdgesByLength2("flat",0.8);
  if( time > 2.8 )
   m1.contractEdgesByLength2("flat",1.2);

  //m1.removePointsByLength();
  m1.flipTriangleEdges();

  //m1.removePointsByNeighbourCheck();
  //m1.checkAngleBetweenPlanes();
  /* **************************************** */

  m1.setInterfaceBC();
  m1.setMiniElement();
  m1.restoreMappingArrays();
  m1.setSurfaceVolume();
  m1.setSurfaceArea();
  m1.triMeshStats();

  // computing velocity field X^(n+1),time+1 at new nodes too!
  Simulator3D s2(m1,s1);
  s2.stepImposedPeriodicField("3d",T,time); // X,Y and Z --> Sol(n+1)
  s2.saveOldData();
  s1 = s2;
  s1.setCentroidVelPos();

  InOut saveEnd(m1,s1); // cria objeto de gravacao
  saveEnd.printMeshReport();
  saveEnd.saveMeshInfo(datFolder);
 }

 PetscFinalize();
 return 0;
}
Esempio n. 28
0
int main(int argc, char* argv[])
{
  // Load the mesh.
  Mesh mesh;
  H2DReader mloader;
  mloader.load("GAMM-channel.mesh", &mesh);

  // Perform initial mesh refinements.
  for (int i = 0; i < INIT_REF_NUM; i++) mesh.refine_all_elements(0);
  //mesh.refine_towards_boundary(BDY_SOLID_WALL_BOTTOM, 2);

  // Initialize boundary condition types and spaces with default shapesets.
  L2Space space_rho(&mesh, P_INIT);
  L2Space space_rho_v_x(&mesh, P_INIT);
  L2Space space_rho_v_y(&mesh, P_INIT);
  L2Space space_e(&mesh, P_INIT);
  int ndof = Space::get_num_dofs(Hermes::vector<Space*>(&space_rho, &space_rho_v_x, &space_rho_v_y, &space_e));
  info("ndof: %d", ndof);

  // Initialize solutions, set initial conditions.
  InitialSolutionEulerDensity prev_rho(&mesh, RHO_EXT);
  InitialSolutionEulerDensityVelX prev_rho_v_x(&mesh, RHO_EXT * V1_EXT);
  InitialSolutionEulerDensityVelY prev_rho_v_y(&mesh, RHO_EXT * V2_EXT);
  InitialSolutionEulerDensityEnergy prev_e(&mesh, QuantityCalculator::calc_energy(RHO_EXT, RHO_EXT * V1_EXT, RHO_EXT * V2_EXT, P_EXT, KAPPA));

  // Numerical flux.
  OsherSolomonNumericalFlux num_flux(KAPPA);

  // Initialize weak formulation.
  EulerEquationsWeakFormSemiImplicitMultiComponent wf(&num_flux, KAPPA, RHO_EXT, V1_EXT, V2_EXT, P_EXT, BDY_SOLID_WALL_BOTTOM, BDY_SOLID_WALL_TOP, 
    BDY_INLET, BDY_OUTLET, &prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e, (P_INIT == 0));

  // Initialize the FE problem.
  bool is_linear = true;  
  DiscreteProblem dp(&wf, Hermes::vector<Space*>(&space_rho, &space_rho_v_x, &space_rho_v_y, &space_e), is_linear);

  // If the FE problem is in fact a FV problem.
  if(P_INIT == 0) 
    dp.set_fvm();  

  // Filters for visualization of Mach number, pressure and entropy.
  MachNumberFilter Mach_number(Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e), KAPPA);
  PressureFilter pressure(Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e), KAPPA);
  EntropyFilter entropy(Hermes::vector<MeshFunction*>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e), KAPPA, RHO_EXT, P_EXT);

  ScalarView pressure_view("Pressure", new WinGeom(0, 0, 600, 300));
  ScalarView Mach_number_view("Mach number", new WinGeom(700, 0, 600, 300));
  ScalarView entropy_production_view("Entropy estimate", new WinGeom(0, 400, 600, 300));
  
  ScalarView s1("1", new WinGeom(0, 0, 600, 300));
  ScalarView s2("2", new WinGeom(700, 0, 600, 300));
  ScalarView s3("3", new WinGeom(0, 400, 600, 300));
  ScalarView s4("4", new WinGeom(700, 400, 600, 300));

  // Set up the solver, matrix, and rhs according to the solver selection.
  SparseMatrix* matrix = create_matrix(matrix_solver);
  Vector* rhs = create_vector(matrix_solver);
  Solver* solver = create_linear_solver(matrix_solver, matrix, rhs);

  // Set up CFL calculation class.
  CFLCalculation CFL(CFL_NUMBER, KAPPA);

  int iteration = 0; double t = 0;
  for(t = 0.0; t < 3.0; t += time_step) {
    info("---- Time step %d, time %3.5f.", iteration++, t);

    // Set the current time step.
    wf.set_time_step(time_step);

    // Assemble the stiffness matrix and rhs.
    info("Assembling the stiffness matrix and right-hand side vector.");
    dp.assemble(matrix, rhs);

    // Solve the matrix problem.
    info("Solving the matrix problem.");
    scalar* solution_vector = NULL;
    if(solver->solve()) {
      solution_vector = solver->get_solution();
      Solution::vector_to_solutions(solution_vector, Hermes::vector<Space *>(&space_rho, &space_rho_v_x, 
      &space_rho_v_y, &space_e), Hermes::vector<Solution *>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));
    }
    else
      error ("Matrix solver failed.\n");

    if(SHOCK_CAPTURING) {
      DiscontinuityDetector discontinuity_detector(Hermes::vector<Space *>(&space_rho, &space_rho_v_x, 
        &space_rho_v_y, &space_e), Hermes::vector<Solution *>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));

      std::set<int> discontinuous_elements = discontinuity_detector.get_discontinuous_element_ids(DISCONTINUITY_DETECTOR_PARAM);

      FluxLimiter flux_limiter(solution_vector, Hermes::vector<Space *>(&space_rho, &space_rho_v_x, 
        &space_rho_v_y, &space_e), Hermes::vector<Solution *>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e));

      flux_limiter.limit_according_to_detector(discontinuous_elements);
    }

    CFL.calculate_semi_implicit(Hermes::vector<Solution *>(&prev_rho, &prev_rho_v_x, &prev_rho_v_y, &prev_e), &mesh, time_step);

    // Visualization.
    
    Mach_number.reinit();
    pressure.reinit();
    entropy.reinit();
    pressure_view.show(&pressure);
    entropy_production_view.show(&entropy);
    Mach_number_view.show(&Mach_number);
    
    /*
    s1.show(&prev_rho);
    s2.show(&prev_rho_v_x);
    s3.show(&prev_rho_v_y);
    s4.show(&prev_e);
    */
    //View::wait();
    
  }
  
  pressure_view.close();
  entropy_production_view.close();
  Mach_number_view.close();

  
  s1.close();
  s2.close();
  s3.close();
  s4.close();
  

  return 0;
}
Esempio n. 29
0
DEF_TEST(SkSLMemoryLayout430Test, r) {
    SkSL::Context context;
    SkSL::MemoryLayout layout(SkSL::MemoryLayout::k430_Standard);

    // basic types
    REPORTER_ASSERT(r,  4 == layout.size(*context.fFloat_Type));
    REPORTER_ASSERT(r,  8 == layout.size(*context.fVec2_Type));
    REPORTER_ASSERT(r, 12 == layout.size(*context.fVec3_Type));
    REPORTER_ASSERT(r, 16 == layout.size(*context.fVec4_Type));
    REPORTER_ASSERT(r,  4 == layout.size(*context.fInt_Type));
    REPORTER_ASSERT(r,  8 == layout.size(*context.fIVec2_Type));
    REPORTER_ASSERT(r, 12 == layout.size(*context.fIVec3_Type));
    REPORTER_ASSERT(r, 16 == layout.size(*context.fIVec4_Type));
    REPORTER_ASSERT(r,  1 == layout.size(*context.fBool_Type));
    REPORTER_ASSERT(r,  2 == layout.size(*context.fBVec2_Type));
    REPORTER_ASSERT(r,  3 == layout.size(*context.fBVec3_Type));
    REPORTER_ASSERT(r,  4 == layout.size(*context.fBVec4_Type));
    REPORTER_ASSERT(r, 16 == layout.size(*context.fMat2x2_Type));
    REPORTER_ASSERT(r, 32 == layout.size(*context.fMat2x4_Type));
    REPORTER_ASSERT(r, 48 == layout.size(*context.fMat3x3_Type));
    REPORTER_ASSERT(r, 32 == layout.size(*context.fMat4x2_Type));
    REPORTER_ASSERT(r, 64 == layout.size(*context.fMat4x4_Type));
    REPORTER_ASSERT(r,  4 == layout.alignment(*context.fFloat_Type));
    REPORTER_ASSERT(r,  8 == layout.alignment(*context.fVec2_Type));
    REPORTER_ASSERT(r, 16 == layout.alignment(*context.fVec3_Type));
    REPORTER_ASSERT(r, 16 == layout.alignment(*context.fVec4_Type));
    REPORTER_ASSERT(r,  4 == layout.alignment(*context.fInt_Type));
    REPORTER_ASSERT(r,  8 == layout.alignment(*context.fIVec2_Type));
    REPORTER_ASSERT(r, 16 == layout.alignment(*context.fIVec3_Type));
    REPORTER_ASSERT(r, 16 == layout.alignment(*context.fIVec4_Type));
    REPORTER_ASSERT(r,  1 == layout.alignment(*context.fBool_Type));
    REPORTER_ASSERT(r,  2 == layout.alignment(*context.fBVec2_Type));
    REPORTER_ASSERT(r,  4 == layout.alignment(*context.fBVec3_Type));
    REPORTER_ASSERT(r,  4 == layout.alignment(*context.fBVec4_Type));
    REPORTER_ASSERT(r,  8 == layout.alignment(*context.fMat2x2_Type));
    REPORTER_ASSERT(r, 16 == layout.alignment(*context.fMat2x4_Type));
    REPORTER_ASSERT(r, 16 == layout.alignment(*context.fMat3x3_Type));
    REPORTER_ASSERT(r,  8 == layout.alignment(*context.fMat4x2_Type));
    REPORTER_ASSERT(r, 16 == layout.alignment(*context.fMat4x4_Type));

    // struct 1
    std::vector<SkSL::Type::Field> fields1;
    fields1.emplace_back(SkSL::Modifiers(), SkString("a"), context.fVec3_Type.get());
    SkSL::Type s1(SkSL::Position(), SkString("s1"), fields1);
    REPORTER_ASSERT(r, 16 == layout.size(s1));
    REPORTER_ASSERT(r, 16 == layout.alignment(s1));

    fields1.emplace_back(SkSL::Modifiers(), SkString("b"), context.fFloat_Type.get());
    SkSL::Type s2(SkSL::Position(), SkString("s2"), fields1);
    REPORTER_ASSERT(r, 16 == layout.size(s2));
    REPORTER_ASSERT(r, 16 == layout.alignment(s2));

    fields1.emplace_back(SkSL::Modifiers(), SkString("c"), context.fBool_Type.get());
    SkSL::Type s3(SkSL::Position(), SkString("s3"), fields1);
    REPORTER_ASSERT(r, 32 == layout.size(s3));
    REPORTER_ASSERT(r, 16 == layout.alignment(s3));

    // struct 2
    std::vector<SkSL::Type::Field> fields2;
    fields2.emplace_back(SkSL::Modifiers(), SkString("a"), context.fInt_Type.get());
    SkSL::Type s4(SkSL::Position(), SkString("s4"), fields2);
    REPORTER_ASSERT(r, 4 == layout.size(s4));
    REPORTER_ASSERT(r, 4 == layout.alignment(s4));

    fields2.emplace_back(SkSL::Modifiers(), SkString("b"), context.fVec3_Type.get());
    SkSL::Type s5(SkSL::Position(), SkString("s5"), fields2);
    REPORTER_ASSERT(r, 32 == layout.size(s5));
    REPORTER_ASSERT(r, 16 == layout.alignment(s5));

    // arrays
    SkSL::Type array1(SkString("float[4]"), SkSL::Type::kArray_Kind, *context.fFloat_Type, 4);
    REPORTER_ASSERT(r, 16 == layout.size(array1));
    REPORTER_ASSERT(r, 4 == layout.alignment(array1));
    REPORTER_ASSERT(r, 4 == layout.stride(array1));

    SkSL::Type array2(SkString("vec4[4]"), SkSL::Type::kArray_Kind, *context.fVec4_Type, 4);
    REPORTER_ASSERT(r, 64 == layout.size(array2));
    REPORTER_ASSERT(r, 16 == layout.alignment(array2));
    REPORTER_ASSERT(r, 16 == layout.stride(array2));
}
Esempio n. 30
0
bool append_test( )
{
  bool rc = true;

  // The sizes of the test strings used here are intended to explore
  // both appending without reallocation and appending with re-
  // allocation. A string can not be reused between tests because
  // an enlarged capacity is never reduced.

  std::string s1( "123456" );
  std::string s2( "x" );
  s1 += s2;
  if( s1 != "123456x"  || s1.size( ) != 7 || INSANE( s1 ) ) FAIL
  s1 += s2;
  if( s1 != "123456xx" || s1.size( ) != 8 || INSANE( s1 ) ) FAIL

  std::string s3( "123456" );
  s3 += "x";
  if( s3 != "123456x"  || s3.size( ) != 7 || INSANE( s3 ) ) FAIL
  s3 += "y";
  if( s3 != "123456xy" || s3.size( ) != 8 || INSANE( s3 ) ) FAIL

  std::string s4( "123456" );
  s4 += 'x';
  if( s4 != "123456x"  || s4.size( ) != 7 || INSANE( s4 ) ) FAIL
  s4 += 'z';
  if( s4 != "123456xz" || s4.size( ) != 8 || INSANE( s4 ) ) FAIL

  std::string s5( "123456" );
  std::string s6( "Hello, World!" );
  s5.append( s6, 12, 1 );
  if( s5 != "123456!"  || s5.size( ) != 7 || INSANE( s5 ) ) FAIL
  s5.append( s6, 0, 3 );
  if( s5 != "123456!Hel" || s5.size( ) != 10 || INSANE( s5 ) ) FAIL

  std::string s7( "123456" );
  s7.append( "fizzle", 1 );
  if( s7 != "123456f"  || s7.size( ) != 7 || INSANE( s7 ) ) FAIL
  s7.append( "fizzle", 3 );
  if( s7 != "123456ffiz" || s7.size( ) != 10 || INSANE( s7 ) ) FAIL

  std::string s8( "123456" );
  s8.append( "x" );
  if( s8 != "123456x"  || s8.size( ) != 7 || INSANE( s8 ) ) FAIL
  s8.append( "abc" );
  if( s8 != "123456xabc" || s8.size( ) != 10 || INSANE( s8 ) ) FAIL

  std::string s9( "123456" );
  s9.append( 1, 'x' );
  if( s9 != "123456x" || s9.size( ) != 7 || INSANE( s9 ) ) FAIL
  s9.append( 3, 'y' );
  if( s9 != "123456xyyy" || s9.size( ) != 10 || INSANE( s9 ) ) FAIL

  std::string s10( "123456" );
  s10.push_back( 'z' );
  if( s10 != "123456z" || s10.size( ) != 7 || INSANE( s10 ) ) FAIL
  s10.push_back( 'a' );
  if( s10 != "123456za" || s10.size( ) != 8 || INSANE( s10 ) ) FAIL

  return( rc );
}