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; }
// 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; }
//--------------------------------------------------------------------------- // 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; }
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"); } }
/* 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*/
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; }
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; }
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); }
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"); } } }
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); }