void RD_SubproblemSolver::GetOptimalityCut( Real_T &value, Real_T *grad, // )
Int_T n, const Scenario &Scen )
{
assert( grad != NULL );
//----------------------------------------------------------------------
// Time to generate an optimality cut g_omega such that:
// g_omega = - Transp( T_omega ) * pi_opt
// where
// g_omega is the gradient,
// Transp( T_omega ) is technology matrix transpose and
// pi_opt is the vector of optimal dual variables.
//
CalculateGradient( grad, n, y, Scen );
//--------------------------------------------------------------------------
// Compute the duality gap - verify the objective value in this manner.
//
assert( DualityGap() < 1e-6 );
value = Result;
}
///-------------------------------------------------------------
/// Main solving loop.
/// Loops over all valid frames and performs IK solving.
///-------------------------------------------------------------
void IKSolver::SolveLoop()
{
// clear saved frame data
UI->mFrameToDofMap.clear();
int maxFrames = mModel->mOpenedC3dFile->GetFrameCount();
#ifdef _DEBUG
std::ofstream logFile("logs/loop_log.txt");
logFile << "Num Frames: " << maxFrames << std::endl << std::endl;
//std::ofstream dofFile("logs/dofs.txt");
#endif
// loop over all valid frames
// this is limited by the number of frames in the constraint file, but it can also be
// limited by a max iteration parameter set for the solver
for (int frameCounter = 0; frameCounter < maxFrames /*&& frameCounter < mMaxNumFrames*/; frameCounter++)
{
std::cout << "Starting frame " << frameCounter << std::endl;
// calculate constraint values
CreateConstraints(frameCounter);
CalculateConstraints(frameCounter);
// evaluate objective function
double objectiveFunction = EvaluateObjectiveFunction(frameCounter);
// various variables for our main loop
double localStepSize = mStepSize;
double localEpsilon = mEpsilon;
int iterations = 0;
int decreaseStepCounter = 0;
// main solving loop
while (objectiveFunction > localEpsilon )//&& iterations < mMaxIterations)
{
#ifdef _DEBUG
// print out some information every 30 frames
if (iterations % mPrintFrequency == 0)
{
std::cout << "Iteration " << iterations << "\tObjective: " << objectiveFunction
<< "\tStep: " << localStepSize << "\tEpsilon: " << localEpsilon << std::endl;
}
#endif
#ifdef _DEBUG
if (iterations % mPrintFrequency == 0)
{
logFile << "Frame: " << frameCounter << std::endl;
logFile << "Iteration: " << iterations << std::endl;
logFile << "Step Size: " << localStepSize << std::endl;
logFile << "Epsilon: " << localEpsilon << std::endl;
logFile << "Objective Function: " << objectiveFunction << std::endl;
}
#endif
// calculate gradient
Vecd gradient = CalculateGradient(frameCounter);
// get old dofs
Vecd oldDofs;
oldDofs.SetSize(mModel->GetDofCount());
mModel->mDofList.GetDofs(&oldDofs);
// move dofs
Vecd newDofs = oldDofs - localStepSize * gradient;
// update dofs
mModel->SetDofs(newDofs);
#ifdef _DEBUG
if (iterations % mPrintFrequency == 0)
{
logFile << "Gradient: " << gradient << std::endl;
logFile << "Old Dofs: " << oldDofs << std::endl;
logFile << "New Dofs: " << newDofs << std::endl;
logFile << std::endl;
}
#endif
// calculate new constraint values
CalculateConstraints(frameCounter);
// calculate new objective function value
double newObjectiveFunction = EvaluateObjectiveFunction(frameCounter);
// make sure we always decrease so that we don't get stuck in some infinite loop
if (newObjectiveFunction < objectiveFunction)
{
// adaptive step size
// if difference between objective functions is greater than epsilon
// and certain number of iterations have passed, increase step size
if (newObjectiveFunction > mEpsilon &&
iterations > 0 && iterations % mStepIncreaseFrequency == 0)
{
localStepSize *= mStepIncreaseFactor;
#ifdef _DEBUG
std::cout << "Increased step size to " << localStepSize << std::endl;
#endif
}
objectiveFunction = newObjectiveFunction;
}
else
{
// count how many times we've decrease step this frame
decreaseStepCounter++;
// decrease step size
localStepSize /= mStepDecreaseFactor;
#ifdef _DEBUG
std::cout << "Decreased step size to " << localStepSize << std::endl;
#endif
// based on how many times we've had to decrease the step, choose different options
if (decreaseStepCounter < mMaxIterations)
{
// reset to old dofs and try again
mModel->SetDofs(oldDofs);
}
else if (decreaseStepCounter < mEpsilonIncreaseFrequency*mMaxIterations)
{
// try increasing epsilon
localEpsilon *= mEpsilonIncreaseFactor;
#ifdef _DEBUG
std::cout << "Increased epsilon to " << localEpsilon << std::endl;
#endif
// reset to old dofs and try again
mModel->SetDofs(oldDofs);
}
else
{
// at this point, give up; it isn't worth it
objectiveFunction = 0.0;
#ifdef _DEBUG
std::cout << "Too many iterations; moving on..." << std::endl;
#endif
}
}
// update iteration counter
iterations++;
}
UI->mFrameCounter_cou->value(frameCounter); // update frame counter
SaveDofs(frameCounter); // save dofs for later playback
#ifdef _DEBUG
// commented out because it slows things down more than really necessary
// and same info can be placed into main log file
// write dofs to file
//Vecd dofs;
//dofs.SetSize(mModel->GetDofCount());
//mModel->mDofList.GetDofs(&dofs);
//dofFile << frameCounter << std::endl << dofs << std::endl << std::endl;
#endif
UI->mGLWindow->flush(); // update screen
std::cout << "Ending frame " << frameCounter;
#ifdef _DEBUG
std::cout << " after " << iterations << " iterations";
#endif
std::cout << std::endl;
}
#ifdef _DEBUG
//dofFile.close();
logFile.close();
#endif
}
ion::Scene::CSimpleMesh * MarchingCubes(SMarchingCubesVolume & Volume)
{
CalculateGradient(Volume);
ion::Scene::CSimpleMesh * Mesh = new ion::Scene::CSimpleMesh();
int CurrentBuffer = 0;
Mesh->Vertices.reserve(1 << 14);
Mesh->Triangles.reserve(1 << 11);
ion::Scene::CSimpleMesh::SVertex Vertices[12];
vec3i VertexIndices[8] =
{
vec3i(0, 0, 0),
vec3i(1, 0, 0),
vec3i(1, 0, 1),
vec3i(0, 0, 1),
vec3i(0, 1, 0),
vec3i(1, 1, 0),
vec3i(1, 1, 1),
vec3i(0, 1, 1),
};
for (s32 z = 0; z < Volume.Dimensions.Z - 1; ++ z)
for (s32 y = 0; y < Volume.Dimensions.Y - 1; ++ y)
for (s32 x = 0; x < Volume.Dimensions.X - 1; ++ x)
{
int Lookup = 0;
if ((Volume.Get(vec3i(x, y, z) + VertexIndices[0]).Value <= 0)) Lookup |= 1;
if ((Volume.Get(vec3i(x, y, z) + VertexIndices[1]).Value <= 0)) Lookup |= 2;
if ((Volume.Get(vec3i(x, y, z) + VertexIndices[2]).Value <= 0)) Lookup |= 4;
if ((Volume.Get(vec3i(x, y, z) + VertexIndices[3]).Value <= 0)) Lookup |= 8;
if ((Volume.Get(vec3i(x, y, z) + VertexIndices[4]).Value <= 0)) Lookup |= 16;
if ((Volume.Get(vec3i(x, y, z) + VertexIndices[5]).Value <= 0)) Lookup |= 32;
if ((Volume.Get(vec3i(x, y, z) + VertexIndices[6]).Value <= 0)) Lookup |= 64;
if ((Volume.Get(vec3i(x, y, z) + VertexIndices[7]).Value <= 0)) Lookup |= 128;
auto Interpolate = [&](vec3i const & v1, vec3i const & v2) -> ion::Scene::CSimpleMesh::SVertex
{
//static CColorTable ColorTable;
ion::Scene::CSimpleMesh::SVertex v;
f32 const d1 = Volume.Get(v1).Value;
f32 const d2 = Volume.Get(v2).Value;
f32 ratio = d1 / (d2 - d1);
if (ratio <= 0.f)
ratio += 1.f;
v.Position =
vec3f(v1) * (ratio) +
vec3f(v2) * (1.f - ratio);
//v.Color = ColorTable.Get(
// Volume.Get(v1x, v1y, v1z).Color * (ratio) +
// Volume.Get(v2x, v2y, v2z).Color * (1.f - ratio));
v.Normal =
Normalize(Volume.Get(v1).Gradient) * (ratio) +
Normalize(Volume.Get(v2).Gradient) * (1.f - ratio);
//f32 const valp1 = Volume.Get(v1).Value;
//f32 const valp2 = Volume.Get(v2).Value;
//vec3f p1 = vec3f(v1);
//vec3f p2 = vec3f(v2);
//float mu;
//if (abs(0 - valp1) < 0.00001)
// return(p1);
//if (abs(0 - valp2) < 0.00001)
// return(p2);
//if (abs(valp1 - valp2) < 0.00001)
// return(p1);
//mu = (0 - valp1) / (valp2 - valp1);
//v.Position.X = p1.X + mu * (p2.X - p1.X);
//v.Position.Y = p1.Y + mu * (p2.Y - p1.Y);
//v.Position.Z = p1.Z + mu * (p2.Z - p1.Z);
return v;
};
u32 EdgeTableLookup = EdgeTable[Lookup];
if (EdgeTable[Lookup] != 0)
{
if (EdgeTable[Lookup] & 1) Vertices[0] = Interpolate(vec3i(x, y, z) + VertexIndices[0], vec3i(x, y, z) + VertexIndices[1]);
if (EdgeTable[Lookup] & 2) Vertices[1] = Interpolate(vec3i(x, y, z) + VertexIndices[1], vec3i(x, y, z) + VertexIndices[2]);
if (EdgeTable[Lookup] & 4) Vertices[2] = Interpolate(vec3i(x, y, z) + VertexIndices[2], vec3i(x, y, z) + VertexIndices[3]);
if (EdgeTable[Lookup] & 8) Vertices[3] = Interpolate(vec3i(x, y, z) + VertexIndices[3], vec3i(x, y, z) + VertexIndices[0]);
if (EdgeTable[Lookup] & 16) Vertices[4] = Interpolate(vec3i(x, y, z) + VertexIndices[4], vec3i(x, y, z) + VertexIndices[5]);
if (EdgeTable[Lookup] & 32) Vertices[5] = Interpolate(vec3i(x, y, z) + VertexIndices[5], vec3i(x, y, z) + VertexIndices[6]);
if (EdgeTable[Lookup] & 64) Vertices[6] = Interpolate(vec3i(x, y, z) + VertexIndices[6], vec3i(x, y, z) + VertexIndices[7]);
if (EdgeTable[Lookup] & 128) Vertices[7] = Interpolate(vec3i(x, y, z) + VertexIndices[7], vec3i(x, y, z) + VertexIndices[4]);
if (EdgeTable[Lookup] & 256) Vertices[8] = Interpolate(vec3i(x, y, z) + VertexIndices[0], vec3i(x, y, z) + VertexIndices[4]);
if (EdgeTable[Lookup] & 512) Vertices[9] = Interpolate(vec3i(x, y, z) + VertexIndices[1], vec3i(x, y, z) + VertexIndices[5]);
if (EdgeTable[Lookup] & 1024) Vertices[10] = Interpolate(vec3i(x, y, z) + VertexIndices[2], vec3i(x, y, z) + VertexIndices[6]);
if (EdgeTable[Lookup] & 2048) Vertices[11] = Interpolate(vec3i(x, y, z) + VertexIndices[3], vec3i(x, y, z) + VertexIndices[7]);
for (u32 i = 0; TriTable[Lookup][i] != -1; i += 3)
{
for (u32 j = i; j < (i+3); ++ j)
Mesh->Vertices.push_back(Vertices[TriTable[Lookup][j]]);
ion::Scene::CSimpleMesh::STriangle Tri;
Tri.Indices[0] = (uint) Mesh->Vertices.size() - 3;
Tri.Indices[1] = (uint) Mesh->Vertices.size() - 2;
Tri.Indices[2] = (uint) Mesh->Vertices.size() - 1;
Mesh->Triangles.push_back(Tri);
}
}
}
return Mesh;
}
void RD_SubproblemSolver::GetFeasibilityCut( Real_T &value, Real_T *grad, // )
Int_T n, const Scenario &Scen )
{
assert( grad != NULL );
//--------------------------------------------------------------------------
// Here we generate a feasibility cut. For that purpose we need to know
// which row of the constraint matrix was most infeasible and what was the
// infeasibility.
//
//
// Find the most violated constraint corresponding to a non-zero basic
// artificial variable. Put the index in 'infrow'.
//
Int_T infrow = -1;
value = 0.0;
for( Int_T j = Int_T( LP.GetStructN() + LP.GetSlackN() ); j < N; j++ )
if( VarType[j] & VT_ARTIF && x[j] > FEASIBILITY_TOL )
if( x[j] > value && A2B[j] >= 0 )
{
infrow = A2B[j];
value = x[j];
}
//
// It is possible that the problem is infeasible, but all artificial
// variables (some of them have non-zero values) are non-basic. In such
// case a dual optimal solution to the first stage problem ('y_t') is
// used to generate the cut.
//
if( infrow < 0 )
{
#ifndef NDEBUG
Bool_T found = False;
for( Int_T i = 0; i < M; i++ )
if( IsNonZero( y_t[i] ) )
{
found = True;
break;
}
if( !found )
FatalError( "Internal error when generating a feasibility cut." );
#endif
for( Int_T j = Int_T( LP.GetStructN() + LP.GetSlackN() ); j < N; j++ )
value += fabs( x[j] );
CalculateGradient( grad, n, y_t, Scen );
}
else
{
//----------------------------------------------------------------------
// Extract the appropriate row of the basis inverse into "pi" vector.
// Use "mark" work vector as a work space for sparsity pattern.
//
WorkVector<Real_T> pi( M );
WorkVector<Int_T> mark( M );
pi.Fill( 0.0, M ); mark.Fill( 0, M );
pi[ infrow ] = 1.0; mark[ infrow ] = 1;
B->SparseBTRAN( pi, mark );
CalculateGradient( grad, n, pi, Scen );
}
}
