bool MultiplyQualityMetric::evaluate_with_Hessian( PatchData& pd,
size_t handle,
double& value,
std::vector<size_t>& indices,
std::vector<Vector3D>& gradient,
std::vector<Matrix3D>& Hessian,
MsqError& err )
{
std::vector<size_t>::iterator i;
size_t j, r, c, n, h;
double val1, val2;
bool rval1, rval2;
rval1 = metric1.evaluate_with_Hessian( pd, handle, val1, indices1, grad1, Hess1, err ); MSQ_ERRZERO(err);
rval2 = metric2.evaluate_with_Hessian( pd, handle, val2, indices2, grad2, Hess2, err ); MSQ_ERRZERO(err);
// merge index lists
indices.resize( indices1.size() + indices2.size() );
i = std::copy( indices1.begin(), indices1.end(), indices.begin() );
std::copy( indices2.begin(), indices2.end(), i );
std::sort( indices.begin(), indices.end() );
indices.erase( std::unique( indices.begin(), indices.end() ), indices.end() );
// calculate grads and convert index lists to indices into output list
gradient.clear();
gradient.resize( indices.size(), Vector3D(0.0) );
for (j = 0; j < indices1.size(); ++j)
{
i = std::lower_bound( indices.begin(), indices.end(), indices1[j] );
indices1[j] = i - indices.begin();
gradient[indices1[j]] += val2 * grad1[j];
}
for (j = 0; j < indices2.size(); ++j)
{
i = std::lower_bound( indices.begin(), indices.end(), indices2[j] );
indices2[j] = i - indices.begin();
gradient[indices2[j]] += val1 * grad2[j];
}
// allocate space for hessians, and zero it
const size_t N = indices.size();
Hessian.clear();
Hessian.resize( N * (N+1) / 2, Matrix3D(0.0) );
// add hessian terms from first metric
n = indices1.size();
h = 0;
for (r = 0; r < n; ++r)
{
const size_t nr = indices1[r];
for (c = r; c < n; ++c)
{
const size_t nc = indices1[c];
Hess1[h] *= val2;
if (nr <= nc)
Hessian[N*nr - nr*(nr+1)/2 + nc] += Hess1[h];
else
Hessian[N*nc - nc*(nc+1)/2 + nr].plus_transpose_equal( Hess1[h] );
++h;
}
}
// add hessian terms from second metric
n = indices2.size();
h = 0;
for (r = 0; r < n; ++r)
{
const size_t nr = indices2[r];
for (c = r; c < n; ++c)
{
const size_t nc = indices2[c];
Hess2[h] *= val1;
if (nr <= nc)
Hessian[N*nr - nr*(nr+1)/2 + nc] += Hess2[h];
else
Hessian[N*nc - nc*(nc+1)/2 + nr].plus_transpose_equal( Hess2[h] );
++h;
}
}
// add gradient outer products
n = indices1.size();
size_t m = indices2.size();
Matrix3D outer;
for (r = 0; r < n; ++r)
{
const size_t nr = indices1[r];
for (c = 0; c < m; ++c)
{
const size_t nc = indices2[c];
outer.outer_product( grad1[r], grad2[c] );
if (nr == nc)
Hessian[N*nr - nr*(nr+1)/2 + nc] += outer.plus_transpose_equal(outer);
else if (nr < nc)
Hessian[N*nr - nr*(nr+1)/2 + nc] += outer;
else
Hessian[N*nc - nc*(nc+1)/2 + nr].plus_transpose_equal(outer);
}
}
value = val1 * val2;
return rval1 && rval2;
}
void test_plus_transpose_equal()
{
mMat1.plus_transpose_equal(mMat2);
CPPUNIT_ASSERT( mMat1==mMat1plus2trans );
}