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;
}
