///-------------------------------------------------------------
/// Calculates gradient for a given frame.
///-------------------------------------------------------------
Vecd IKSolver::CalculateGradient(int frameNum)
{
// setup initial gradient to be zero
// the dimensions should be equal to the number of DOFs we have
Vecd gradient;
gradient.SetSize(mModel->GetDofCount());
gradient.MakeZero();
// loop over all of our constraints, getting Jacobian and
// combining to form final value to add to total gradient
for (int i = 0; i < mConstraintList.size(); i++)
{
// get constraint
Constraint & constraint = mConstraintList[i];
Vec4d constraintVec(constraint.GetConstraintValue(), 1.0);
// get Jacobian - TODO
ConstraintJacobian jacobian(mModel, constraint);
Matd jacobianMatrix = jacobian.CalculateJacobian();
// calculate current value and add to gradient
Vecd currentValue = jacobianMatrix * constraintVec;
gradient = gradient + currentValue;
}
// final computation of doubling after above calculation
gradient = 2.0 * gradient;
return gradient;
}
void Solver::solve() {
// Utility constants
const int sIFreq = 20;
const double sIF = 4.0;
const double sDF = 2.0;
const int eIFreq = 2;
const double eIF = 2.0;
// Loop over all valid frames
for (int f = 0; f < mMaxFrames; f++)
{
// Build C[]
buildConstraintMat(f);
//Objective function value
double o = 0.0;
Markers &handles = mModel->mHandleList;
for (int i = 0; i < constraintIDs.size(); i++)
{
// Get handle and target position for the given constraint
Marker *h = handles[constraintIDs[i]];
Vec3d cPos = mModel->mOpenedC3dFile->GetMarkerPos(f, constraintIDs[i]);
// Calculate values and store
Vec3d cVal = h->mGlobalPos - cPos;
double cLength = sqrlen(cVal);
//Store results
constraintVals.push_back(cVal);
constraintLengths.push_back(cLength);
//Update objective function
o += cLength;
}
// Main loop
double e = mEps;
double s = mStep;
int iterCount = 0;
int stepCount = 0;
while (o > e) //while Objective Function > Epsilon
{
// Compute gradient -- TODO: Move this somewhere else for the sake of keeping this compact!
Vecd gradient;
gradient.SetSize(mModel->GetDofCount());
gradient.MakeZero();
for (int i = 0; i < constraintIDs.size(); i++)
{
//Get Constraint
Vec4d cVec(constraintVals[i], 1.0);
//Get Jacobian
Matd J = computeJ(mModel->mHandleList, constraintIDs[i]);
//Calculate current and add to gradient matrix
Vecd currentVal = J * cVec;
gradient = gradient + currentVal;
}
// Finally, double it
gradient = 2.0 * gradient;
// Get previous DOFs
Vecd prevDofs;
prevDofs.SetSize(mModel->GetDofCount());
mModel->mDofList.GetDofs(&prevDofs);
// Update DOFs
Vecd nextDofs = prevDofs - s * gradient;
mModel->SetDofs(nextDofs);
// Recalculate constraints and objective function
double newO = 0.0;
Markers &handles = mModel->mHandleList;
for (int i = 0; i < constraintIDs.size(); i++)
{
// Get handle and target position for the given constraint
Marker *h = handles[constraintIDs[i]];
Vec3d cPos = mModel->mOpenedC3dFile->GetMarkerPos(f, constraintIDs[i]);
// Calculate values and store
Vec3d cVal = h->mGlobalPos - cPos;
double cLength = sqrlen(cVal);
//Store results
constraintVals.push_back(cVal);
constraintLengths.push_back(cLength);
//Update objective function
newO += cLength;
}
// Attempting to make sure we don't go infinite...
if (newO < o)
{
if (newO > mEps && iterCount > 0 && iterCount % 20 == 0)
{
s *= 3.0;
}
o = newO;
}
else
{
stepCount++;
// Decrease step size
s = s / 2.0;
// based on how many times we've had to decrease the step size, choose different options
if (stepCount < mMaxIters)
{
// Reset to the previous dofs and try again
mModel->SetDofs(prevDofs);
}
else if (stepCount < eIFreq*mMaxIters)
{
// Try increasing epsilon
e *= 3.0;
// Reset to the previous dofs and try again
mModel->SetDofs(prevDofs);
}
else
{
// Otherwise, halt or we will never finish
o = 0.0;
}
}
iterCount++;
}
// Update the frame counter
UI->mFrameCounter_cou->value(f);
//Refresh the screen
UI->mGLWindow->flush();
}
}
