PhysicsInterface::JointObject Bullet::createHingeJoint(BodyObject firstBodyObject, BodyObject secondBodyObject,
const Vec3& globalAnchor, const Vec3& globalAxis)
{
if (!firstBodyObject || !secondBodyObject || firstBodyObject == secondBodyObject)
{
LOG_ERROR << "Invalid bodies";
return nullptr;
}
auto firstBody = reinterpret_cast<Body*>(firstBodyObject);
auto secondBody = reinterpret_cast<Body*>(secondBodyObject);
auto firstBodyTransform = btTransform();
auto secondBodyTransform = btTransform();
firstBody->bulletBody->getMotionState()->getWorldTransform(firstBodyTransform);
secondBody->bulletBody->getMotionState()->getWorldTransform(secondBodyTransform);
auto btConstraint = new btHingeConstraint(*firstBody->bulletBody, *secondBody->bulletBody,
firstBodyTransform.inverse() * toBullet(globalAnchor),
secondBodyTransform.inverse() * toBullet(globalAnchor),
firstBodyTransform.getBasis().inverse() * toBullet(globalAxis),
secondBodyTransform.getBasis().inverse() * toBullet(globalAxis));
joints_.append(new Joint(firstBody, secondBody, btConstraint));
dynamicsWorld_->addConstraint(joints_.back()->bulletConstraint, true);
return joints_.back();
}
Pose6d Pose6d::operator * ( Pose6d pose2) const
{
tf::Matrix3x3 R1 = getBasis();
tf::Matrix3x3 R2 = pose2.getBasis();
tf::Vector3 T1 = getOrigin();
tf::Vector3 T2 = pose2.getOrigin();
tf::Matrix3x3 R3;
R3[0][0] = R1[0][0] * R2[0][0] + R1[0][1]*R2[1][0] + R1[0][2]*R2[2][0];
R3[1][0] = R1[1][0] * R2[0][0] + R1[1][1]*R2[1][0] + R1[1][2]*R2[2][0];
R3[2][0] = R1[2][0] * R2[0][0] + R1[2][1]*R2[1][0] + R1[2][2]*R2[2][0];
R3[0][1] = R1[0][0] * R2[0][1] + R1[0][1]*R2[1][1] + R1[0][2]*R2[2][1];
R3[1][1] = R1[1][0] * R2[0][1] + R1[1][1]*R2[1][1] + R1[1][2]*R2[2][1];
R3[2][1] = R1[2][0] * R2[0][1] + R1[2][1]*R2[1][1] + R1[2][2]*R2[2][1];
R3[0][2] = R1[0][0] * R2[0][2] + R1[0][1]*R2[1][2] + R1[0][2]*R2[2][2];
R3[1][2] = R1[1][0] * R2[0][2] + R1[1][1]*R2[1][2] + R1[1][2]*R2[2][2];
R3[2][2] = R1[2][0] * R2[0][2] + R1[2][1]*R2[1][2] + R1[2][2]*R2[2][2];
double tempx, tempy, tempz;
tempx = R1[0][0] * T2.x() + R1[0][1]*T2.y() + R1[0][2]*T2.z() + T1.x();
tempy = R1[1][0] * T2.x() + R1[1][1]*T2.y() + R1[1][2]*T2.z() + T1.y();
tempz = R1[2][0] * T2.x() + R1[2][1]*T2.y() + R1[2][2]*T2.z() + T1.z();
tf::Vector3 T3(tempx, tempy, tempz);
Pose6d pose;
pose.setBasis(R3);
pose.setOrigin(T3);
return(pose);
}
void
State::print( std::ostream &out, bool basisOnly ) const
{
Config *cfp;
out << "State " << getStateIndex() << ":\n";
if ( basisOnly )
cfp = getBasis();
else
cfp = getConfig();
while ( cfp )
{
cfp->print( out );
out << std::endl;
if ( basisOnly )
cfp = cfp->getNextBasis();
else
cfp = cfp->getNext();
}
out << std::endl;
out << " Actions:" << std::endl;
for ( size_t i = 0; i < myActions.getNumActions(); ++i )
{
const Action &act = myActions.getNthAction( i );
if ( act.print( out ) )
out << std::endl;
}
}
Pose6d Pose6d::getInverse() const
{
double newx,newy,newz;
tf::Matrix3x3 R = getBasis();
newx =-( R[0][0] * x + R[1][0] * y + R[2][0] * z);
newy = -(R[0][1] * x + R[1][1] * y + R[2][1] * z);
newz = -(R[0][2] * x + R[1][2] * y + R[2][2] * z);
Pose6d new_pose(newx, newy, newz, -ax, -ay, -az);
return(new_pose);
}
// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void InsertAtoms::readFilterParameters(AbstractFilterParametersReader* reader, int index)
{
reader->openFilterGroup(this, index);
setVertexDataContainerName(reader->readString("VertexDataContainerName", getVertexDataContainerName() ) );
setVertexAttributeMatrixName(reader->readString("VertexAttributeMatrixName", getVertexAttributeMatrixName() ) );
setAtomFeatureLabelsArrayName(reader->readString("AtomFeatureLabelsArrayName", getAtomFeatureLabelsArrayName() ) );
setAvgQuatsArrayPath(reader->readDataArrayPath("AvgQuatsArrayPath", getAvgQuatsArrayPath() ) );
setSurfaceMeshFaceLabelsArrayPath(reader->readDataArrayPath("SurfaceMeshFaceLabelsArrayPath", getSurfaceMeshFaceLabelsArrayPath() ) );
setLatticeConstants( reader->readFloatVec3("LatticeConstants", getLatticeConstants() ) );
setBasis( reader->readValue("Basis", getBasis() ) );
reader->closeFilterGroup();
}
void CbcSolverLongThin::initialSolve()
{
modelPtr_->scaling(0);
setBasis(basis_,modelPtr_);
modelPtr_->dual();
basis_ = getBasis(modelPtr_);
assert(!modelPtr_->specialOptions());
modelPtr_->setLogLevel(0);
if (!tryCut) {
tryCut = new CglDuplicateRow(this);
tryCut->setLogLevel(2);
}
}
//#############################################################################
// Solve methods
//#############################################################################
void CbcSolver2::initialSolve()
{
modelPtr_->scaling(0);
setBasis(basis_,modelPtr_);
// Do long thin by sprint
ClpSolve options;
options.setSolveType(ClpSolve::usePrimalorSprint);
options.setPresolveType(ClpSolve::presolveOff);
options.setSpecialOption(1,3,30);
modelPtr_->initialSolve(options);
basis_ = getBasis(modelPtr_);
modelPtr_->setLogLevel(0);
}
// mesh = m, dir = vector(1, 0, 0)
void Intersector::init()
{
int i, j, k;
const vector<MeshVertex> &vtc = mesh->vertices;
const vector<MeshEdge> &edg = mesh->edges;
dir = dir.normalize();
getBasis(dir, v1, v2);
// 建立一个以dir, v1, v2为坐标轴的坐标系
// dir(1, 0, 0) v1(0, 0, 1) v2(0, -1, 0)
points.resize(vtc.size());
sNormals.resize(edg.size() / 3);
for(i = 0; i < (int)vtc.size(); ++i) {
// *表示内积
points[i] = Vector2(vtc[i].pos * v1, vtc[i].pos * v2);
}
bounds = Rect2(points.begin(), points.end());
// points数组存储的是每个顶点在v1和v2方向上的投影，bounds是这些投影的二维包围盒
triangles.resize(cells * cells);
for(i = 0; i < (int)edg.size(); i += 3) {
Rect2 triRect;
for(j = 0; j < 3; ++j)
triRect |= Rect2(points[edg[i + j].vertex]);
int fromx, fromy, tox, toy;
getIndex(triRect.getLo(), fromx, fromy);
getIndex(triRect.getHi(), tox, toy);
// 得到的fromx, fromy分别是将bounds分为200*200的网格后，其包围盒左下角在整个网格(bounds)中的x,y方向上的网格索引号
// tox, toy是三角形包围盒右上角在网格中的索引号
for(j = fromy; j <= toy; ++j) for(k = fromx; k <= tox; ++k) {
triangles[j * cells + k].push_back(i);
}
// triangles中存储的是与这些网格有交点的三角面片
Vector3 cross = (vtc[edg[i + 1].vertex].pos - vtc[edg[i].vertex].pos) % (vtc[edg[i + 2].vertex].pos - vtc[edg[i].vertex].pos);
j = i / 3;
// j为面片的索引号
sNormals[j] = cross.normalize();
if(fabs(sNormals[j] * dir) <= 1e-8)
sNormals[j] = Vector3(); //zero if coplanar
else
sNormals[j] = sNormals[j] / (sNormals[j] * dir); //prescaled for intersection
// sNormals[j]为平面的法向在dir方向上的投影
}
}
//-----------------------------------------------------------------------------
void CbcSolverLongThin::resolve()
{
int * whichRow = NULL;
int * whichColumn = NULL;
// problem may be small enough to do nested search
const double * colLower = modelPtr_->columnLower();
const double * colUpper = modelPtr_->columnUpper();
int numberIntegers = model_->numberIntegers();
const int * integerVariable = model_->integerVariable();
int numberRows=modelPtr_->numberRows();
int numberColumns = modelPtr_->numberColumns();
int i;
int nFix=0;
int nNewRow=0;
int nNewCol=0;
int sizeDynamic = COIN_INT_MAX;
int smallOriginalNumberRows=0;
if (algorithm_==0) {
for (i=0;i<numberIntegers;i++) {
int iColumn=integerVariable[i];
if (colLower[iColumn]==colUpper[iColumn])
nFix++;
}
} else {
whichRow = new int[numberRows];
whichColumn = new int [numberColumns];
// more sophisticated
OsiCuts cs;
tryCut->generateCuts(*this,cs);
int numberCuts = cs.sizeColCuts();
if (numberCuts) {
for ( i = 0 ; i < numberCuts ; i++) {
const OsiColCut *thisCut = cs.colCutPtr(i) ;
const CoinPackedVector & ubs = thisCut->ubs() ;
int n = ubs.getNumElements() ;
const int * which = ubs.getIndices() ;
const double * values = ubs.getElements() ;
for (int j = 0;j<n;j++) {
int iColumn = which[j] ;
this->setColUpper(iColumn,values[j]) ;
}
}
}
#if 1
const int * duplicate = tryCut->duplicate();
sizeDynamic = tryCut->sizeDynamic();
int nOrig = tryCut->numberOriginalRows();
for (i=0;i<nOrig;i++) {
if (duplicate[i]==-1)
whichRow[nNewRow++]=i;
else
modelPtr_->setRowStatus(i,ClpSimplex::basic);
}
smallOriginalNumberRows=nNewRow;
for (;i<numberRows;i++) {
whichRow[nNewRow++]=i;
}
#else
for (i=0;i<numberRows;i++)
whichRow[i]=i;
nNewRow=numberRows;
#endif
for (i=0;i<numberIntegers;i++) {
int iColumn=integerVariable[i];
if (colLower[iColumn]==colUpper[iColumn])
nFix++;
bool choose;
if (algorithm_==1)
choose = true;
else
choose = (node_[i]>count_-memory_&&node_[i]>0);
if ((choose&&colUpper[i])
||(modelPtr_->getStatus(i)!=ClpSimplex::atLowerBound&&
modelPtr_->getStatus(i)!=ClpSimplex::isFixed)
||colLower[i]>0.0)
whichColumn[nNewCol++]=i;
}
}
if (nestedSearch_<1.0&&model_&&model_->phase()==2) {
if (((double) sizeDynamic)*((double) nNewCol)<1000000000&&sizeDynamic<10000000) {
// could do Dynamic Programming
// back to original number of rows
nNewRow = smallOriginalNumberRows;
// and get rid of any basics
int nNewCol=0;
for (i=0;i<numberColumns;i++) {
if (colUpper[i]||colLower[i]>0.0)
whichColumn[nNewCol++]=i;
}
ClpSimplex temp(modelPtr_,nNewRow,whichRow,nNewCol,whichColumn);
int returnCode;
double * rowLower2 = temp.rowLower();
double * rowUpper2 = temp.rowUpper();
int numberColumns2 = temp.numberColumns();
double * colLower2 = temp.columnLower();
double * colUpper2 = temp.columnUpper();
const CoinPackedMatrix * matrix = temp.matrix();
const double * element = matrix->getElements();
const int * row = matrix->getIndices();
const CoinBigIndex * columnStart = matrix->getVectorStarts();
const int * columnLength = matrix->getVectorLengths();
double offset=0.0;
const double * objective = temp.objective();
bool feasible=true;
for (i=0;i<numberColumns2;i++) {
double value = colLower2[i];
if (value) {
offset += value*objective[i];
colLower2[i]=0.0;
colUpper2[i] -= value;
for (int j=columnStart[i];
j<columnStart[i]+columnLength[i];j++) {
int iRow=row[j];
rowLower2[iRow] -= value*element[j];
rowUpper2[iRow] -= value*element[j];
if (rowUpper2[iRow]<-1.0e-8) {
feasible=false;
printf("odd - problem is infeasible\n");
}
}
}
}
temp.setObjectiveOffset(-offset);
OsiClpSolverInterface temp2(&temp);
double * solutionDP = NULL;
if (feasible) {
for (i=0;i<numberColumns2;i++)
temp2.setInteger(i);
CbcModel modelSmall(temp2);
modelSmall.messageHandler()->setLogLevel(0);
CbcFathomDynamicProgramming fathom1(modelSmall);
// Set maximum space allowed
fathom1.setMaximumSize(100000000);
temp2.writeMps("small");
returnCode=fathom1.fathom(solutionDP);
if (returnCode!=1) {
printf("probably not enough memory\n");
abort();
}
}
if (solutionDP) {
double objValue = 0.0;
double * solution = modelPtr_->primalColumnSolution();
const double * objective = modelPtr_->objective();
for (i=0;i<numberColumns;i++)
solution[i]=colLower[i];
for (i=0;i<nNewCol;i++) {
int iColumn = whichColumn[i];
solution[iColumn]+=solutionDP[i];
}
for (i=0;i<numberColumns;i++)
objValue += solution[i]*objective[i];
if (objValue<model_->getCutoff()) {
printf("good solution %g by dynamic programming\n",objValue);
returnCode = 0;
// paranoid check
double * rowLower = modelPtr_->rowLower();
double * rowUpper = modelPtr_->rowUpper();
// Column copy
const CoinPackedMatrix * matrix2 = modelPtr_->matrix();
element = matrix2->getElements();
row = matrix2->getIndices();
columnStart = matrix2->getVectorStarts();
columnLength = matrix2->getVectorLengths();
double * rowActivity = new double [numberRows];
memset(rowActivity,0,numberRows*sizeof(double));
for (i=0;i<numberColumns;i++) {
int j;
double value = solution[i];
assert (value>=colLower[i]&&value<=colUpper[i]);
if (value) {
printf("%d has value %g\n",i,value);
for (j=columnStart[i];
j<columnStart[i]+columnLength[i];j++) {
int iRow=row[j];
rowActivity[iRow] += value*element[j];
}
}
}
// check was feasible
bool feasible=true;
for (i=0;i<numberRows;i++) {
if(rowActivity[i]<rowLower[i]) {
if (rowActivity[i]<rowLower[i]-1.0e-8)
feasible = false;
} else if(rowActivity[i]>rowUpper[i]) {
if (rowActivity[i]>rowUpper[i]+1.0e-8)
feasible = false;
}
}
if (!feasible) {
printf("** Bad solution by dynamic programming\n");
abort();
}
delete [] rowActivity;
model_->setBestSolution(CBC_TREE_SOL,objValue,solution);
} else {
returnCode=2;
}
} else {
returnCode=2;
}
temp2.releaseClp();
modelPtr_->setProblemStatus(1);
delete [] whichRow;
delete [] whichColumn;
return;
}
if (nFix>nestedSearch_*numberIntegers) {
// Do nested search
// back to original number of rows
nNewRow = smallOriginalNumberRows;
// and get rid of any basics
int nNewCol=0;
for (i=0;i<numberColumns;i++) {
if (colUpper[i]||colLower[i]>0.0)
whichColumn[nNewCol++]=i;
}
#if 0
// We clone from continuous solver so set some stuff
OsiSolverInterface * solver = model_->continuousSolver();
CbcSolverLongThin * osiclp = dynamic_cast< CbcSolverLongThin*> (solver);
assert (osiclp);
// up special options
if (osiclp->specialOptions()==3)
osiclp->setSpecialOptions(7);
double saveNested = osiclp->getNested();
int saveAlgorithm = osiclp->getAlgorithm();
osiclp->setNested(1.0);
osiclp->setAlgorithm(0);
int numberObjects = model_->numberObjects();
if (numberObjects>model_->numberIntegers()) {
// for now only integers
//assert (numberObjects == model_->numberIntegers()+1);
model_->setNumberObjects(model_->numberIntegers());
// try follow on
//model_->setNumberObjects(model_->numberIntegers()+1);
}
double saveMaxTime = model_->getDblParam(CbcModel::CbcMaximumSeconds);
model_->setDblParam(CbcModel::CbcMaximumSeconds,1.0e5);
// up special options
#if 1
int returnCode= model_->subBranchAndBound(colLower,colUpper,2000);
#else
CbcModel * model3 = model_->cleanModel(colLower,colUpper);
// integer presolve
int returnCode=0;
CbcModel * model2 = model3->integerPresolve(false);
if (!model2||!model2->getNumRows()) {
delete model2;
delete model3;
returnCode= 2;
} else {
if (handler_->logLevel()>1)
printf("Reduced model has %d rows and %d columns\n",
model2->getNumRows(),model2->getNumCols());
if (true) {
OsiSolverInterface * solver = model2->solver();
OsiSolverInterface * osiclp = dynamic_cast< OsiSolverInterface*> (solver);
assert (osiclp);
int * priority = new int [numberColumns+1];
int n=0;
int iColumn;
for ( iColumn=0;iColumn<numberColumns;iColumn++) {
if (solver->isInteger(iColumn)) {
priority[n++]=10000;
}
}
priority[n]=1;
CbcObject * newObject =new CbcFollowOn2(model2);
model2->addObjects(1,&newObject);
delete newObject;
model2->passInPriorities(priority,false);
delete [] priority;
}
returnCode= model_->subBranchAndBound(model3,model2,4000);
}
#endif
model_->setDblParam(CbcModel::CbcMaximumSeconds,saveMaxTime);
model_->setNumberObjects(numberObjects);
osiclp->setNested(saveNested);
osiclp->setAlgorithm(saveAlgorithm);
#else
// start again very simply
ClpSimplex temp(modelPtr_,nNewRow,whichRow,nNewCol,whichColumn);
int returnCode;
OsiClpSolverInterface temp2(&temp);
temp2.setupForRepeatedUse(2);
int numberColumns2 = temp.numberColumns();
const double * colUpper2 = temp2.getColUpper();
const double * colLower2 = temp2.getColLower();
const double * solution2 = temp.getColSolution();
double * cleanSolution2 = new double [numberColumns2];
for (i=0;i<numberColumns2;i++) {
temp2.setInteger(i);
double value = solution2[i];
value = CoinMin(CoinMax(value,colLower2[i]),colUpper2[i]);
cleanSolution2[i] = value;
}
temp2.setColSolution(cleanSolution2);
delete [] cleanSolution2;
CbcModel modelSmall(temp2);
modelSmall.setNumberStrong(0);
CglProbing generator1;
generator1.setUsingObjective(true);
generator1.setMaxPass(3);
generator1.setMaxProbe(100);
generator1.setMaxLook(50);
generator1.setRowCuts(3);
CglGomory generator2;
// try larger limit
generator2.setLimit(300);
CglKnapsackCover generator3;
CglOddHole generator4;
generator4.setMinimumViolation(0.005);
generator4.setMinimumViolationPer(0.00002);
// try larger limit
generator4.setMaximumEntries(200);
CglClique generator5;
generator5.setStarCliqueReport(false);
generator5.setRowCliqueReport(false);
CglMixedIntegerRounding mixedGen;
CglFlowCover flowGen;
// Add in generators
modelSmall.addCutGenerator(&generator1,-1,"Probing",true,false,false,-1);
modelSmall.addCutGenerator(&generator2,-99,"Gomory",true,false,false,-99);
modelSmall.addCutGenerator(&generator3,-99,"Knapsack",true,false,false,-99);
modelSmall.addCutGenerator(&generator4,-99,"OddHole",true,false,false,-99);
modelSmall.addCutGenerator(&generator5,-99,"Clique",true,false,false,-99);
modelSmall.addCutGenerator(&flowGen,-99,"FlowCover",true,false,false,-99);
modelSmall.addCutGenerator(&mixedGen,-99,"MixedIntegerRounding",true,false,false,-100);
#if 1
const CoinPackedMatrix * matrix = temp2.getMatrixByCol();
const int * columnLength = matrix->getVectorLengths();
int * priority = new int [numberColumns2+1];
// do pseudo costs and priorities - take a reasonable guess
CbcObject ** objects = new CbcObject * [numberColumns2+1];
int n=0;
const double * objective = modelSmall.getObjCoefficients();
for (i=0;i<numberColumns2;i++) {
CbcSimpleIntegerPseudoCost * newObject =
new CbcSimpleIntegerPseudoCost(&modelSmall,n,i,objective[i],0.5*objective[i]);
newObject->setMethod(3);
objects[n]= newObject;
priority[n++]=10000-columnLength[i];
}
priority[n]=1;
objects[n++]=new CbcFollowOn2(&modelSmall);
modelSmall.addObjects(n,objects);
for (i=0;i<n;i++)
delete objects[i];
delete [] objects;
modelSmall.passInPriorities(priority,false);
delete [] priority;
#endif
modelSmall.setCutoff(model_->getCutoff());
//if (!onPathX&&modelSmall.getCutoff()>480.5)
//modelSmall.setCutoff(480.5);
//printf("cutoff %g\n",model_->getCutoff());
modelSmall.messageHandler()->setLogLevel(1);
modelSmall.solver()->messageHandler()->setLogLevel(0);
modelSmall.messagesPointer()->setDetailMessage(3,9);
modelSmall.messagesPointer()->setDetailMessage(3,6);
modelSmall.messagesPointer()->setDetailMessage(3,4);
modelSmall.messagesPointer()->setDetailMessage(3,13);
modelSmall.messagesPointer()->setDetailMessage(3,14);
modelSmall.messagesPointer()->setDetailMessage(3,1);
modelSmall.messagesPointer()->setDetailMessage(3,3007);
modelSmall.branchAndBound();
temp2.releaseClp();
if (modelSmall.bestSolution()) {
double objValue = 0.0;
const double * solution2 = modelSmall.bestSolution();
double * solution = modelPtr_->primalColumnSolution();
const double * objective = modelPtr_->objective();
for (i=0;i<numberColumns;i++)
solution[i]=colLower[i];
for (i=0;i<nNewCol;i++) {
int iColumn = whichColumn[i];
solution[iColumn]=solution2[i];
}
for (i=0;i<numberColumns;i++)
objValue += solution[i]*objective[i];
assert (objValue<model_->getCutoff());
if (objValue<model_->getCutoff()) {
//printf("good solution \n");
model_->setBestSolution(CBC_TREE_SOL,objValue,solution);
returnCode = 0;
} else {
returnCode=2;
}
} else {
returnCode=2;
}
#endif
if (returnCode!=0&&returnCode!=2) {
printf("pretending entire search done\n");
returnCode=0;
}
if (returnCode==0||returnCode==2) {
modelPtr_->setProblemStatus(1);
delete [] whichRow;
delete [] whichColumn;
return;
}
}
}
if ((count_<100&&algorithm_==2)||!algorithm_) {
delete [] whichRow;
delete [] whichColumn;
assert(!modelPtr_->specialOptions());
int saveOptions = modelPtr_->specialOptions();
int startFinishOptions;
bool takeHint;
OsiHintStrength strength;
bool gotHint = (getHintParam(OsiDoInBranchAndCut,takeHint,strength));
assert (gotHint);
if (strength!=OsiHintIgnore&&takeHint) {
// could do something - think about it
//printf("thin hint %d %c\n",strength,takeHint ? 'T' :'F');
}
if((specialOptions_&1)==0) {
startFinishOptions=0;
modelPtr_->setSpecialOptions(saveOptions|(64|1024));
} else {
startFinishOptions=1+2+4;
if((specialOptions_&4)==0)
modelPtr_->setSpecialOptions(saveOptions|(64|128|512|1024|4096));
else
modelPtr_->setSpecialOptions(saveOptions|(64|128|512|1024|2048|4096));
}
//printf("thin options %d size %d\n",modelPtr_->specialOptions(),modelPtr_->numberColumns());
setBasis(basis_,modelPtr_);
//modelPtr_->setLogLevel(1);
modelPtr_->dual(0,0);
basis_ = getBasis(modelPtr_);
modelPtr_->setSpecialOptions(saveOptions);
if (modelPtr_->status()==0) {
count_++;
double * solution = modelPtr_->primalColumnSolution();
int i;
for (i=0;i<numberColumns;i++) {
if (solution[i]>1.0e-6||modelPtr_->getStatus(i)==ClpSimplex::basic) {
node_[i]=CoinMax(count_,node_[i]);
howMany_[i]++;
}
}
} else {
if (!algorithm_==2)
printf("infeasible early on\n");
}
} else {
// use counts
int i;
const double * lower = modelPtr_->columnLower();
const double * upper = modelPtr_->columnUpper();
setBasis(basis_,modelPtr_);
ClpSimplex * temp = new ClpSimplex(modelPtr_,nNewRow,whichRow,nNewCol,whichColumn);
//temp->setLogLevel(2);
//printf("small has %d rows and %d columns\n",nNewRow,nNewCol);
temp->setSpecialOptions(128+512);
temp->setDualObjectiveLimit(1.0e50);
temp->dual();
if (temp->status()) {
// In some cases we know that it must be infeasible
if (believeInfeasible_||algorithm_==1) {
modelPtr_->setProblemStatus(1);
printf("assuming infeasible!\n");
//modelPtr_->writeMps("infeas.mps");
//temp->writeMps("infeas2.mps");
//abort();
delete temp;
delete [] whichRow;
delete [] whichColumn;
return;
}
}
double * solution = modelPtr_->primalColumnSolution();
if (!temp->status()) {
const double * solution2 = temp->primalColumnSolution();
memset(solution,0,numberColumns*sizeof(double));
for (i=0;i<nNewCol;i++) {
int iColumn = whichColumn[i];
solution[iColumn]=solution2[i];
modelPtr_->setStatus(iColumn,temp->getStatus(i));
}
double * rowSolution = modelPtr_->primalRowSolution();
const double * rowSolution2 = temp->primalRowSolution();
double * dual = modelPtr_->dualRowSolution();
const double * dual2 = temp->dualRowSolution();
memset(dual,0,numberRows*sizeof(double));
for (i=0;i<nNewRow;i++) {
int iRow=whichRow[i];
modelPtr_->setRowStatus(iRow,temp->getRowStatus(i));
rowSolution[iRow]=rowSolution2[i];
dual[iRow]=dual2[i];
}
// See if optimal
double * dj = modelPtr_->dualColumnSolution();
// get reduced cost for large problem
// this assumes minimization
memcpy(dj,modelPtr_->objective(),numberColumns*sizeof(double));
modelPtr_->transposeTimes(-1.0,dual,dj);
modelPtr_->setObjectiveValue(temp->objectiveValue());
modelPtr_->setProblemStatus(0);
int nBad=0;
for (i=0;i<numberColumns;i++) {
if (modelPtr_->getStatus(i)==ClpSimplex::atLowerBound
&&upper[i]>lower[i]&&dj[i]<-1.0e-5)
nBad++;
}
//modelPtr_->writeMps("bada.mps");
//temp->writeMps("badb.mps");
if (nBad) {
assert (algorithm_==2);
//printf("%d bad\n",nBad);
timesBad_++;
modelPtr_->primal();
}
} else {
// infeasible - do all
modelPtr_->setSpecialOptions(64+128+512);
setBasis(basis_,modelPtr_);
//modelPtr_->setLogLevel(1);
modelPtr_->dual(0,0);
basis_ = getBasis(modelPtr_);
modelPtr_->setSpecialOptions(0);
if (modelPtr_->status()) {
printf("really infeasible!\n");
delete temp;
delete [] whichRow;
delete [] whichColumn;
return;
} else {
printf("initially infeasible\n");
}
}
delete temp;
delete [] whichRow;
delete [] whichColumn;
basis_ = getBasis(modelPtr_);
modelPtr_->setSpecialOptions(0);
count_++;
if ((count_%100)==0&&algorithm_==2)
printf("count %d, bad %d\n",count_,timesBad_);
for (i=0;i<numberColumns;i++) {
if (solution[i]>1.0e-6||modelPtr_->getStatus(i)==ClpSimplex::basic) {
node_[i]=CoinMax(count_,node_[i]);
howMany_[i]++;
}
}
if (modelPtr_->objectiveValue()>=modelPtr_->dualObjectiveLimit())
modelPtr_->setProblemStatus(1);
}
}
void Test_evaluation_basis()
{
LMM::Index indexStart = 2; //1Y
LMM::Index indexEnd = 20; //10Y
Tenor floatingLegTenorType = Tenor::_6M;
Tenor fixedLegTenorType = Tenor::_1YR;
assert(indexStart%2==0&&indexEnd%2==0);
LMMTenorStructure_PTR lmmTenorStructure( new LMMTenorStructure(floatingLegTenorType, indexEnd/2));
std::vector<std::string> mkt_file_list = InputFileManager::get_VCUB_FileList();
const std::string& mkt_data_file = mkt_file_list.back();
std::string folder_name; // = "TotalCalib\\" ; config.use_positive_constraint_=true;
std::string base_name_file = LMMPATH::get_BaseFileName(mkt_data_file) + "\\";
folder_name+=base_name_file;
LMMPATH::reset_Output_SubFolder(folder_name );
LmmCalibrationConfig config;
config.floatLegTenor_=floatingLegTenorType;
config.fixedLegTenor_=fixedLegTenorType;
config.model_nbYear_ = indexEnd/2;
size_t fixedFloatRatio = config.fixedLegTenor_.ratioTo(config.floatLegTenor_);
config.correl_FullRank_ = fixedFloatRatio*config.model_nbYear_+1;
LmmSwaptionMarketData_PTR pLmmSwaptionMarketData = get_LmmSwaptionMarketData(config, mkt_data_file);
const std::vector<double>& initLiborValues = pLmmSwaptionMarketData->get_LiborQuotes()->get_InitLibor();
double strike = 0.0137;
std::vector<LMM::Index> exerciseDates;
exerciseDates.push_back(2);
//exerciseDates.push_back(4);
//exerciseDates.push_back(6);
//exerciseDates.push_back(8);
//exerciseDates.push_back(10);
//exerciseDates.push_back(12);
//exerciseDates.push_back(14);
//exerciseDates.push_back(16);
//exerciseDates.push_back(18);
exerciseDates.push_back(20);
McLmm_PTR mcLmm_for_pricer = getMcLmmExample(lmmTenorStructure, initLiborValues, LmmCalibrationConfig());
size_t fixedFloatingRatio = fixedLegTenorType.ratioTo(floatingLegTenorType);
std::vector<std::vector<size_t>> subset;
for(size_t i = 0; i <= 2; i++)
{
subset.push_back(std::vector<size_t>());
subset.back().push_back(0);
subset.back().push_back(0);
subset.back().push_back(i);
}
//for(size_t j = 1; j <= 2; j++)
//{
// subset.push_back(std::vector<size_t>());
// subset.back().push_back(0);
// subset.back().push_back(j);
// subset.back().push_back(0);
//}
/* for(size_t k = 1; k <= 2; k++)
{
subset.push_back(std::vector<size_t>());
subset.back().push_back(k);
subset.back().push_back(0);
subset.back().push_back(0);
} */
size_t regressionIndex=2;
LMM::Index liborIndex = indexStart + regressionIndex*fixedFloatingRatio;
LMM::Index paymentIndex = indexStart + regressionIndex*fixedFloatingRatio + 1;
VanillaSwap vanillaSwap( strike,
liborIndex,
indexEnd,
floatingLegTenorType,
fixedLegTenorType,
lmmTenorStructure);
std::vector<Basis_CONSTPTR> basis_vect;
std::vector<Basis_Evaluator_CONSTPTR> basis_evaluator_vect;
for(size_t basisIndex = 0; basisIndex<subset.size(); basisIndex++)
{
basis_vect.push_back(getBasis(subset[basisIndex], 1.0, vanillaSwap, strike, liborIndex));
basis_evaluator_vect.push_back(getBasisEvaluator(subset[basisIndex], McLmmVanillaSwapPricer(mcLmm_for_pricer)));
}
LS::Regression rg(LS::RegressionRepresentation(basis_vect, basis_evaluator_vect));
McLmm_PTR mcLmm = getMcLmmExample(lmmTenorStructure, initLiborValues, LmmCalibrationConfig());
McLmm_LS mcLmm_LS(mcLmm);
mcLmm_LS.simulateLMM(1);
size_t nb = 30000;
clock_t startTime = clock();
std::vector<std::vector<double>> vect(nb);
for(size_t i=0; i<nb; i++)
{
//rg.getRegressionRepresentation().evaluate_basis(mcLmm_LS.lmmSimualtionResults_[0]);
vect[i].resize(3);
vect[i]=rg.getRegressionRepresentation().getBasis_val_buffer();
vect[i]=std::vector<double>(3, 1.0);
}
clock_t endTime = clock();
clock_t time = endTime - startTime;
double time_in_second = time/(double) CLOCKS_PER_SEC;
cout << "time_in_second "<< time_in_second << endl;
const matrix& m = mcLmm_LS.lmmSimualtionResults_[0].get_liborMatrix();
const std::vector<double>& numeraire = mcLmm_LS.lmmSimualtionResults_[0].get_numeraire();
std::vector<size_t> basis1;
basis1.push_back(1);
basis1.push_back(0);
basis1.push_back(0);
Basis_CONSTPTR basisA=getBasis(basis1, 1.0, vanillaSwap, strike, liborIndex);
Basis_Evaluator_CONSTPTR basis_EvaluatorA = getBasisEvaluator(basis1, McLmmVanillaSwapPricer(mcLmm_for_pricer));
basis_EvaluatorA->evaluate(basisA,m, numeraire);
clock_t startTime1 = clock();
for(size_t i=0; i<1000; i++)
basis_EvaluatorA->evaluate(basisA,m, numeraire);
clock_t endTime1 = clock();
clock_t time1 = endTime1 - startTime1;
double time1_in_second = time1/(double) CLOCKS_PER_SEC;
cout << "time1_in_second "<< time1_in_second << endl;
std::vector<size_t> basis2;
basis2.push_back(0);
basis2.push_back(1);
basis2.push_back(0);
Basis_CONSTPTR basisB=getBasis(basis2, 1.0, vanillaSwap, strike, liborIndex);
Basis_Evaluator_CONSTPTR basis_EvaluatorB = getBasisEvaluator(basis2, McLmmVanillaSwapPricer(mcLmm_for_pricer));
clock_t startTime2 = clock();
for(size_t i=0; i<1000; i++)
basis_EvaluatorB->evaluate(basisB, m, numeraire);
clock_t endTime2 = clock();
clock_t time2 = endTime2 - startTime2;
double time2_in_second = time2/(double) CLOCKS_PER_SEC;
cout << "time2_in_second "<< time2_in_second << endl;
std::vector<size_t> basis3;
basis3.push_back(0);
basis3.push_back(0);
basis3.push_back(1);
Basis_CONSTPTR basisC=getBasis(basis3, 1.0, vanillaSwap, strike, liborIndex);
Basis_Evaluator_CONSTPTR basis_EvaluatorC = getBasisEvaluator(basis3, McLmmVanillaSwapPricer(mcLmm_for_pricer));
clock_t startTime3 = clock();
for(size_t i=0; i<1000; i++)
basis_EvaluatorC->evaluate(basisC, m, numeraire);
clock_t endTime3 = clock();
clock_t time3 = endTime3 - startTime3;
double time3_in_second = time3/(double) CLOCKS_PER_SEC;
cout << "time3_in_second "<< time3_in_second << endl;
EV_Evaluator_CONSTPTR ev_evaluator_libor(new EV_LiborRate_Evaluator());
EV_Evaluator_CONSTPTR ev_evaluator_swaprate(new EV_VanillaSwapRate_Evaluator(McLmmVanillaSwapPricer(mcLmm_for_pricer)));
EV_CONSTPTR ev_swaprate(new EV_VanillaSwapRate( Rate1_CONSTPTR( new VanillaSwapRate(VanillaSwap(vanillaSwap)))));
EV_CONSTPTR ev_libor(new EV_LiborRate(Rate1_CONSTPTR(new LiborRate(2,Tenor(Tenor::_6M)))));
clock_t startTime4 = clock();
for(size_t i=0; i<1000; i++)
ev_evaluator_swaprate->evaluate(ev_swaprate,m,numeraire);
clock_t endTime4 = clock();
clock_t time4 = endTime4 - startTime4;
double time4_in_second = time4/(double) CLOCKS_PER_SEC;
cout << "time4_in_second "<< time4_in_second << endl;
clock_t startTime5 = clock();
for(size_t i=0; i<1000; i++)
ev_evaluator_libor->evaluate(ev_libor,m,numeraire);
clock_t endTime5 = clock();
clock_t time5 = endTime5 - startTime5;
double time5_in_second = time5/(double) CLOCKS_PER_SEC;
cout << "time5_in_second "<< time5_in_second << endl;
}
//-----------------------------------------------------------------------------
void CbcSolver2::resolve()
{
int numberColumns = modelPtr_->numberColumns();
if ((count_<10&&algorithm_==2)||!algorithm_) {
OsiClpSolverInterface::resolve();
if (modelPtr_->status()==0) {
count_++;
double * solution = modelPtr_->primalColumnSolution();
int i;
for (i=0;i<numberColumns;i++) {
if (solution[i]>1.0e-6||modelPtr_->getStatus(i)==ClpSimplex::basic) {
node_[i]=CoinMax(count_,node_[i]);
howMany_[i]++;
}
}
} else {
if (!algorithm_==2)
printf("infeasible early on\n");
}
} else {
// use counts
int numberRows=modelPtr_->numberRows();
int * whichRow = new int[numberRows];
int * whichColumn = new int [numberColumns];
int i;
const double * lower = modelPtr_->columnLower();
const double * upper = modelPtr_->columnUpper();
const double * rowUpper = modelPtr_->rowUpper();
bool equality=false;
for (i=0;i<numberRows;i++) {
if (rowUpper[i]==1.0) {
equality=true;
break;
}
}
setBasis(basis_,modelPtr_);
int nNewCol=0;
// Column copy
//const double * element = modelPtr_->matrix()->getElements();
const int * row = modelPtr_->matrix()->getIndices();
const CoinBigIndex * columnStart = modelPtr_->matrix()->getVectorStarts();
const int * columnLength = modelPtr_->matrix()->getVectorLengths();
int * rowActivity = new int[numberRows];
memset(rowActivity,0,numberRows*sizeof(int));
int * rowActivity2 = new int[numberRows];
memset(rowActivity2,0,numberRows*sizeof(int));
char * mark = new char[numberColumns];
memset(mark,0,numberColumns);
// Get rows which are satisfied
for (i=0;i<numberColumns;i++) {
if (lower[i]>0.0) {
CoinBigIndex j;
for (j=columnStart[i];
j<columnStart[i]+columnLength[i];j++) {
int iRow=row[j];
rowActivity2[iRow] ++;
}
} else if (!upper[i]) {
mark[i]=2; // no good
}
}
// If equality - check not infeasible
if (equality) {
bool feasible=true;
for (i=0;i<numberRows;i++) {
if (rowActivity2[i]>1) {
feasible=false;
break;
}
}
if (!feasible) {
delete [] rowActivity;
delete [] rowActivity2;
modelPtr_->setProblemStatus(1);
delete [] whichRow;
delete [] whichColumn;
delete [] mark;
printf("infeasible by inspection (over)\n");
return;
}
}
int nNoGood=0;
for (i=0;i<numberColumns;i++) {
if (mark[i]==2) {
nNoGood++;
continue;
}
bool choose;
if (algorithm_==1)
choose = true;
else
choose = (node_[i]>count_-memory_&&node_[i]>0);
bool any;
if (equality) {
// See if forced to be zero
CoinBigIndex j;
any=true;
for (j=columnStart[i];
j<columnStart[i]+columnLength[i];j++) {
int iRow=row[j];
if (rowActivity2[iRow])
any=false; // can't be in
}
} else {
// See if not useful
CoinBigIndex j;
any=false;
for (j=columnStart[i];
j<columnStart[i]+columnLength[i];j++) {
int iRow=row[j];
if (!rowActivity2[iRow])
any=true; // useful
}
}
if (!any&&!lower[i]) {
choose=false;
// and say can't be useful
mark[i]=2;
nNoGood++;
}
if (strategy_&&modelPtr_->getColumnStatus(i)==ClpSimplex::basic)
choose=true;
if (choose||lower[i]>0.0) {
mark[i]=1;
whichColumn[nNewCol++]=i;
CoinBigIndex j;
double value = upper[i];
if (value) {
for (j=columnStart[i];
j<columnStart[i]+columnLength[i];j++) {
int iRow=row[j];
rowActivity[iRow] ++;
}
}
}
}
// If equality add in slacks
CoinModel build;
if (equality) {
int row=0;
for (i=0;i<numberRows;i++) {
// put in all rows if wanted
if(strategy_)
rowActivity2[i]=0;
if (!rowActivity2[i]) {
double element=1.0;
build.addColumn(1,&row,&element,0.0,1.0,1.0e8); // large cost
row++;
}
}
}
int nOK=0;
int nNewRow=0;
for (i=0;i<numberRows;i++) {
if (rowActivity[i])
nOK++;
if (!rowActivity2[i])
whichRow[nNewRow++]=i; // not satisfied
else
modelPtr_->setRowStatus(i,ClpSimplex::basic); // make slack basic
}
if (nOK<numberRows) {
for (i=0;i<numberColumns;i++) {
if (!mark[i]) {
CoinBigIndex j;
int good=0;
for (j=columnStart[i];
j<columnStart[i]+columnLength[i];j++) {
int iRow=row[j];
if (!rowActivity[iRow]) {
rowActivity[iRow] ++;
good++;
}
}
if (good) {
nOK+=good;
whichColumn[nNewCol++]=i;
}
}
}
}
delete [] rowActivity;
delete [] rowActivity2;
if (nOK<numberRows) {
modelPtr_->setProblemStatus(1);
delete [] whichRow;
delete [] whichColumn;
delete [] mark;
printf("infeasible by inspection\n");
return;
}
bool allIn=false;
if (nNewCol+nNoGood+numberRows>numberColumns) {
// add in all
allIn=true;
for (i=0;i<numberColumns;i++) {
if (!mark[i]) {
whichColumn[nNewCol++]=i;
}
}
}
ClpSimplex * temp = new ClpSimplex(modelPtr_,nNewRow,whichRow,nNewCol,whichColumn);
if (equality)
temp->addColumns(build);
temp->setLogLevel(1);
printf("small has %d rows and %d columns (%d impossible to help) %s\n",
nNewRow,nNewCol,nNoGood,allIn ? "all in" : "");
temp->setSpecialOptions(128+512);
temp->setDualObjectiveLimit(1.0e50);
temp->dual();
assert (!temp->status());
double * solution = modelPtr_->primalColumnSolution();
const double * solution2 = temp->primalColumnSolution();
memset(solution,0,numberColumns*sizeof(double));
for (i=0;i<nNewCol;i++) {
int iColumn = whichColumn[i];
solution[iColumn]=solution2[i];
modelPtr_->setStatus(iColumn,temp->getStatus(i));
}
double * rowSolution = modelPtr_->primalRowSolution();
const double * rowSolution2 = temp->primalRowSolution();
double * dual = modelPtr_->dualRowSolution();
const double * dual2 = temp->dualRowSolution();
memset(dual,0,numberRows*sizeof(double));
for (i=0;i<nNewRow;i++) {
int iRow=whichRow[i];
modelPtr_->setRowStatus(iRow,temp->getRowStatus(i));
rowSolution[iRow]=rowSolution2[i];
dual[iRow]=dual2[i];
}
// See if optimal
double * dj = modelPtr_->dualColumnSolution();
// get reduced cost for large problem
// this assumes minimization
memcpy(dj,modelPtr_->objective(),numberColumns*sizeof(double));
modelPtr_->transposeTimes(-1.0,dual,dj);
modelPtr_->setObjectiveValue(temp->objectiveValue());
modelPtr_->setProblemStatus(0);
int nBad=0;
for (i=0;i<numberColumns;i++) {
if (modelPtr_->getStatus(i)==ClpSimplex::atLowerBound
&&upper[i]>lower[i]&&dj[i]<-1.0e-5)
nBad++;
}
//modelPtr_->writeMps("bada.mps");
//temp->writeMps("badb.mps");
delete temp;
if (nBad&&!allIn) {
assert (algorithm_==2);
//printf("%d bad\n",nBad);
timesBad_++;
// just non mark==2
int nAdded=0;
for (i=0;i<numberColumns;i++) {
if (!mark[i]) {
whichColumn[nNewCol++]=i;
nAdded++;
}
}
assert (nAdded);
{
temp = new ClpSimplex(modelPtr_,nNewRow,whichRow,nNewCol,whichColumn);
if (equality)
temp->addColumns(build);
temp->setLogLevel(2);
temp->setSpecialOptions(128+512);
temp->setDualObjectiveLimit(1.0e50);
temp->primal(1);
assert (!temp->status());
double * solution = modelPtr_->primalColumnSolution();
const double * solution2 = temp->primalColumnSolution();
memset(solution,0,numberColumns*sizeof(double));
for (i=0;i<nNewCol;i++) {
int iColumn = whichColumn[i];
solution[iColumn]=solution2[i];
modelPtr_->setStatus(iColumn,temp->getStatus(i));
}
double * rowSolution = modelPtr_->primalRowSolution();
const double * rowSolution2 = temp->primalRowSolution();
double * dual = modelPtr_->dualRowSolution();
const double * dual2 = temp->dualRowSolution();
memset(dual,0,numberRows*sizeof(double));
for (i=0;i<nNewRow;i++) {
int iRow=whichRow[i];
modelPtr_->setRowStatus(iRow,temp->getRowStatus(i));
rowSolution[iRow]=rowSolution2[i];
dual[iRow]=dual2[i];
}
modelPtr_->setObjectiveValue(temp->objectiveValue());
modelPtr_->setProblemStatus(0);
iterationsBad_ += temp->numberIterations();
printf("clean %d\n",temp->numberIterations());
delete temp;
}
}
delete [] mark;
delete [] whichRow;
delete [] whichColumn;
basis_ = getBasis(modelPtr_);
modelPtr_->setSpecialOptions(0);
count_++;
if ((count_%100)==0&&algorithm_==2)
printf("count %d, bad %d - iterations %d\n",count_,timesBad_,iterationsBad_);
for (i=0;i<numberColumns;i++) {
if (solution[i]>1.0e-6||modelPtr_->getStatus(i)==ClpSimplex::basic) {
node_[i]=CoinMax(count_,node_[i]);
howMany_[i]++;
}
}
if (modelPtr_->objectiveValue()>=modelPtr_->dualObjectiveLimit())
modelPtr_->setProblemStatus(1);
}
}
