bool ConstrainedInterpolator::Make(const Config& qa,const Config& qb,vector<Config>& path,bool checkConstraints) { Vector temp(constraint->NumDimensions()); ConstraintValue(qa,temp); if(temp.maxAbsElement() > ftol) { fprintf(stderr,"ConstrainedInterpolator: Warning, initial point a is not on manifold, error %g\n",temp.maxAbsElement()); } ConstraintValue(qb,temp); if(temp.maxAbsElement() > ftol) { fprintf(stderr,"ConstrainedInterpolator: Warning, initial point b is not on manifold, error %g\n",temp.maxAbsElement()); } list<Config> lpath; lpath.push_back(qa); lpath.push_back(qb); priority_queue<Segment,vector<Segment> > q; Segment s; s.prev = lpath.begin(); s.length = Distance(space,qa,qb); q.push(s); Config x; while(!q.empty()) { s=q.top(); q.pop(); if(s.length <= xtol) continue; list<Config>::iterator a = s.prev; list<Config>::iterator b=a; b++; Interpolate(space,*a,*b,0.5,x); if(!Project(x)) { cout<<"Unable to project "<<x<<endl; cout<<"Midpoint "<<*a<<" -> "<<*b<<endl; return false; } if(checkConstraints && space && !space->IsFeasible(x)) { cout<<"Infeasible configuration "<<x<<endl; return false; } list<Config>::iterator m=lpath.insert(b,x); //insert the split segments back in the queue Real l1=Distance(space,*a,x); Real l2=Distance(space,x,*b); if(l1 > 0.5*(1+maxGrowth)*s.length) { cout<<"Excessive growth: "<<l1<<" > "<<0.5*(1+maxGrowth)*s.length<<endl; return false; } if(l2 > 0.5*(1+maxGrowth)*s.length) { cout<<"Excessive growth: "<<l2<<" > "<<0.5*(1+maxGrowth)*s.length<<endl; return false; } s.prev = a; s.length = l1; if(s.length > xtol) q.push(s); s.prev = m; s.length = l2; if(s.length > xtol) q.push(s); } //read out the path path.resize(lpath.size()); size_t k=0; for(list<Config>::iterator i=lpath.begin();i!=lpath.end();i++,k++) { path[k] = *i; } return true; }
bool SmoothConstrainedInterpolator::Make(const Config& qa,const Vector& da,const Config& qb,const Vector& db, GeneralizedCubicBezierSpline& path, bool checkConstraints) { Vector temp(constraint->NumDimensions()); ConstraintValue(qa,temp); if(temp.maxAbsElement() > ftol) { fprintf(stderr,"ConstrainedInterpolator: Warning, initial point a is not on manifold, error %g\n",temp.maxAbsElement()); } ConstraintValue(qb,temp); if(temp.maxAbsElement() > ftol) { fprintf(stderr,"ConstrainedInterpolator: Warning, initial point b is not on manifold, error %g\n",temp.maxAbsElement()); } Vector da2=da,db2=db; if(!da.empty()) { if(!ProjectVelocity(qa,da2)) { fprintf(stderr,"ConstrainedInterpolator: Warning, initial velocity a could not be projected\n"); da2.setZero(); } } if(!db.empty()) { if(!ProjectVelocity(qb,db2)) { fprintf(stderr,"ConstrainedInterpolator: Warning, initial velocity b could not be projected\n"); db2.setZero(); } } const static double third = 1.0/3.0; const static double sixth = 1.0/6.0; list<pair<GeneralizedCubicBezierCurve,double> > lpath; GeneralizedCubicBezierCurve curve(space); curve.x0 = qa; curve.x3 = qb; curve.SetNaturalTangents(da2,db2); bool redo = (da2.empty() || db2.empty()); if(da2.empty()) { curve.Deriv(0,da2); if(!ProjectVelocity(qa,da2)) { fprintf(stderr,"ConstrainedInterpolator: Warning, start velocity a could not be projected\n"); da2.setZero(); } } if(db2.empty()) { curve.Deriv(1,db2); if(!ProjectVelocity(qb,db2)) { fprintf(stderr,"ConstrainedInterpolator: Warning, end velocity b could not be projected\n"); db2.setZero(); } } if(redo) curve.SetNaturalTangents(da2,db2); #if CONDITION_INITIAL_TANGENTS ConditionTangents(curve); #endif lpath.push_back(pair<GeneralizedCubicBezierCurve,double>(curve,1.0)); priority_queue<Segment2,vector<Segment2> > q; Segment2 s; s.prev = lpath.begin(); s.length = curve.OuterLength(); q.push(s); GeneralizedCubicBezierCurve c1(space),c2(space); Config x,v; while(!q.empty()) { s=q.top(); q.pop(); if(s.length <= xtol) continue; list<pair<GeneralizedCubicBezierCurve,double> >::iterator c = s.prev; list<pair<GeneralizedCubicBezierCurve,double> >::iterator n = c; ++n; /* //optimize end tangents to release tension Config* prev=NULL, *next=NULL; Real prevdur = 1.0, nextdur = 1.0; if(c != lpath.begin()) { list<pair<GeneralizedCubicBezierCurve,double> >::iterator p = c; --p; prev = &p->first.x0; prevdur = p->second; } if(n != lpath.end()) { next = &n->first.x3; nextdur = n->second; } c->first.SetSmoothTangents(prev,next,prevdur/c->second,nextdur/c->second); Vector v1,v2; c->first.Deriv(0,v1); c->first.Deriv(1,v2); ProjectVelocity(c->first.x0,v1); ProjectVelocity(c->first.x3,v2); c->first.SetNaturalTangents(v1,v2); */ //c->first.Eval(0.5,x); c->first.Midpoint(x); //cout<<"Depth: "<<c->second<<endl; //cout<<"Bspline midpoint: "<<x<<", "<<v<<endl; //cout<<"Original point :"<<x<<endl; if(!Project(x)) { ConstraintValue(x,temp); if(gConstrainedInterpolateVerbose) cout<<"Projection of point "<<x<<" failed, "<<" error "<<temp.maxAbsElement()<<endl; return false; } //cout<<"Projected midpoint: "<<x<<", "<<v<<endl; //getchar(); #if OPTIMIZE_TANGENTS if(space) FatalError("Can't optimize tangents with a manifold"); //scale the tangents of the curve so that the midpoint gets closer to x //xmid = (x0/8+3/8 x1 + 3/8 x2 + x3/8) + 3/8 (alpha (x1-x0) - beta (x3-x2)) //Solve least squares //cout<<"Projected point :"<<x<<endl; Vector t1=c->first.x1-c->first.x0,t2=c->first.x3-c->first.x2; Vector xmid = (c->first.x0+c->first.x3)*0.5 + 3.0/8.0*(t1-t2); Vector rhs = (x - xmid)*8.0*third; Real origDist = xmid.distance(x); //cout<<"Xmid "<<xmid<<endl; Vector2 Atb(dot(rhs,t1),-dot(rhs,t2)); Matrix2 AtA; AtA(0,0) = dot(t1,t1) + 1e-1*s.length; AtA(0,1) = AtA(1,0) = -dot(t1,t2); AtA(1,1) = dot(t2,t2) + 1e-1*s.length; //cout<<"AtA: "<<AtA<<endl; bool res = AtA.inplaceInverse(); if(res) { Vector2 alphabeta = AtA*Atb; //cout<<"Scaling: "<<alphabeta<<endl; if(space) { space->Integrate(c->first.x0,(1.0+alphabeta.x)*t1,c->first.x1); space->Integrate(c->first.x3,-(1.0+alphabeta.y)*t2,c->first.x2); } else { c->first.x1 = c->first.x0 + (1.0+alphabeta.x)*t1; c->first.x2 = c->first.x3 - (1.0+alphabeta.y)*t2; } duration1 *= (1.0+alphabeta.x); duration2 *= (1.0+alphabeta.y); c->first.Eval(0.5,rhs); Real newDist = rhs.distance(x); //Assert(newDist <= origDist+Epsilon); //cout<<"Result: "<<rhs<<endl; //getchar(); } #endif // OPTIMIZE_TANGENTS if(checkConstraints && space && !space->IsFeasible(x)) return false; //c->first.Deriv(0.5,v); c->first.MidpointDeriv(v); ProjectVelocity(x,v); //subdivide, insert the split segments into the queue c1.x0 = c->first.x0; if(space) { space->Interpolate(c->first.x0,c->first.x1,0.5,c1.x1); space->Integrate(x,v*(-sixth),c1.x2); } else { interpolate(c->first.x0,c->first.x1,0.5,c1.x1); c1.x2 = x-v*sixth; } c1.x3 = x; c2.x0 = x; if(space) { space->Integrate(x,v*sixth,c2.x1); } else { c2.x1 = x+v*sixth; } Interpolate(space,c->first.x2,c->first.x3,0.5,c2.x2); c2.x3 = c->first.x3; //sanity check /* Vector temp; c1.Deriv(0,temp); if(!temp.isEqual((c->first.x1-c->first.x0)*3.0*0.5,1e-4)) { cout<<"Invalid starting derivative of subdivision 1"<<endl; cout<<temp<<" vs "<<(c->first.x1-c->first.x0)*3.0*0.5<<endl; getchar(); } c1.Deriv(1,temp); if(!temp.isEqual(v*0.5,1e-4)) { cout<<"Invalid ending derivative of subdivision 1"<<endl; cout<<temp<<" vs "<<v*0.5<<endl; getchar(); } c2.Deriv(0,temp); if(!temp.isEqual(v*0.5,1e-4)) { cout<<"Invalid starting derivative of subdivision 2"<<endl; cout<<temp<<" vs "<<v*0.5<<endl; getchar(); } c2.Deriv(1,temp); if(!temp.isEqual((c->first.x3-c->first.x2)*3.0*0.5,1e-4)) { cout<<"Invalid ending derivative of subdivision 2"<<endl; cout<<temp<<" vs "<<(c->first.x3-c->first.x2)*3.0*0.5<<endl; getchar(); } */ #if CONDITION_MIDDLE_TANGENTS ConditionMiddleTangent(c1,c2); #endif // CONDITION_MIDDLE_TANGENTS Real l1=c1.OuterLength(); Real l2=c2.OuterLength(); if(l1 > 0.5*(1.0+maxGrowth)*s.length) { if(gConstrainedInterpolateVerbose) { cout<<"Projection exceeded growth factor: "; cout<<l1<<" > "<<0.5*(1.0+maxGrowth)*s.length<<endl; } /* cout<<c->first.x0<<", "<<c->first.x1<<", "<<c->first.x2<<", "<<c->first.x3<<endl; c->first.Midpoint(x); c->first.MidpointDeriv(v); cout<<"Midpoint: "<<x<<", deriv "<<v<<endl; Project(x); ProjectVelocity(x,v); cout<<"Projected midpoint: "<<x<<", deriv "<<v<<endl; */ //getchar(); return false; } if(l2 > 0.5*(1.0+maxGrowth)*s.length) { if(gConstrainedInterpolateVerbose) { cout<<"Projection exceeded growth factor: "; cout<<l2<<" > "<<0.5*(1.0+maxGrowth)*s.length<<endl; } /* cout<<c->first.x0<<", "<<c->first.x1<<", "<<c->first.x2<<", "<<c->first.x3<<endl; c->first.Midpoint(x); c->first.MidpointDeriv(v); cout<<"Midpoint: "<<x<<", deriv "<<v<<endl; Project(x); ProjectVelocity(x,v); cout<<"Projected midpoint: "<<x<<", deriv "<<v<<endl; */ return false; } //need to scale previous and next durations by 0.5 Real origDuration = c->second; c->first = c1; c->second = 0.5*origDuration; list<pair<GeneralizedCubicBezierCurve,double> >::iterator m=lpath.insert(n,pair<GeneralizedCubicBezierCurve,double>(c2,0.5*origDuration)); s.prev = c; s.length = l1; if(s.length > xtol) q.push(s); s.prev = m; s.length = l2; if(s.length > xtol) q.push(s); } //read out the path path.segments.resize(lpath.size()); path.durations.resize(lpath.size()); size_t k=0; for(list<pair<GeneralizedCubicBezierCurve,double> >::iterator i=lpath.begin();i!=lpath.end();i++,k++) { path.segments[k] = i->first; path.durations[k] = i->second; } return true; }
//______________________________________________________________________________ Int_t ROMEXMLDataBase::SearchTable(ROMEPath *path,ROMEStr2DArray *values,const char* dataBasePath, Long64_t runNumber,Long64_t eventNumber,Bool_t write) { int i,j; ROMEString value; ROMEString val; ROMEString id; ROMEString xmlPath; bool handleArray = false; ROMEString basePath; char *cstop; int nArrayTables = -1; ROMEString ConstraintPath; ROMEString ConstraintTable; ROMEString ConstraintField; ROMEStr2DArray ConstraintValue(1,1); ROMEStrArray valueArr; fOrderTableIndex = -1; fPointerArray.RemoveAll(); // decode path if (!path->Decode(dataBasePath,runNumber,eventNumber)) { ROMEPrint::Error("\nPath decode error : %s\n", dataBasePath); return 0; } // path->Print(); // search through the tables for (i=0;i<path->GetNumberOfTables();i++) { valueArr.RemoveAll(); // open file fFileName = fDirectoryPath; fFileName += "/"; fFileName += path->GetTableNameAt(i); fFileName += ".xml"; if (!xml->OpenFileForPath(fFileName.Data())) { ROMEPrint::Error("\nFailed to load xml database file : '%s'\n", fFileName.Data()); return 0; } // create path xmlPath = "/"; xmlPath += fDataBaseName; xmlPath += "/"; xmlPath += path->GetTableNameAt(i); // add constraint if(strlen(path->GetTableConstraintAt(i))){ int itmp; if (!path->DecodeConstraint(path->GetTableConstraintAt(i))) { return 0; } for(j=0;j<path->GetNumberOfConstraints();j++){ xmlPath += "["; xmlPath += path->GetConstraintFieldAt(j); ConstraintTable = path->GetConstraintValueAt(j); if ((itmp=ConstraintTable.Index(".",1,0,TString::kExact))!=-1) { ConstraintField = ConstraintTable(itmp,ConstraintTable.Length()); ConstraintTable = ConstraintTable(0,itmp); ConstraintTable.ReplaceAll(" ",""); path->GetAbsolutePath(ConstraintPath,ConstraintTable); ConstraintPath += "/"; ConstraintPath += ConstraintField(1,ConstraintField.Length()); Read(&ConstraintValue,ConstraintPath.Data(),runNumber,eventNumber); xmlPath += "="; xmlPath += ConstraintValue.At(0,0).Data(); } else { xmlPath += "="; xmlPath += path->GetConstraintValueAt(j); } xmlPath += "]"; } } if (i>0) { if (strlen(path->GetTableIDNameAt(i-1))>0) { xmlPath += "["; xmlPath += path->GetTableIDNameAt(i-1); xmlPath += "="; xmlPath += id.Data(); xmlPath += "]"; } } xmlPath += "/"; // Data base constraint if (strlen(path->GetTableDBConstraintAt(i))>0) { basePath = xmlPath; int istart,iend; ROMEString newDataBasePath; ROMEString dbPath = dataBasePath; // build new path if ((istart=dbPath.Index("[@",2,0,TString::kExact))==-1) { ROMEPrint::Error("\nData base constraint statment not found : %s\n", dataBasePath); return 0; } if ((iend=dbPath.Index("]",1,istart,TString::kExact))==-1) { ROMEPrint::Error("\nData base constraint statment not closed : %s\n", dataBasePath); return 0; } newDataBasePath = dbPath(0,istart); newDataBasePath += dbPath(iend+1,dbPath.Length()-iend-1); // read data base constraint xmlPath += path->GetTableDBConstraintAt(i); if (!xml->GetPathValue(xmlPath,value)) { ROMEPrint::Error("\nWrong path for data base constraint : %s\n", xmlPath.Data()); return 0; } ROMEString temp = path->GetTableNameAt(i+1); temp += "=\""; int is,ie; // constraint is a path if ((is=value.Index(temp,temp.Length(),0,TString::kIgnoreCase))!=-1) { if ((ie=value.Index("\"",1,is+temp.Length(),TString::kIgnoreCase))!=-1) { value = value(is+temp.Length(),ie-is-temp.Length()); if(value(value.Length()-1)=='/') { value.Remove(value.Length()-1,value.Length()); } while ((is=value.Index("(@@",3,0,TString::kIgnoreCase))!=-1) { if ((ie=value.Index(")",1,is+3,TString::kIgnoreCase))==-1) { ie = value.Length(); } val = value(is+3,ie-is-3); path->GetAbsolutePath(ConstraintPath,path->GetTableNameAt(i)); ConstraintPath += "/"; ConstraintPath += val; Read(&ConstraintValue,ConstraintPath.Data(),runNumber,eventNumber); value.Remove(is,ie-is+1); value.Insert(is,ConstraintValue.At(0,0)); } newDataBasePath.Insert(istart, value.Data()); // decode new path if (write) { if (Write(values,newDataBasePath.Data(),runNumber,eventNumber)) { return -1; } } else { if (Read(values,newDataBasePath.Data(),runNumber,eventNumber)) { return -1; } } return 0; } } // constraint is not a path xmlPath = basePath; xmlPath += "["; xmlPath += value; xmlPath += "]"; } // next table fXMLBase = xmlPath; id.Resize(0); if (strlen(path->GetTableIDNameAt(i))>0&&i<path->GetNumberOfTables()-1) { xmlPath += path->GetTableNameAt(i+1); xmlPath += "_"; xmlPath += path->GetTableIDNameAt(i); if (!xml->GetPathValue(xmlPath,id)) { ROMEPrint::Error("\nWrong data base path : %s\n", xmlPath.Data()); return 0; } } if (path->IsOrderArray()&&!strcmp(path->GetTableNameAt(i),path->GetOrderTableName())) { handleArray = true; fOrderTableIndex = i; } if (handleArray) { nArrayTables++; fIDX.RemoveAll(); xmlPath = fXMLBase; if (!strcmp(path->GetTableNameAt(i),path->GetOrderTableName())) { xmlPath += path->GetOrderFieldName(); } else { xmlPath += path->GetTableIDXNameAt(i-1); } if (!xml->GetPathValues(xmlPath,&fIDX)) { ROMEPrint::Error("\nWrong data base path : %s\n", xmlPath.Data()); return 0; } xmlPath = fXMLBase; // Table pointers if (i<path->GetNumberOfTables()-1) { xmlPath += path->GetTableNameAt(i+1); xmlPath += "_"; if (!strcmp(path->GetTableNameAt(i),path->GetOrderTableName())) { xmlPath += path->GetOrderFieldName(); } else { xmlPath += path->GetTableIDXNameAt(i-1); } if (!xml->GetPathValues(xmlPath,&valueArr)) { ROMEPrint::Error("\nWrong data base path : %s\n",xmlPath.Data()); return 0; } if (valueArr.GetEntriesFast()!=fIDX.GetEntriesFast()) { ROMEPrint::Error("\nInvalid Table : %s\n", path->GetTableNameAt(i)); return 0; } for (j=0;j<valueArr.GetEntriesFast();j++) { int idxValue = strtol(fIDX.At(j).Data(),&cstop,10); if (idxValue>=0) { fPointerArray.SetAt(valueArr.At(j),nArrayTables,idxValue); } } } else { // Field : Array Folder return 2; } } else { if (i==path->GetNumberOfTables()-1) { // Field : Single Folder return 1; } } } return 2; }