Example #1
0
double MathFunction::Evaluate(double t, int diff) const //if diff=1, then differentiate w.r.t. time
{
    //t .. time or input parameter
    double val = 0;
    switch (funcmode)
    {
    case TMFempty: //no function
    {
        //val = 0;
        break;
    }
    case TMFpolynomial: //polynomial, coeffs contain polynomial coeffs a_1 * 1 + a_2 * t + a_3 * t^2 + ...
    {
        //if (t > 1) t = 1; //hack!!!!

        if (vectime.Length() == 2)
        {
            if (t < vectime(1)) t = vectime(1);
            else if (t > vectime(2)) t = vectime(2);
        }

        double pt = 1;
        if (!diff)
        {
            for (int i=1; i <= coeff.Length(); i++)
            {
                val += coeff(i) * pt;
                pt *= t;
            }
        }
        else if (diff==1) //a_2 * 1 + a_3 * 2* t + a_4 * 3* t^2 + ...
        {
            for (int i=2; i <= coeff.Length(); i++)
            {
                val += coeff(i) * pt * ((double)i-1);
                pt *= t;
            }
        }
        break;
    }
    case TMFpiecewiseconst: //time points, not interpolated, differentiation not possible
    {
        if (vectime.Length() != 0 && !diff)
        {
            int index = 1;
            index = FindIndexPiecewise(t);
            val = valX(index);
        }
        break;
    }
    case TMFpiecewiselinear: //time points linearly interpolated
    {
        if (vectime.Length() != 0 && !diff)
        {
            int index = 1;
            index = FindIndexPiecewise(t);
            val = InterpolatePiecewise(t, index);
        }
        break;
    }
    case TMFpiecewisequad: //time points quadratic interpolated, not yet implemented!
    {
        if (vectime.Length() != 0)
        {
            int i = 1;

            while (i <= vectime.Length() && t > vectime(i)) {
                i++;
            }

            if (i > vectime.Length())
            {
                i = vectime.Length();

                if (diff)
                    val = valY(i); //vel
                else
                    val = valX(i); //pos
            }
            else
            {
                double t1 = 0, t2;
                double x1 = 0;
                double x2 = 0;
                double v1 = 0;
                double v2 = 0;

                if (i >= 2)
                {
                    t1 = vectime(i-1);
                    x1 = valX(i-1); //pos
                    v1 = valY(i-1); //vel
                }
                double dt = vectime(i)-t1;
                if (dt <= 0) dt = 1;

                x2 = valX(i); //pos
                v2 = valY(i); //vel
                t2 = vectime(i);

                if (diff)
                    val = (1.-(t-t1)/dt)*v1 + (1.+(t-t2)/dt)*v2; //linearly interpolate velocity
                else
                {
                    val = x1 + (t-t1)*v1 + 0.5*(v2-v1)*Sqr(t-t1)/dt; //linearly interpolate velocity
                }
            }
        }
        break;
    }
    case TMFharmonic: //harmonic function
    {
        if (valX.Length() != 0 && valX.Length() == valY.Length() && valX.Length() == valZ.Length())
        {
            val = 0;
            for (int i=1; i <= valX.Length(); i++)
            {
                if (!diff)
                    val += valZ(i)*sin(valX(i)*t+valY(i));
                else
                    val += valZ(i)*valX(i)*cos(valX(i)*t+valY(i));
            }
        }
        break;
    }
    case TMFsin:
    {
        //input variable:t
        //Evaluate val = coeff(1) * Sin(coeff(2)*t + coeff(3))

        val = coeff(1) * sin(coeff(2)*t + coeff(3));

        break;
    }
    case TMFcos: //***
    {
        //input variable:t
        val = coeff(1) * cos(coeff(2)*t + coeff(3));

        break;
    }
    case TMFstaticDynamicFricion: //***
    {
        assert(0 && "ERROR in mathfunc.cpp: SetStaticDynamicFricionFunction not anymore supported by MathFunction, use TMFExpression instead!");

        ////input variable: velocity
        //if(coeff(3) > 0.0 && fabs(t) <= coeff(3))
        //{
        //	val = coeff(4)/(coeff(3))*t;
        //}else
        //{
        //	//if(-1e-10<t && t<1e-10)t=0.0;
        //	val = coeff(1)*Sgn(t) + coeff(2)*t;
        //}
        //break;
    }
    case TMFuserdefined: //***
    {
        val = ufunc(t,coeff);
        break;
    }
    case TMFexpression:
    {
        //val = mbsPI->EvaluateParsedFunction1D(parsedFunctionExpression, parsedFunctionVariables, t);               //$JG2013-4-29: old slow code of YV
        val = pf.Evaluate(t);                        //$JG2013-4-29: new code, faster: Parsed function is only evaluated, not interpreted!
        break;
    }
    default:
        ;
    }
    return val;
}
Example #2
0
// bfd - gets BFD(1:1) difference between Lab1, Lab2
double LCMSEXPORT cmsBFDdeltaE(LPcmsCIELab Lab1, LPcmsCIELab Lab2)
{
    double lbfd1,lbfd2,AveC,Aveh,dE,deltaL,
           deltaC,deltah,dc,t,g,dh,rh,rc,rt,bfd;
    cmsCIELCh LCh1, LCh2;


    if (Lab1 ->L == 0 && Lab2 ->L == 0) return 0;

    lbfd1 = ComputeLBFD(Lab1);
    lbfd2 = ComputeLBFD(Lab2);
    deltaL = lbfd2 - lbfd1;

    cmsLab2LCh(&LCh1, Lab1);
    cmsLab2LCh(&LCh2, Lab2);

    deltaC = LCh2.C - LCh1.C;
    AveC = (LCh1.C+LCh2.C)/2;
    Aveh = (LCh1.h+LCh2.h)/2;

    dE = cmsDeltaE(Lab1, Lab2);

    if (Sqr(dE)>(Sqr(Lab2->L-Lab1->L)+Sqr(deltaC)))
        deltah = sqrt(Sqr(dE)-Sqr(Lab2->L-Lab1->L)-Sqr(deltaC));
    else
        deltah =0;


    dc   = 0.035 * AveC / (1 + 0.00365 * AveC)+0.521;
    g    = sqrt(Sqr(Sqr(AveC))/(Sqr(Sqr(AveC))+14000));
    t    = 0.627+(0.055*cos((Aveh-254)/(180/M_PI))-
                  0.040*cos((2*Aveh-136)/(180/M_PI))+
                  0.070*cos((3*Aveh-31)/(180/M_PI))+
                  0.049*cos((4*Aveh+114)/(180/M_PI))-
                  0.015*cos((5*Aveh-103)/(180/M_PI)));

    dh    = dc*(g*t+1-g);
    rh    = -0.260*cos((Aveh-308)/(180/M_PI))-
            0.379*cos((2*Aveh-160)/(180/M_PI))-
            0.636*cos((3*Aveh+254)/(180/M_PI))+
            0.226*cos((4*Aveh+140)/(180/M_PI))-
            0.194*cos((5*Aveh+280)/(180/M_PI));

    rc = sqrt((AveC*AveC*AveC*AveC*AveC*AveC)/((AveC*AveC*AveC*AveC*AveC*AveC)+70000000));
    rt = rh*rc;

    bfd = sqrt(Sqr(deltaL)+Sqr(deltaC/dc)+Sqr(deltah/dh)+(rt*(deltaC/dc)*(deltah/dh)));

    return bfd;
}
Example #3
0
//---------------------------------------------------------------------------
// All the precipitation rate calcs are done in this routine 
// Takes the product composition and calculates rates from this
// Rates (in kg/s) are put into global x
//---------------------------------------------------------------------------
void CPrecipitator::EvalPrecipRates(MStream & Prod, double T) 
{
  T = Prod.T;   // May want to do the rate calculation at other temperature
  
  MIBayer & ProdB=Prod.IF<MIBayer>(false);
  MISSA & ProdSSA = Prod.IF<MISSA>(false);
  dResidenceTime = dTankVol/GTZ(Prod.Volume(MP_SL));
  for (int i=0; i<nPRECIPCOMPS; i++)
    xo[i]=x[i];   // Save old values for convergence test, damping
  double SAL = ProdSSA.SpecificSurfaceAreaVol();
  double SSA = ProdSSA.SpecificSurfaceAreaMass();
  double dSSat;
  if (ProdB.CausticConc(T) > 0)
    dSSat = (ProdB.AluminaConc(T)-ProdB.AluminaConcSat(T))/ProdB.CausticConc(T);
  else
    dSSat = 0.0; 
  if (dSSat < 0) dSSat=0.0;
  
  // Supersaturation...

  switch (iGrowthRateMethod) {
    
  case GRM_Fixed:
    x[iALUMINA] = dGrowthRate;
    x[1] = x[iBOUND_ORGANICS] = 0.0;
    break;
  case GRM_White: 
    {
      MIBayer & FeedB = Feed.IF<MIBayer>(false);
      double Ain = FeedB.AluminaConc(T);
      double Aout = ProdB.AluminaConc(T);
      dGrowthRateFactor = gF_White*K_White*Exps(-ER_White/T);
      dGrowthRate = dGrowthRateFactor*Sqr(dSSat);
      x[iALUMINA] = dGrowthRate*SAL*dTankVol/1000.;
      double sodaRate = m_dK_Soda*Sqr(dSSat)*Exps(m_dE_Soda/T)*(Aout - Ain)*dTankVol/1000.;
      if (sodaRate <0.0) sodaRate = 0.0;
      x[iBOUND_SODA] = sodaRate*(1.0-m_dBndOrgSoda);
      x[iBOUND_ORGANICS] = sodaRate*m_dBndOrgSoda;
      
			   
    }
    
    break;
  case GRM_TTTest: 
    { // Alumina Precipitation.... as per QPRECIPD.cpp
      MIBayer & FeedB = Feed.IF<MIBayer>(false);
      double C  = ProdB.CausticConc(T);
      double C25 = ProdB.CausticConc(C2K(25));
      double CtoS = ProdB.CtoS();
      double S_out = C25/CtoS;
      //      double FC = ProdB.FreeCaustic(T);
      double FC = ProdB.FreeCaustic(C2K(25));
      double ACeq = ProdB.AluminaConcSat(T)/C25;   
      double TOOC=ProdB.TOC(C2K(25))*MW_Na2CO3/MW_C;
      double a=Exps(-m_dActivationEnergy/T);
      double b=Exps(-TOOC*m_dk_TOC);
      double c=Pow(GTZ(S_out), m_dn_s);
      double d=Pow(GTZ(FC), m_dn_fc);
      double e=Pow(GTZ(ACeq), m_dn_eq);

      double K = m_dK0*a*b*c*d*e;
      double ACout = ProdB.AtoC();
      double VLiq = Prod.Volume()*3600.;
      double MSolids     = Prod.MassVector[spTHA]*3600.0/1000.0;
      double Sol = MSolids*1000.0/GTZ(VLiq);
      double deltaAC = K * dResidenceTime/3600 *
	Pow(GTZ(ACout-ACeq),m_dn_) * Pow(GTZ(Sol),m_dn_sol) * Pow(GTZ(m_dSSA),m_dn_ssa);
      double ACoutEst = dACin - deltaAC;
      double VolOut = Prod.Volume(MP_Liq, C2K(25.0));
      double xx = deltaAC*C25*VolOut*2*MW_Alumina/MW_Al2O3;
      if (xx<0.0) xx=0.0;
      x[iALUMINA] = xx;
      
      // Bound Soda calculations... slurped from QAL file
      double k1x = m_dK1 * (0.000598*C25 - 0.00036*K2C(T) + 0.019568*TOOC/C) * (1.0 - 0.758*CtoS);
      double BoundSoda = k1x * Sqr(ACoutEst-ACeq);
      
      if (x[iALUMINA]>=0.0) {
	double BndSoda    = BoundSoda*(x[iALUMINA]*MW_Al2O3/(2.0*MW_THA))*(1.0-m_dBndOrgSoda)*((2.0*MW_NaOH)/MW_Na2O);
	double BndOrgSoda = BoundSoda*(x[iALUMINA]*MW_Al2O3/(2.0*MW_THA))*(m_dBndOrgSoda)*(MW_OrganicSoda/MW_Na2O);
	/*  If represented as Na2O
	    double BndSoda    = BoundSoda*GibbsRate*dBndOrgSoda;
	    double BndOrgSoda = BoundSoda*GibbsRate*(1.0-dBndOrgSoda);
	*/	
	x[iBOUND_SODA] = BndSoda;
	x[iBOUND_ORGANICS] = BndOrgSoda;

      }
      

    }
    
    break;
  }

  return;
}
Example #4
0
inline int ColorDelta (ubyte *palette, int r, int g, int b, int j)
{
return Sqr (r - palette [j]) + Sqr (g - palette [j + 1]) + Sqr (b - palette [j + 2]);
}
void CMSSM_high_scale_constraint<Two_scale>::apply()
{
   assert(model && "Error: CMSSM_high_scale_constraint::apply():"
          " model pointer must not be zero");

   if (std::fabs(model->get_g1()) > 3.54491) {
#ifdef ENABLE_VERBOSE
      ERROR("CMSSM_high_scale_constraint: Non-perturbative gauge "
            "coupling g1 = " << model->get_g1());
#endif
      model->set_g1(3.54491);
   }
   if (std::fabs(model->get_g2()) > 3.54491) {
#ifdef ENABLE_VERBOSE
      ERROR("CMSSM_high_scale_constraint: Non-perturbative gauge "
            "coupling g2 = " << model->get_g2());
#endif
      model->set_g2(3.54491);
   }
   if (std::fabs(model->get_g3()) > 3.54491) {
#ifdef ENABLE_VERBOSE
      ERROR("CMSSM_high_scale_constraint: Non-perturbative gauge "
            "coupling g3 = " << model->get_g3());
#endif
      model->set_g3(3.54491);
   }

   update_scale();

   const auto Azero = INPUTPARAMETER(Azero);
   const auto m0 = INPUTPARAMETER(m0);
   const auto m12 = INPUTPARAMETER(m12);
   const auto Ye = MODELPARAMETER(Ye);
   const auto Yd = MODELPARAMETER(Yd);
   const auto Yu = MODELPARAMETER(Yu);

   MODEL->set_TYe((Azero*Ye).real());
   MODEL->set_TYd((Azero*Yd).real());
   MODEL->set_TYu((Azero*Yu).real());
   MODEL->set_mHd2(Re(Sqr(m0)));
   MODEL->set_mHu2(Re(Sqr(m0)));
   MODEL->set_mq2((Sqr(m0)*UNITMATRIX(3)).real());
   MODEL->set_ml2((Sqr(m0)*UNITMATRIX(3)).real());
   MODEL->set_md2((Sqr(m0)*UNITMATRIX(3)).real());
   MODEL->set_mu2((Sqr(m0)*UNITMATRIX(3)).real());
   MODEL->set_me2((Sqr(m0)*UNITMATRIX(3)).real());
   MODEL->set_MassB(Re(m12));
   MODEL->set_MassWB(Re(m12));
   MODEL->set_MassG(Re(m12));

   {
      const auto g1 = MODELPARAMETER(g1);
      const auto g2 = MODELPARAMETER(g2);
      const auto g3 = MODELPARAMETER(g3);
      const auto Yd = MODELPARAMETER(Yd);
      const auto Ye = MODELPARAMETER(Ye);
      const auto Yu = MODELPARAMETER(Yu);

      if (MaxAbsValue(g1) > 3.5449077018110318)
         model->get_problems().flag_non_perturbative_parameter("g1", MaxAbsValue(g1), model->get_scale(), 3.5449077018110318);
      else
         model->get_problems().unflag_non_perturbative_parameter("g1");
      if (MaxAbsValue(g2) > 3.5449077018110318)
         model->get_problems().flag_non_perturbative_parameter("g2", MaxAbsValue(g2), model->get_scale(), 3.5449077018110318);
      else
         model->get_problems().unflag_non_perturbative_parameter("g2");
      if (MaxAbsValue(g3) > 3.5449077018110318)
         model->get_problems().flag_non_perturbative_parameter("g3", MaxAbsValue(g3), model->get_scale(), 3.5449077018110318);
      else
         model->get_problems().unflag_non_perturbative_parameter("g3");
      if (MaxAbsValue(Yd) > 3.5449077018110318)
         model->get_problems().flag_non_perturbative_parameter("Yd", MaxAbsValue(Yd), model->get_scale(), 3.5449077018110318);
      else
         model->get_problems().unflag_non_perturbative_parameter("Yd");
      if (MaxAbsValue(Ye) > 3.5449077018110318)
         model->get_problems().flag_non_perturbative_parameter("Ye", MaxAbsValue(Ye), model->get_scale(), 3.5449077018110318);
      else
         model->get_problems().unflag_non_perturbative_parameter("Ye");
      if (MaxAbsValue(Yu) > 3.5449077018110318)
         model->get_problems().flag_non_perturbative_parameter("Yu", MaxAbsValue(Yu), model->get_scale(), 3.5449077018110318);
      else
         model->get_problems().unflag_non_perturbative_parameter("Yu");

   }
}
Example #6
0
/* Calculates scalar magnitude of a vector_t argument */
void
Magnitude(vector_t *v)
{
  v->w = sqrt(Sqr(v->x) + Sqr(v->y) + Sqr(v->z));
} /*Procedure Magnitude*/
Example #7
0
bool GenerateAndTimeOptimizeMultiPath(Robot& robot,MultiPath& multipath,Real xtol,Real dt)
{
  Timer timer;
  vector<GeneralizedCubicBezierSpline > paths;
  if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol))
    return false;
  printf("Generated interpolating path in time %gs\n",timer.ElapsedTime());

  RobotCSpace cspace(robot);
  for(size_t i=0;i<multipath.sections.size();i++) {
    for(size_t j=0;j<paths[i].segments.size();j++) {
      paths[i].segments[j].space = &cspace;
      paths[i].segments[j].manifold = &cspace;
    }
    for(int iters=0;iters<gNumTimescaleBisectIters;iters++)
      paths[i].Bisect();
  }

#if SAVE_INTERPOLATING_CURVES
  int index=0;
  printf("Saving sections, element %d to section_x_bezier.csv\n",index);
  for(size_t i=0;i<paths.size();i++) {
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"duration,x0,x1,x2,x3"<<endl;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_vel.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"v(0),v(0.5),v(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Deriv(0,temp);
	temp /= paths[i].durations[j];
	out<<temp[index];
	paths[i].segments[j].Deriv(0.5,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	paths[i].segments[j].Deriv(1,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_acc.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"a(0),a(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Accel(0,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<temp[index];
	paths[i].segments[j].Accel(1,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
  }
#endif //SAVE_INTERPOLATING_CURVES
#if SAVE_LORES_INTERPOLATING_CURVES
  paths.clear();
  if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol*2.0))
    return false;
  for(size_t i=0;i<multipath.sections.size();i++) {
    for(size_t j=0;j<paths[i].segments.size();j++) {
      paths[i].segments[j].space = &cspace;
      paths[i].segments[j].manifold = &cspace;
    }
  }
  for(size_t i=0;i<paths.size();i++) {
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_x2.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"duration,x0,x1,x2,x3"<<endl;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_vel_x2.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"v(0),v(0.5),v(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Deriv(0,temp);
	temp /= paths[i].durations[j];
	out<<temp[index];
	paths[i].segments[j].Deriv(0.5,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	paths[i].segments[j].Deriv(1,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_acc_x2.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"a(0),a(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Accel(0,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<temp[index];
	paths[i].segments[j].Accel(1,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
  }

  paths.clear();
  if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol*4.0))
    return false;
  for(size_t i=0;i<multipath.sections.size();i++) {
    for(size_t j=0;j<paths[i].segments.size();j++) {
      paths[i].segments[j].space = &cspace;
      paths[i].segments[j].manifold = &cspace;
    }
  }
  for(size_t i=0;i<paths.size();i++) {
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_x4.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"duration,x0,x1,x2,x3"<<endl;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_vel_x4.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"v(0),v(0.5),v(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Deriv(0,temp);
	temp /= paths[i].durations[j];
	out<<temp[index];
	paths[i].segments[j].Deriv(0.5,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	paths[i].segments[j].Deriv(1,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_acc_x4.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"a(0),a(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Accel(0,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<temp[index];
	paths[i].segments[j].Accel(1,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
  }

  paths.clear();
  if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol*8.0))
    return false;
  for(size_t i=0;i<multipath.sections.size();i++) {
    for(size_t j=0;j<paths[i].segments.size();j++) {
      paths[i].segments[j].space = &cspace;
      paths[i].segments[j].manifold = &cspace;
    }
  }
  for(size_t i=0;i<paths.size();i++) {
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_x8.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"duration,x0,x1,x2,x3"<<endl;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_vel_x8.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"v(0),v(0.5),v(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Deriv(0,temp);
	temp /= paths[i].durations[j];
	out<<temp[index];
	paths[i].segments[j].Deriv(0.5,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	paths[i].segments[j].Deriv(1,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_acc_x8.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"a(0),a(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Accel(0,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<temp[index];
	paths[i].segments[j].Accel(1,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
  }


#endif //SAVE_LORES_INTERPOLATING_CURVES

  //concatenate sections into a single curve
  TimeScaledBezierCurve traj;
  vector<int> edgeToSection,sectionEdges(1,0);
  for(size_t i=0;i<multipath.sections.size();i++) {
    traj.path.Concat(paths[i]);
    for(size_t j=0;j<paths[i].segments.size();j++)
      edgeToSection.push_back((int)i);
    sectionEdges.push_back(sectionEdges.back()+(int)paths[i].segments.size());
  }

  timer.Reset();
  bool res=traj.OptimizeTimeScaling(robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
  if(!res) {
    printf("Failed to optimize time scaling\n");
    return false;
  }
  else {
    printf("Optimized into a path with duration %g, (took %gs)\n",traj.EndTime(),timer.ElapsedTime());
  }
  double T = traj.EndTime();
  int numdivs = (int)Ceil(T/dt);
  printf("Discretizing at time resolution %g\n",T/numdivs);
  numdivs++;

  Vector x,v;
  int sCur = -1;
  for(int i=0;i<numdivs;i++) {
    Real t=T*Real(i)/Real(numdivs-1);
    int trajEdge = traj.timeScaling.TimeToSegment(t);
    if(trajEdge == (int)edgeToSection.size()) trajEdge--; //end of path
    Assert(trajEdge < (int)edgeToSection.size());
    int s=edgeToSection[trajEdge];
    if(s < sCur) {
      fprintf(stderr,"Strange: edge index is going backward? %d -> %d\n",sCur,s);
      fprintf(stderr,"  time %g, division %d, traj segment %d\n",t,i,trajEdge);
    }
    Assert(s - sCur >=0);
    while(sCur < s) {
      //close off the current section and add a new one
      Real switchTime=traj.timeScaling.times[sectionEdges[sCur+1]];
      Assert(switchTime <= t);
      traj.Eval(switchTime,x);
      traj.Deriv(switchTime,v);
      if(sCur >= 0) {
	multipath.sections[sCur].times.push_back(switchTime);
	multipath.sections[sCur].milestones.push_back(x);
	multipath.sections[sCur].velocities.push_back(v);
      }
      multipath.sections[sCur+1].milestones.resize(0);
      multipath.sections[sCur+1].velocities.resize(0);
      multipath.sections[sCur+1].times.resize(0);
      multipath.sections[sCur+1].milestones.push_back(x);
      multipath.sections[sCur+1].velocities.push_back(v);
      multipath.sections[sCur+1].times.push_back(switchTime);
      sCur++;
    }
    if(t == multipath.sections[s].times.back()) continue;
    traj.Eval(t,x);
    traj.Deriv(t,v);
    multipath.sections[s].times.push_back(t);
    multipath.sections[s].milestones.push_back(x);
    multipath.sections[s].velocities.push_back(v);
  }

#if DO_TEST_TRIANGULAR
  timer.Reset();
  TimeScaledBezierCurve trajTri;
  Real Ttrap = 0;
  printf("%d paths?\n",paths.size());
  for(size_t i=0;i<paths.size();i++)
    Ttrap += OptimalTriangularTimeScaling(paths[i],robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax,trajTri);
  printf("Optimal trapezoidal time scaling duration %g, calculated in %gs\n",Ttrap,timer.ElapsedTime());
  printf("Saving plot to opt_tri_multipath.csv\n");
  trajTri.Plot("opt_tri_multipath.csv",robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
#endif // DO_TEST_TRAPEZOIDAL


#if DO_CHECK_BOUNDS  
  CheckBounds(robot,traj,dt);
#endif // DO_CHECK_BOUNDS

#if DO_SAVE_PLOT
  printf("Saving plot to opt_multipath.csv\n");
  traj.Plot("opt_multipath.csv",robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
#endif //DO_SAVE_PLOT

#if DO_SAVE_LIMITS
  SaveLimits(robot,traj,dt,"opt_multipath_limits.csv");
#endif // DO_SAVE_LIMITS

  {
    multipath.settings.set("resolution",xtol);
    multipath.settings.set("program","GenerateAndTimeOptimizeMultiPath");
  }
  return true;
}
Example #8
0
Real OptimalTriangularTimeScaling(const GeneralizedCubicBezierSpline& path,const Vector& vmin,const Vector& vmax,const Vector& amin,const Vector& amax,TimeScaledBezierCurve& traj)
{
  traj.path = path;
  traj.pathSegments.resize(traj.path.durations.size()+1);
  traj.pathSegments[0] = 0;
  for(size_t i=0;i<traj.path.durations.size();i++)
    traj.pathSegments[i+1] = traj.pathSegments[i]+traj.path.durations[i];
  traj.timeScaling.params = traj.pathSegments;
  traj.timeScaling.times.resize(traj.timeScaling.params.size());
  traj.timeScaling.ds.resize(traj.timeScaling.params.size());

  //get T=1 triangular time scaling
  Real pathlen = path.TotalTime();
  Real invpathlen = 1.0/pathlen;
  Real u=0;
  for(size_t i=0;i<=path.segments.size();i++) {
    if(u < 0.5*pathlen) {
      traj.timeScaling.times[i] = Sqrt(0.5*u*invpathlen);
      traj.timeScaling.ds[i] = 4.0*pathlen*traj.timeScaling.times[i];
    }
    else {
      traj.timeScaling.times[i] = 1.0 - Sqrt(0.5 - 0.5*u*invpathlen);
      traj.timeScaling.ds[i] = 4.0*pathlen*(1.0 - traj.timeScaling.times[i]);
    }
    //printf("u: %g / %g, t: %g / 1\n",u,pathlen,traj.timeScaling.times[i]);
    if(i < path.segments.size()) {
      Real du = path.durations[i];
      u += du;
    }
  }

  Real vmaxrel = 0, amaxrel = 0;
  for(size_t i=0;i<path.segments.size();i++) {
    Real du = path.durations[i];
    Vector vimin,vimax,aimin,aimax;
    path.segments[i].GetDerivBounds(vimin,vimax,aimin,aimax);
    vimin /= du;
    vimax /= du;
    aimin /= Sqr(du);
    aimax /= Sqr(du);

    //get the max height of velocity profile, and speed at this point
    Assert(traj.timeScaling.times[i+1] > traj.timeScaling.times[i]);
    Real h = Max(traj.timeScaling.ds[i],traj.timeScaling.ds[i+1]);
    Real dds = (Sqr(traj.timeScaling.ds[i+1])-Sqr(traj.timeScaling.ds[i]))/(2.0*path.durations[i]);
    for(int j=0;j<vmin.n;j++) {
      if (vimin[j]*h < vmaxrel*vmin[j])
	vmaxrel = vimin[j]*h / vmin[j];
      if (vimax[j]*h > vmaxrel*vmax[j])
	vmaxrel = vimax[j]*h / vmax[j];
    }
    if(u <= 0.5*pathlen) {
      //positive acceleration
      for(int j=0;j<vmin.n;j++) {
	if((aimin[j]*Sqr(h) + vimin[j]*dds) < amaxrel*amin[j]) 
	  amaxrel = (aimin[j]*Sqr(h) + vimin[j]*dds)/amin[j];
	if((aimax[j]*Sqr(h) + vimax[j]*dds) > amaxrel*amax[j]) 
	  amaxrel = (aimax[j]*Sqr(h) + vimax[j]*dds)/amax[j];
      }
    }
    if(u+du >= 0.5*pathlen) {
      //negative acceleration
      for(int j=0;j<vmin.n;j++) {
	if((aimin[j]*Sqr(h) - vimin[j]*dds) < amaxrel*amin[j]) 
	  amaxrel = (aimin[j]*Sqr(h) - vimin[j]*dds)/amin[j];
	if((aimax[j]*Sqr(h) - vimax[j]*dds) > amaxrel*amax[j]) 
	  amaxrel = (aimax[j]*Sqr(h) - vimax[j]*dds)/amax[j];

      }
    }
    u += du;
  }
  //can set rate to 1.0/Max(vmaxrel,amaxrel)
  Real maxRate = Max(vmaxrel,amaxrel);
  //duration of ramp up is 2*pathlen/2 / rate, ramp down is the same
  Real T = 2.0*Sqrt(maxRate/4.0);
  printf("Pathlen %g, time %g, max relative velocity %g, acceleration %g\n",pathlen,T,vmaxrel,amaxrel);

  for(size_t i=0;i<=path.durations.size();i++) 
    traj.timeScaling.ds[i] *= 1.0/Sqrt(maxRate);

  for(size_t i=0;i<path.durations.size();i++) {
    Real dt = 2*path.durations[i]/(traj.timeScaling.ds[i]+traj.timeScaling.ds[i+1]);    
    traj.timeScaling.times[i+1]=traj.timeScaling.times[i]+dt;
  }
  printf("Recalculated time %g\n",traj.timeScaling.times.back());
  return T;
}
Example #9
0
static inline int ColorDelta (tRgbColorb& palette, int r, int g, int b)
{
return Sqr (r - palette.red) + Sqr (g - palette.green) + Sqr (b - palette.blue);
}
Example #10
0
int main (int argc, char **argv)
{
    Cmplx *work;
    real *corrSum[3], *corrSumSq[3], damp, deltaT, deltaTCorr,
         omegaMax, tMax, w, x;
    int doFourier, doWindow, j, k, n, nData, nFunCorr, nSet,
        nSetSkip, nv, nValCorr, stepCorr;
    char *bp, *fName, buff[BUFF_LEN];
    FILE *fp;

    n = 1;
    if (-- argc < 1 || ! strcmp (argv[1], "-h")) PrintHelp (argv[0]);
    doFourier = 1;
    doWindow = 0;
    nSetSkip = 1;
    while (-- argc >= 0) {
        if (! strcmp (argv[n], "-t")) doFourier = 0;
        else if (! strcmp (argv[n], "-w")) doWindow = 1;
        else if (! strcmp (argv[n], "-s")) nSetSkip = atoi (argv[n] + 2);
        else {
            fName = argv[n];
            break;
        }
        ++ n;
    }
    if (argc > 0) PrintHelp (argv[0]);
    omegaMax = 10.;
    tMax = 5.;
    if ((fp = fopen (fName, "r")) == 0) {
        printf ("no file\n");
        exit (0);
    }
    while (1) {
        bp = fgets (buff, BUFF_LEN, fp);
        if (*bp == CHAR_MINUS) break;
        NameVal (deltaT);
        NameVal (nFunCorr);
        NameVal (nValCorr);
        NameVal (stepCorr);
    }
    deltaTCorr = stepCorr * deltaT;
    for (j = 0; j < 3; j ++) {
        AllocMem (corrSum[j], nFunCorr * nValCorr, real);
        AllocMem (corrSumSq[j], nFunCorr * nValCorr, real);
        for (n = 0; n < nFunCorr * nValCorr; n ++)
            corrSum[j][n] = corrSumSq[j][n] = 0.;
    }
    AllocMem (work, 2 * (nValCorr - 1), Cmplx);
    nData = 0;
    nSet = 0;
    while (1) {
        if (! (bp = fgets (buff, BUFF_LEN, fp))) break;
        if (! strncmp (bp, txtCorr, strlen (txtCorr))) {
            ++ nSet;
            if (nSet < nSetSkip) continue;
            ++ nData;
            for (j = 0; j < 3; j ++) {
                bp = fgets (buff, BUFF_LEN, fp);
                for (n = 0; n < nValCorr; n ++) {
                    bp = fgets (buff, BUFF_LEN, fp);
                    w = strtod (bp, &bp);
                    for (k = 0; k < nFunCorr; k ++) {
                        w = strtod (bp, &bp);
                        corrSum[j][k * nValCorr + n] += w;
                        corrSumSq[j][k * nValCorr + n] += Sqr (w);
                    }
                }
            }
        }
    }
    fclose (fp);
    printf ("%d\n", nData);
    for (j = 0; j < 3; j ++) {
        for (n = 0; n < nFunCorr * nValCorr; n ++) {
            corrSum[j][n] /= nData;
            corrSumSq[j][n] = sqrt (corrSumSq[j][n] / nData - Sqr (corrSum[j][n]));
        }
    }
    if (doFourier) {
        for (j = 0; j < 3; j ++) {
            for (k = 0; k < nFunCorr; k ++) {
                for (n = 0; n < nValCorr; n ++) {
                    if (doWindow) damp = (nValCorr - n) / (nValCorr + 0.5);
                    else damp = 1.;
                    CSet (work[n], corrSum[j][k * nValCorr + n] * damp, 0.);
                }
                for (n = nValCorr; n < 2 * (nValCorr - 1); n ++)
                    work[n] = work[2 * (nValCorr - 1) - n];
                FftComplex (work, 2 * nValCorr - 2);
                for (n = 0; n < nValCorr; n ++)
                    corrSum[j][k * nValCorr + n] = work[n].R;
            }
        }
        omegaMax = Min (omegaMax, M_PI / deltaTCorr);
        nv = nValCorr * omegaMax / (M_PI / deltaTCorr);
    } else {
        for (j = 0; j < 3; j ++) {
            for (k = 0; k < nFunCorr; k ++) {
                for (n = 1; n < nValCorr; n ++)
                    corrSum[j][k * nValCorr + n] /= corrSum[j][k * nValCorr];
                corrSum[j][k * nValCorr] = 1.;
            }
        }
        tMax = Min (tMax, (nValCorr - 1) * deltaTCorr);
        nv = nValCorr * tMax / ((nValCorr - 1) * deltaTCorr);
    }
    for (j = 0; j < 3; j ++) {
        printf ("%s\n", header[j]);
        for (n = 0; n < nv; n ++) {
            if (doFourier) x = n * omegaMax / nv;
            else x = n * deltaTCorr;
            printf ("%9.4f", x);
            for (k = 0; k < nFunCorr; k ++)
                printf (" %9.4f", corrSum[j][k * nValCorr + n]);
            printf ("\n");
        }
    }
}
Example #11
0
int Torus::Intersect(const BasicRay& ray, DBL *Depth, TraceThreadData *Thread) const
{
    int i, n;
    DBL len, R2, Py2, Dy2, PDy2, k1, k2;
    DBL y1, y2, r1, r2;
    DBL c[5];
    DBL r[4];
    Vector3d P, D;
    DBL DistanceP;            // Distance from P to torus center (origo).
    DBL BoundingSphereRadius; // Sphere fully (amply) enclosing torus.
    DBL Closer;               // P is moved Closer*D closer to torus.

    Thread->Stats()[Ray_Torus_Tests]++;

    /* Transform the ray into the torus space. */

    MInvTransPoint(P, ray.Origin, Trans);

    MInvTransDirection(D, ray.Direction, Trans);

    len = D.length();

    D /= len;

    i = 0;

    y1 = -MinorRadius;
    y2 =  MinorRadius;
    r1 = Sqr(MajorRadius - MinorRadius);

    if ( MajorRadius < MinorRadius )
        r1 = 0;

    r2 = Sqr(MajorRadius + MinorRadius);

#ifdef TORUS_EXTRA_STATS
    Thread->Stats()[Torus_Bound_Tests]++;
#endif

    if (Test_Thick_Cylinder(P, D, y1, y2, r1, r2))
    {
#ifdef TORUS_EXTRA_STATS
        Thread->Stats()[Torus_Bound_Tests_Succeeded]++;
#endif

        // Move P close to bounding sphere to have more precise root calculation.
        // Bounding sphere radius is R + r, we add r once more to ensure
        // that P is safely outside sphere.
        BoundingSphereRadius = MajorRadius + MinorRadius + MinorRadius;
        DistanceP = P.lengthSqr(); // Distance is currently squared.
        Closer = 0.0;
        if (DistanceP > Sqr(BoundingSphereRadius))
        {
            DistanceP = sqrt(DistanceP); // Now real distance.
            Closer = DistanceP - BoundingSphereRadius;
            P += Closer * D;
        }

        R2   = Sqr(MajorRadius);
        r2   = Sqr(MinorRadius);

        Py2  = P[Y] * P[Y];
        Dy2  = D[Y] * D[Y];
        PDy2 = P[Y] * D[Y];

        k1   = P[X] * P[X] + P[Z] * P[Z] + Py2 - R2 - r2;
        k2   = P[X] * D[X] + P[Z] * D[Z] + PDy2;

        c[0] = 1.0;

        c[1] = 4.0 * k2;

        c[2] = 2.0 * (k1 + 2.0 * (k2 * k2 + R2 * Dy2));

        c[3] = 4.0 * (k2 * k1 + 2.0 * R2 * PDy2);

        c[4] = k1 * k1 + 4.0 * R2 * (Py2 - r2);

        n = Solve_Polynomial(4, c, r, Test_Flag(this, STURM_FLAG), ROOT_TOLERANCE, Thread->Stats());

        while(n--)
            Depth[i++] = (r[n] + Closer) / len;
    }

    if (i)
        Thread->Stats()[Ray_Torus_Tests_Succeeded]++;

    return(i);
}