double distance2(struct point* p, struct point* p1, struct point* p2)
{
struct point proj;
double length2;
double tangent;
length2 = length_squared(p1, p2);
if ( length2 == 0.0 ) {
// p1 == p2
return length_squared(p, p1);
}
tangent = dot(p, p1, p2);
tangent = tangent / length2;
if ( tangent < 0.0 ) {
// Beyond p1
return length_squared(p, p1);
} else if ( tangent > 1.0 ) {
// Beyond p2
return length_squared(p, p2);
}
projection(&proj, p1, p2, tangent);
return length_squared(p, &proj);
}
Point SphereShape::sample_point_eye(const Point& eye,
float u1, float u2, Normal& out_n) const
{
float dist_squared = length_squared(eye.v);
if(dist_squared - this->radius * this->radius < 1e-3) {
float ray_epsilon;
return this->sample_point(u1, u2, out_n, ray_epsilon);
}
float dist = sqrt(dist_squared);
float inv_dist = 1.f / dist;
float cos_theta_max = sqrt(dist_squared - this->radius * this->radius) * inv_dist;
Vec3 cone_vec = uniform_cone_sample(cos_theta_max, u1, u2);
Vector cone_z = -Vector(eye.v) * inv_dist;
Vector cone_x, cone_y;
coordinate_system(cone_z, cone_x, cone_y);
Vector ray_dir = cone_x * cone_vec.x + cone_y * cone_vec.y + cone_z * cone_vec.z;
Ray ray(eye, ray_dir);
float t_hit;
if(!this->solve_hit_t(ray, t_hit)) {
t_hit = dist;
}
Point pt = ray.point_t(t_hit);
out_n = Normal(pt.v) / this->radius;
return pt;
}
void RunCommonVectorOrQuaternionTests(std::string const& typeName)
{
RunCommonArithmeticTests<T>(typeName);
RunPerfTest<T, T>(typeName + " operator/", [](T* value, T const& param)
{
*value /= param;
});
RunPerfTest<float, T>(typeName + " length", [](float* value, T const& param)
{
*value += length(param);
});
RunPerfTest<float, T>(typeName + " length_squared", [](float* value, T const& param)
{
*value += length_squared(param);
});
RunPerfTest<T, T>(typeName + " dot", [](T* value, T const& param)
{
value->x = dot(*value, param);
});
RunPerfTest<T, T>(typeName + " normalize", [](T* value, T& param)
{
auto t = param;
param = *value;
*value = normalize(t);
});
}
void assign_formation(const box_formation& formation, units& group)
{
auto down = -normalize(formation.direction) * formation.spacing;
auto right = vec2f{-down[1], down[0]};
int w_count = formation.width / formation.spacing;
auto lu_corner = formation.origin - right * w_count * 0.5f;
std::vector<bool> selected(group.size(), false);
for (int y = 0; std::count(selected.begin(), selected.end(), false) > 0; ++y)
{
for (int x = 0; x != w_count; ++x)
{
auto goal = lu_corner + down * y + right * x;
float distance = std::numeric_limits<float>::max();
std::size_t found = 0;
for (std::size_t i = 0; i != group.size(); ++i)
if (not selected[i])
{
float distance2 = length_squared(group[i].position - goal);
if (distance2 >= distance)
continue;
distance = distance2;
found = i;
}
if (distance == std::numeric_limits<float>::max())
return;
group[found].target_position = goal;
selected[found] = true;
}
}
}
Spectrum PointLight::sample_radiance(const Point& eye, float eye_epsilon,
Vector& out_wi, float& out_pdf, ShadowTest& out_shadow,
LightSample) const
{
out_wi = normalize(this->pt - eye);
out_pdf = 1.f;
out_shadow.init_point_point(eye, eye_epsilon, this->pt, 0.f);
return this->intensity / length_squared(this->pt - eye);
}
float SphereShape::point_eye_pdf(const Point& eye, const Vector& w) const {
float dist_squared = length_squared(eye.v);
if(dist_squared - this->radius * this->radius < 1e-3) {
return Shape::point_eye_pdf(eye, w);
}
float dist = sqrt(dist_squared);
float inv_dist = 1.f / dist;
float cos_theta_max = sqrt(dist_squared - this->radius * this->radius) * inv_dist;
return uniform_cone_pdf(cos_theta_max);
}
sf::Vector2f QuantumField::friction(const PointMass& caller) const
{
if (fp::not_equal(length_squared(caller.velocity), 0.f))
{
return unit(caller.velocity) * (-1.f) * 10.f;
}
else
{
return {0.f, 0.f};
}
}
void TerminationCriterion::accumulate_inner( PatchData& pd,
double of_value,
Vector3D* grad_array,
MsqError& err )
{
//if terminating on the norm of the gradient
//currentGradL2NormSquared = HUGE_VAL;
if (terminationCriterionFlag & (GRADIENT_L2_NORM_ABSOLUTE | GRADIENT_L2_NORM_RELATIVE))
{
currentGradL2NormSquared = length_squared(grad_array, pd.num_free_vertices()); // get the L2 norm
MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << " o Info -- gradient L2 norm: " << " "
<< RPM(std::sqrt(currentGradL2NormSquared)) << std::endl;
}
//currentGradInfNorm = 10e6;
if (terminationCriterionFlag & (GRADIENT_INF_NORM_ABSOLUTE | GRADIENT_INF_NORM_RELATIVE))
{
currentGradInfNorm = Linf(grad_array, pd.num_free_vertices()); // get the Linf norm
MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << " o Info -- gradient Inf norm: " << " "
<< RPM(currentGradInfNorm) << std::endl;
}
if (terminationCriterionFlag & VERTEX_MOVEMENT_RELATIVE)
{
maxSquaredInitialMovement = pd.get_max_vertex_movement_squared(
initialVerticesMemento, err ); MSQ_ERRRTN(err);
MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << " o Info -- max initial vertex movement: " << " "
<< RPM(maxSquaredInitialMovement) << std::endl;
}
previousOFValue = currentOFValue;
currentOFValue = of_value;
if (terminationCriterionFlag & OF_FLAGS) {
MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << " o Info -- OF Value: " << " " << RPM(of_value) << " iterationCounter= " << iterationCounter << std::endl;
}
else if (grad_array) {
MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << " o OF Value: " << " " << RPM(of_value) << " iterationCounter= " << iterationCounter
//<< " terminationCriterionFlag= " << terminationCriterionFlag << " OF_FLAGS = " << OF_FLAGS
<< std::endl;
}
++iterationCounter;
if (timeStepFileType)
write_timestep( pd, grad_array, err);
if (plotFile.is_open())
plotFile << iterationCounter
<< '\t' << mTimer.since_birth()
<< '\t' << of_value
<< '\t' << std::sqrt( currentGradL2NormSquared )
<< '\t' << currentGradInfNorm
<< '\t' << (maxSquaredMovement > 0.0 ? std::sqrt( maxSquaredMovement ) : 0.0)
<< '\t' << globalInvertedCount
<< std::endl;
}
float Shape::point_eye_pdf(const Point& eye, const Vector& w) const {
DiffGeom diff_geom;
float t_hit;
float ray_epsilon;
Ray ray(eye, w, 0.f);
if(!this->hit(ray, t_hit, ray_epsilon, diff_geom)) {
return 0.f;
}
return length_squared(ray.point_t(t_hit) - eye) /
(abs_dot(w, diff_geom.nn) * this->area());
}
void add_points(Sphere& s, u32 num, u32 stride, const void* points)
{
float rr = s.r*s.r;
const char* pts = (const char*)points;
for (u32 i = 0; i < num; ++i, pts += stride)
{
const Vector3& pi = *(const Vector3*)pts;
rr = fmax(rr, length_squared(pi - s.c));
}
s.r = sqrtf(rr);
}
void add_spheres(Sphere& s, u32 num, const Sphere* spheres)
{
for (u32 i = 0; i < num; ++i)
{
const Sphere& si = spheres[i];
const f32 dist = length_squared(si.c - s.c);
if (dist < (si.r + s.r) * (si.r + s.r))
{
if (si.r*si.r > s.r*s.r)
s.r = sqrtf(dist + si.r*si.r);
}
}
}
struct point { T x, y; };ttt point<T> operator + (pca a, pca b) { return { a.x+b.x, a.y+b.y }; }ttt point<T> operator - (pca a, pca b) { return { a.x-b.x, a.y-b.y }; }ttt point<T> operator - (pca a) { return { -a.x, -a.y }; }ttt point<T> operator * (T a, pca b) { return { a*b.x, a*b.y }; }ttt std::pair<T,T> to_pair(pca a) { return { a.x, a.y }; }ttt bool operator == (pca a, pca b) { return to_pair(a) == to_pair(b); }ttt bool operator != (pca a, pca b) { return to_pair(a) != to_pair(b); }ttt bool operator < (pca a, pca b) { return to_pair(a) < to_pair(b); }ttt bool operator <= (pca a, pca b) { return to_pair(a) <= to_pair(b); }ttt bool operator >= (pca a, pca b) { return to_pair(a) >= to_pair(b); }ttt bool operator > (pca a, pca b) { return to_pair(a) > to_pair(b); }ttt T length_squared(pca p) { return p.x*p.x + p.y*p.y; }ttt double length(pca p) { return sqrt(length_squared(p)); }ttt point<T> normalized(pca a) { return (1 / length(a)) * a; }ttt T dot(pca p, pca q) { return p.x * q.x + p.y * q.y; }ttt T cross(pca p, pca q) { return p.x * q.y - p.y * q.x; }ttt int ccw(pca a, pca b, pca c) { T x = cross(b - a, c - a); return x > 0 ? 1 : x < 0 ? -1 : 0; }
float VectorT<T>::length() const
{
return sqrt(length_squared());
}
const double Vector2d::length() const{
return sqrt(length_squared());
}
void QuasiNewton::optimize_vertex_positions( PatchData& pd, MsqError& err )
{
TerminationCriterion& term = *get_inner_termination_criterion();
OFEvaluator& func = get_objective_function_evaluator();
const double sigma = 1e-4;
const double beta0 = 0.25;
const double beta1 = 0.80;
const double tol1 = 1e-8;
const double epsilon = 1e-10;
double norm_r; //, norm_g;
double alpha, beta;
double obj, objn;
size_t i;
// Initialize stuff
const size_t nn = pd.num_free_vertices();
double a[QNVEC], b[QNVEC], r[QNVEC];
for (i = 0; i < QNVEC; ++i)
r[i] = 0;
for (i = 0; i <= QNVEC; ++i) {
v[i].clear();
v[i].resize( nn, Vector3D(0.0) );
w[i].clear();
w[i].resize( nn, Vector3D(0.0) );
}
d.resize( nn );
mHess.resize( nn ); //hMesh(mesh);
bool valid = func.update( pd, obj, v[QNVEC], mHess, err ); MSQ_ERRRTN(err);
if (!valid) {
MSQ_SETERR(err)("Initial objective function is not valid", MsqError::INVALID_MESH);
return;
}
while (!term.terminate()) {
pd.recreate_vertices_memento( mMemento, err ); MSQ_ERRRTN(err);
pd.get_free_vertex_coordinates( w[QNVEC] );
x = v[QNVEC];
for (i = QNVEC; i--; ) {
a[i] = r[i] * inner( &(w[i][0]), arrptr(x), nn );
plus_eq_scaled( arrptr(x), -a[i], &v[i][0], nn );
}
solve( arrptr(d), arrptr(x) );
for (i = QNVEC; i--; ) {
b[i] = r[i] * inner( &(v[i][0]), arrptr(d), nn );
plus_eq_scaled( arrptr(d), a[i]-b[i], &(w[i][0]), nn );
}
alpha = -inner( &(v[QNVEC][0]), arrptr(d), nn ); /* direction is negated */
if (alpha > 0.0) {
MSQ_SETERR(err)("No descent.", MsqError::INVALID_MESH);
return;
}
alpha *= sigma;
beta = 1.0;
pd.move_free_vertices_constrained( arrptr(d), nn, -beta, err ); MSQ_ERRRTN(err);
valid = func.evaluate( pd, objn, v[QNVEC], err );
if (err.error_code() == err.BARRIER_VIOLATED)
err.clear(); // barrier violated does not represent an actual error here
MSQ_ERRRTN(err);
if (!valid ||
(obj - objn < -alpha*beta - epsilon &&
length( &(v[QNVEC][0]), nn ) >= tol1)) {
if (!valid) // function not defined at trial point
beta *= beta0;
else // unacceptable iterate
beta *= beta1;
for (;;) {
if (beta < tol1) {
pd.set_to_vertices_memento( mMemento, err ); MSQ_ERRRTN(err);
MSQ_SETERR(err)("Newton step not good", MsqError::INTERNAL_ERROR);
return;
}
pd.set_free_vertices_constrained( mMemento, arrptr(d), nn, -beta, err ); MSQ_ERRRTN(err);
valid = func.evaluate( pd, objn, err );
if (err.error_code() == err.BARRIER_VIOLATED)
err.clear(); // barrier violated does not represent an actual error here
MSQ_ERRRTN(err);
if (!valid) // function undefined at trial point
beta *= beta0;
else if (obj - objn < -alpha*beta - epsilon) // unacceptlable iterate
beta *= beta1;
else
break;
}
}
for (i = 0; i < QNVEC-1; ++i) {
r[i] = r[i+1];
w[i].swap( w[i+1] );
v[i].swap( v[i+1] );
}
w[QNVEC-1].swap( w[0] );
v[QNVEC-1].swap( v[0] );
func.update( pd, obj, v[QNVEC], mHess, err ); MSQ_ERRRTN(err);
norm_r = length_squared( &(v[QNVEC][0]), nn );
//norm_g = sqrt(norm_r);
// checks stopping criterion
term.accumulate_patch( pd, err ); MSQ_ERRRTN(err);
term.accumulate_inner( pd, objn, &v[QNVEC][0], err ); MSQ_ERRRTN(err);
}
}
int calc_hb(char o_str[256],int i, int j)
{
int l, /*ri, rj, gap,*/ h, near_h;
/*char bndtyp[3], acctyp[4], dontyp[4], space=' ';*/
float /*ca_d,*/ d, ha2,/*IM*/tmp_d, hd, ha, haaa_ang, daaa_ang;
struct vect nearest, tmp_point, aromatic_axis, aa[MAXCON] /* acceptor antecedants*/;
char buf1[20],buf2[20]; /* buffers for atomid routines */
/* void find_ss()*/
/* {*/
/* int i,j;*/
/* float d;*/
short aromatic_flg;
*o_str=0;
/* if(i==203 && j==210)
{
printf("Testing from %s",atomid(i,buf1));
printf(" to %s at start of calc_hb.\n",atomid(j,buf1));
debug=0;
}
else
debug=0;
*/
d = length_squared( to(atom[i].p, atom[j].p) );
if (debug==2)
printf("don-acc distance ^2 = %f\n",d);
if (d <= SQR(HBDIST))
/* these are nearby ! */; /*IM begins*/
else return(0);
if (debug) printf("Acceptable distance\n");
/* time to fill the acceptor array */
daaa_ang= -99.9; /* reset it . . . . */
aromatic_flg = ( atom[j].aacode < TOTNAA )?(accepts[atom[j].aacode][atom[j].atmtyp] < 0):0;
if (!aromatic_flg)
{
for(l=0;l<MAXCON;l++)
{
if (icon[j][l]>-1)
{
float daaa_tmp,aaa;
char buf1[20],buf2[20];
aa[l]=atom[icon[j][l] ].p;
daaa_tmp= dot_product(
to( aa[l], atom[j].p ), to( atom[i].p, atom[j].p ) ); /* = DA * A-AA * cos D-A-AA */
aaa=vector_length ( to(aa[l], atom[j].p) );
daaa_tmp/= aaa * sqrt( d); /* = cos D-A-AA */
/* d= DA^2 */
if (daaa_tmp>daaa_ang) daaa_ang=daaa_tmp; /* select lowest */
if (aaa<0.8 || aaa > 6.0)
printf("WARNING: %5.2fA between %s and %s ((D..)A-AA bond).\n", aaa,atomid(icon[j][l],buf1),atomid(j,buf2));
}
}
if (daaa_ang > cosMIN_DAAA && daaa_ang>-99.9)
if (!nnbflg)
return(0);
/*NB -99 was not originally used as a default whilst checking
because we were wrongly trying to find a lowest possible
value */
/* printf("lowest daaa_ang %6.1f\n",daaa_ang);*/
}
else /*aromatic_hbond*/
{
float daaa_tmp;
/*if( !strncmp("4DFR/A0030-TRP CE3",atomid(j,buf1),18))
debug=1;*/
if (debug){
printf("NOTE: %s is aromatic.\n",atomid(j,buf1));
printf(" connects to ");
for(l=0;l< atom[j].ncon;l++)
printf("%s ",atomid(icon[j][l],buf1));
printf("\n");
}
if ( accepts[atom[j].aacode][atom[j].atmtyp] != -1 )
printf("BUG: %s not registered as aromatic acceptor in \"aromatic_hbond\".\n",atomid(j,buf1));
if ( atom[j].ncon!=2 && atom[j].ncon!=3 )
printf("BUG: %s has connectivity %d but is registered as aromatic.\n",atomid(j,buf1),atom[j].ncon);
if ( atom[j].ncon==3 )
{
aromatic_axis = unit_vector(
vector_plus( perpendicular( atom[ icon[j][0] ].p, atom[j].p, atom[ icon[j][1] ].p),
vector_plus( perpendicular( atom[ icon[j][0] ].p, atom[j].p, atom[ icon[j][2] ].p),
perpendicular( atom[ icon[j][1] ].p, atom[j].p, atom[ icon[j][2] ].p) ) ) );
}
else
aromatic_axis = unit_vector(perpendicular( atom[ icon[j][0] ].p, atom[j].p, atom[ icon[j][1] ].p));
aa[0]= vector_plus( aromatic_axis, atom[j].p );
aa[1]= vector_plus( float_times_vect( -1.0, aromatic_axis), atom[j].p );
if (debug) printf("Perpendicular gives angles %5.3f and %5.3f with 1,2 / %d.\n",angle( atom[ icon[j][0] ].p, atom[j].p, aa[0] ), angle( atom[ icon[j][1] ].p, atom[j].p, aa[0] ) , atom[j].ncon);
for(l=0;l<2;l++)
{
daaa_tmp= dot_product( to( aa[l], atom[j].p), to( atom[i].p, atom[j].p ) ); /*=DA*A-AA*cosD-A-AA */
daaa_tmp /= sqrt(d); /* = cos D-A-AX */
if ( daaa_tmp > daaa_ang ) daaa_ang=daaa_tmp; /* select high values of cos D-A-AX ie low, better angles*/
if (debug) printf("Cosine of D-A-AX = %5.3f\n",daaa_tmp);
if (debug) {
printf("AX co-ods");
printf(TF,VXYZ( aa[l] ) );
printf("\n");
}
}
if (daaa_ang < cosMAX_DAAX)
{
if (debug)
printf("Rejected because DAAX too high.\n");
if (!nnbflg)
return(0);
}
}
#define nearer(vector) { tmp_d=length_squared( to( atom[j].p, vector ) ); if (ha2>tmp_d) { ha2=tmp_d; nearest=vector; near_h = h;} }
/* find the nearest possible H position on donor i */
ha2=100;
/* if (i==362) debug=1;
else debug=0;*/
if (debug && atom[i].hetflg) printf("H on %s %d ",atom[i].resnam, atom[i].aanum);
if (debug) printf("%d Hs positioned\n",atom[i].nh);
for(h=atom[i].h_ptr;h<atom[i].nh+atom[i].h_ptr;h++)
{
if (debug)
printf("Hydrogen %d - type %d\n",atom[i].h_ptr,h_atm[h].typ);
if (debug) printf("atom[i].nh %d, ].h_ptr %d, h %d\n",atom[i].nh,atom[i].h_ptr,h);
switch (h_atm[h].typ)
{
case 0:
if (debug) printf("Atom %d (%s%s %d) has H %d missing\n",i,atom[i].atmnam,atom[i].resnam, atom[i].aanum, h);
/* no H present, must be HETATM */
;/* used to be ha2=1, cannot decide why */
break;
case alternatives:
nearer( h_atm[h].a );
if (debug) printf("Checked alternative position: ha2 = %f\n",ha2);
case fixed:
nearer( h_atm[h].p );
if (debug) printf("Checked default position: ha2 = %f\n",ha2);
break;
case circle:
tmp_point= intersect(to(atom[i].p,h_atm[h].a),
h_atm[h].p, atom[i].p, atom[j].p );
tmp_point= onto_sphere(tmp_point, h_atm[h].a,
vector_length( to(h_atm[h].a, h_atm[h].p) ));
nearer(tmp_point);
tmp_point= vector_plus( tmp_point,
float_times_vect(2,to(tmp_point, h_atm[h].a)));
nearer(tmp_point);
break;
default:
printf("\nBUG: Unforseen htyp for atom %s in find_hb.\n", atomid(i,buf1));
printf("Please mail [email protected]\n");
}/* end of switch*/
}/* end of h loop */
if (ha2<100) /* ie if any Hydrogens have been found and
their positions taken. Or, to put it
another way, if it anything other than a hetatm
which is flagged in .nh as having hydrogens but
has no positons in h_atm[].typ. */
{
hd=vector_length( to(nearest, atom[i].p));
ha=vector_length( to(nearest, atom[j].p));
if (hd<0.9 || hd > 2.0)
{
printf("WARNING: %5.2fA between %s and %d (H).\n", hd, atomid(i,buf1),near_h-atom[i].h_ptr+1);
if (debug) printf("atom[i].nh %d, ].h_ptr %d, h %d\n",atom[i].nh,atom[i].h_ptr,h);
}
if (debug) printf("hd=%f, ha=%f\n",hd,ha);
if (!nnbflg)
{
if (ha>MAX_HA)
return(0);
if (debug)
printf("HA length OK\n");
if (dot_product( to(nearest, atom[i].p),
to(nearest, atom[j].p) ) >= hd*ha*cosMIN_DHA)
return(0);
}
haaa_ang=100.0; /* reset it . . . . */
if (!aromatic_flg)
{
for(l=0;l<MAXCON;l++)
{
if (icon[j][l]>-1)
{
float haaa_tmp,aaa;
aa[l]=atom[icon[j][l] ].p;
haaa_tmp= dot_product(
to(atom[j].p, aa[l] ), to( atom[j].p, nearest ) );
aaa= vector_length( to(aa[l],atom[j].p) );
haaa_tmp/= aaa *ha;
if (haaa_tmp<haaa_ang) haaa_ang=haaa_tmp; /* select lowest */
if (aaa<0.8 || aaa > 6.0)
printf("WARNING: %5.2fA between %s and %s ((H..)A-AA).\n", aaa,atomid(j,buf1),atomid(icon[j][l],buf2));
}
}
if ((haaa_ang > cosMIN_HAAA && haaa_ang<100.0) && !nnbflg)
return(0);
else
if (haaa_ang==100.0)
haaa_ang= -99.9;
/*NB -99 could not have been used as a default whilst checking
because we are trying to find a lowest possible value */
if (debug)
printf("Angle OK\n");
}
else
{
float haaa_tmp;
haaa_ang= -99; /*dealing with hAax, prefer small, ie big cosines*/
for(l=0;l<2;l++)
{
haaa_tmp=dot_product( to(atom[j].p,aa[l]),to(atom[j ].p,nearest));
haaa_tmp/= ha;
if (debug) printf("Cosine of H-A-AX = %5.3f\n",haaa_tmp);
if (haaa_tmp > haaa_ang) {
if (debug) printf("%5.3f > %5.3f.\n",haaa_tmp,haaa_ang);
haaa_ang=haaa_tmp; /* select the low ie more meaningful */
if (debug) printf("Lowest yet.\n");
}
}
if (haaa_ang < cosMAX_HAAX /* && haaa_ang<100.0 */ )
{
if (debug) {
printf("Rejected because of wide HAAX angle.\n");
printf("Cos haaa_ang = %5.3f, cosMAX_HAAX = %5.3f.\n", haaa_ang, cosMAX_HAAX);
}
if (!nnbflg)
return(0);
}
}
}/* end of 'if any Hs actually found' clause */
else
if (d>SQR(MAX_HA+1.0))
if (!nnbflg)
return(0); /* if the hydrogens are unfixed, knock
anything more than 1.0 Ang - the
normal DH distance - back. */
sprintf_hb(o_str,i,j,nearest,d,ha,daaa_ang,haaa_ang);
return(1);
}
void TrustRegion::optimize_vertex_positions( PatchData& pd, MsqError& err )
{
TerminationCriterion& term = *get_inner_termination_criterion();
OFEvaluator& func = get_objective_function_evaluator();
const double cg_tol = 1e-2;
const double eta_1 = 0.01;
const double eta_2 = 0.90;
const double tr_incr = 10;
const double tr_decr_def = 0.25;
const double tr_decr_undef = 0.25;
const double tr_num_tol = 1e-6;
const int max_cg_iter = 10000;
double radius = 1000; /* delta*delta */
const int nn = pd.num_free_vertices();
wVect.resize(nn); Vector3D* w = arrptr(wVect);
zVect.resize(nn); Vector3D* z = arrptr(zVect);
dVect.resize(nn); Vector3D* d = arrptr(dVect);
pVect.resize(nn); Vector3D* p = arrptr(pVect);
rVect.resize(nn); Vector3D* r = arrptr(rVect);
double norm_r, norm_g;
double alpha, beta, kappa;
double rz, rzm1;
double dMp, norm_d, norm_dp1, norm_p;
double obj, objn;
int cg_iter;
bool valid;
mHess.initialize( pd, err ); //hMesh(mesh);
valid = func.update( pd, obj, mGrad, mHess, err ); MSQ_ERRRTN(err);
if (!valid) {
MSQ_SETERR(err)("Initial objective function is not valid", MsqError::INVALID_MESH);
return;
}
compute_preconditioner( err ); MSQ_ERRRTN(err);
pd.recreate_vertices_memento( mMemento, err ); MSQ_ERRRTN(err);
while (!term.terminate() && (radius > 1e-20)) {
norm_r = length_squared(arrptr(mGrad), nn);
norm_g = sqrt(norm_r);
memset(d, 0, 3*sizeof(double)*nn);
memcpy(r, arrptr(mGrad), nn*sizeof(Vector3D)); //memcpy(r, mesh->g, 3*sizeof(double)*nn);
norm_g *= cg_tol;
apply_preconditioner( z, r, err); MSQ_ERRRTN(err); //prec->apply(z, r, prec, mesh);
negate(p, z, nn);
rz = inner(r, z, nn);
dMp = 0;
norm_p = rz;
norm_d = 0;
cg_iter = 0;
while ((sqrt(norm_r) > norm_g) &&
#ifdef DO_STEEP_DESC
(norm_d > tr_num_tol) &&
#endif
(cg_iter < max_cg_iter))
{
++cg_iter;
memset(w, 0, 3*sizeof(double)*nn);
//matmul(w, mHess, p); //matmul(w, mesh, p);
mHess.product( w, p );
kappa = inner(p, w, nn);
if (kappa <= 0.0) {
alpha = (sqrt(dMp*dMp+norm_p*(radius-norm_d))-dMp)/norm_p;
plus_eq_scaled( d, alpha, p, nn );
break;
}
alpha = rz / kappa;
norm_dp1 = norm_d + 2.0*alpha*dMp + alpha*alpha*norm_p;
if (norm_dp1 >= radius) {
alpha = (sqrt(dMp*dMp+norm_p*(radius-norm_d))-dMp)/norm_p;
plus_eq_scaled( d, alpha, p, nn );
break;
}
plus_eq_scaled( d, alpha, p, nn );
plus_eq_scaled( r, alpha, w, nn );
norm_r = length_squared(r, nn);
apply_preconditioner( z, r, err); MSQ_ERRRTN(err); //prec->apply(z, r, prec, mesh);
rzm1 = rz;
rz = inner(r, z, nn);
beta = rz / rzm1;
times_eq_minus( p, beta, z, nn );
dMp = beta*(dMp + alpha*norm_p);
norm_p = rz + beta*beta*norm_p;
norm_d = norm_dp1;
}
#ifdef DO_STEEP_DESC
if (norm_d <= tr_num_tol) {
norm_g = length(arrptr(mGrad), nn);
double ll = 1.0;
if (norm_g < tr_num_tol)
break;
if (norm_g > radius)
ll = radius / nurm_g;
for (int i = 0; i < nn; ++i)
d[i] = ll * mGrad[i];
}
#endif
alpha = inner( arrptr(mGrad), d, nn ); // inner(mesh->g, d, nn);
memset(p, 0, 3*sizeof(double)*nn);
//matmul(p, mHess, d); //matmul(p, mesh, d);
mHess.product( p, d );
beta = 0.5*inner(p, d, nn);
kappa = alpha + beta;
/* Put the new point into the locations */
pd.move_free_vertices_constrained( d, nn, 1.0, err ); MSQ_ERRRTN(err);
valid = func.evaluate( pd, objn, err ); MSQ_ERRRTN(err);
if (!valid) {
/* Function not defined at trial point */
radius *= tr_decr_undef;
pd.set_to_vertices_memento( mMemento, err ); MSQ_ERRRTN(err);
continue;
}
if ((fabs(kappa) <= tr_num_tol) && (fabs(objn - obj) <= tr_num_tol)) {
kappa = 1;
}
else {
kappa = (objn - obj) / kappa;
}
if (kappa < eta_1) {
/* Iterate is unacceptable */
radius *= tr_decr_def;
pd.set_to_vertices_memento( mMemento, err ); MSQ_ERRRTN(err);
continue;
}
/* Iterate is acceptable */
if (kappa >= eta_2) {
/* Iterate is a very good step, increase radius */
radius *= tr_incr;
if (radius > 1e20) {
radius = 1e20;
}
}
func.update( pd, obj, mGrad, mHess, err );
compute_preconditioner( err ); MSQ_ERRRTN(err);
pd.recreate_vertices_memento( mMemento, err ); MSQ_ERRRTN(err);
// checks stopping criterion
term.accumulate_patch( pd, err ); MSQ_ERRRTN(err);
term.accumulate_inner( pd, objn, arrptr(mGrad), err ); MSQ_ERRRTN(err);
}
}
/*!Reset function using using a PatchData object. This function is
called for the inner-stopping criterion directly from the
loop over mesh function in VertexMover. For outer criterion,
it is called from the reset function which takes a MeshSet object.
This function prepares the object to be used by setting the initial
values of some of the data members. As examples, if needed, it resets
the cpu timer to zero, the iteration counter to zero, and the
initial and previous objective function values to the current
objective function value for this patch.
The return value for this function is similar to that of terminate().
The function returns false if the checked criteria have not been
satisfied, and true if they have been. reset() only checks the
GRADIENT_INF_NORM_ABSOLUTE, GRADIENT_L2_NORM_ABSOLUTE, and the
QUALITY_IMPROVEMENT_ABSOLUTE criteria. Checking these criteria
allows the QualityImprover to skip the entire optimization if
the initial mesh satisfies the appropriate conditions.
*/
void TerminationCriterion::reset_inner(PatchData &pd, OFEvaluator& obj_eval,
MsqError &err)
{
const unsigned long totalFlag = terminationCriterionFlag | cullingMethodFlag;
// clear flag for BOUNDED_VERTEX_MOVEMENT
vertexMovementExceedsBound = 0;
// Use -1 to denote that this isn't initialized yet.
// As all valid values must be >= 0.0, a negative
// value indicates that it is uninitialized and is
// always less than any valid value.
maxSquaredMovement = -1;
// Clear the iteration count.
iterationCounter = 0;
//reset the inner timer if needed
if(totalFlag & CPU_TIME){
mTimer.reset();
}
//GRADIENT
currentGradInfNorm = initialGradInfNorm = 0.0;
currentGradL2NormSquared = initialGradL2NormSquared = 0.0;
if(totalFlag & GRAD_FLAGS)
{
if (!obj_eval.have_objective_function()) {
MSQ_SETERR(err)("Error termination criteria set which uses objective "
"functions, but no objective function is available.",
MsqError::INVALID_STATE);
return;
}
int num_vertices=pd.num_free_vertices();
mGrad.resize( num_vertices );
//get gradient and make sure it is valid
bool b = obj_eval.evaluate(pd, currentOFValue, mGrad, err); MSQ_ERRRTN(err);
if (!b) {
MSQ_SETERR(err)("Initial patch is invalid for gradient computation.",
MsqError::INVALID_STATE);
return;
}
//get the gradient norms
if (totalFlag & (GRADIENT_INF_NORM_ABSOLUTE|GRADIENT_INF_NORM_RELATIVE))
{
currentGradInfNorm = initialGradInfNorm = Linf(mGrad);
MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << " o Initial gradient Inf norm: " << " "
<< RPM(initialGradInfNorm) << std::endl;
}
if (totalFlag & (GRADIENT_L2_NORM_ABSOLUTE|GRADIENT_L2_NORM_RELATIVE))
{
currentGradL2NormSquared = initialGradL2NormSquared = length_squared(mGrad);
MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << " o Initial gradient L2 norm: " << " "
<< RPM(std::sqrt(initialGradL2NormSquared)) << std::endl;
}
//the OFvalue comes for free, so save it
previousOFValue=currentOFValue;
initialOFValue=currentOFValue;
}
//find the initial objective function value if needed and not already
//computed. If we needed the gradient, we have the OF value for free.
// Also, if possible, get initial OF value if writing plot file. Solvers
// often supply the OF value for subsequent iterations so by calculating
// the initial value we can generate OF value plots.
else if ((totalFlag & OF_FLAGS) ||
(plotFile.is_open() && pd.num_free_vertices() && obj_eval.have_objective_function()))
{
//ensure the obj_ptr is not null
if(!obj_eval.have_objective_function()){
MSQ_SETERR(err)("Error termination criteria set which uses objective "
"functions, but no objective function is available.",
MsqError::INVALID_STATE);
return;
}
bool b = obj_eval.evaluate(pd, currentOFValue, err); MSQ_ERRRTN(err);
if (!b){
MSQ_SETERR(err)("Initial patch is invalid for evaluation.",MsqError::INVALID_STATE);
return;
}
//std::cout<<"\nReseting initial of value = "<<initialOFValue;
previousOFValue=currentOFValue;
initialOFValue=currentOFValue;
}
if (totalFlag & (GRAD_FLAGS|OF_FLAGS))
MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << " o Initial OF value: " << " " << RPM(initialOFValue) << std::endl;
// Store current vertex locations now, because we'll
// need them later to compare the current movement with.
if (totalFlag & VERTEX_MOVEMENT_RELATIVE)
{
if (initialVerticesMemento)
{
pd.recreate_vertices_memento( initialVerticesMemento, err );
}
else
{
initialVerticesMemento = pd.create_vertices_memento( err );
}
MSQ_ERRRTN(err);
maxSquaredInitialMovement = DBL_MAX;
}
else {
maxSquaredInitialMovement = 0;
}
if (terminationCriterionFlag & UNTANGLED_MESH) {
globalInvertedCount = count_inverted( pd, err );
//if (innerOuterType==TYPE_OUTER) MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << " o Num Inverted: " << " " << globalInvertedCount << std::endl;
patchInvertedCount = 0;
MSQ_ERRRTN(err);
}
if (timeStepFileType) {
// If didn't already calculate gradient abive, calculate it now.
if (!(totalFlag & GRAD_FLAGS)) {
mGrad.resize( pd.num_free_vertices() );
obj_eval.evaluate(pd, currentOFValue, mGrad, err);
err.clear();
}
write_timestep( pd, mGrad.empty() ? 0 : arrptr(mGrad), err);
}
if (plotFile.is_open()) {
// two newlines so GNU plot knows that we are starting a new data set
plotFile << std::endl << std::endl;
// write column headings as comment in data file
plotFile << "#Iter\tCPU\tObjFunc\tGradL2\tGradInf\tMovement\tInverted" << std::endl;
// write initial values
plotFile << 0
<< '\t' << mTimer.since_birth()
<< '\t' << initialOFValue
<< '\t' << std::sqrt( currentGradL2NormSquared )
<< '\t' << currentGradInfNorm
<< '\t' << 0.0
<< '\t' << globalInvertedCount
<< std::endl;
}
}
void sprintf_hb(char * o_str, int i, int j, struct vect nearest, float d, float ha, float daaa_ang, float haaa_ang)
{
char bndtyp[3], acctyp[4], dontyp[4], space=' ';
int ca_d, ri, rj, gap;
char buf1[20],buf2[20]; /* buffers for atomid routines */
float tmp;
/*we have a hydrogen bond!*/
bndtyp[0] = bndtyp[1] = '?';
acctyp[3] = dontyp[3] = bndtyp[2] = '\0';
if (atom[i].aacode == TOTNAA || atom[i].hetflg )
bndtyp[0] = 'H';
else
{
if (atom[i].aacode <STDAA)
{
bndtyp[0] = (atom[i].atmtyp <=3) ? 'M' : 'S';
}
else
{
bndtyp[0] = (atom[i].atmtyp <=3) ? 'm' : 's';
}
}
if (atom[j].aacode == TOTNAA || atom[j].hetflg )
bndtyp[1] = 'H';
else
{
if (atom[j].aacode <STDAA)
{
bndtyp[1] = (atom[j].atmtyp <=3) ? 'M' : 'S';
}
else
{
bndtyp[1] = (atom[j].atmtyp <=3) ? 'm' : 's';
}
}
ri = atom[i].caindex;
rj = atom[j].caindex;
gap=find_gap( ri, rj);
#if 0
if (atom[i].aacode==TOTNAA || (ri == -99) || !residue[ri].ca || atom[j].aacode==TOTNAA || (rj == -99) || !residue[rj].ca )
gap = -1;
else
{
gap = cagap(ri,rj);
}
#endif
if (atom[i].aacode==TOTNAA || (ri == -99) || !residue[ri].ca ||
atom[j].aacode==TOTNAA || (rj == -99) || !residue[rj].ca )
ca_d = -1.0;
else
{
ca_d = length_squared( to(*(residue[ri].ca),*(residue[rj].ca)) );
if (ca_d > SQR(CAWARN) && debug)
{
printf("WARNING: unusual CA-CA distance of %5.2f for",sqrt(ca_d));
printf("%s number ??\n", (nnbflg)?" interaction":" H-bond"/*,nhb*/);
printf(" between %s and %s\n", atomid(i,buf1),atomid(j,buf2));
}
}
#if 0
printf("%4s", brcode);
printf("%c", (atom[i].chnid == '-') ? space : atom[i].chnid);
printf("%4s/", brcode);
printf("%c", atom[i].chnid);
printf("%04d", atom[i].aanum);
if(OMLINSERT && atom[i].hetflg)
printf("h");
else
printf("%c", atom[i].inscode);
/* printf("%3s", dontyp); */
printf("%3s", atom[i].resnam);
printf("%4s", atom[i].atmnam);
printf("%c", atom[i].strucsum);
printf("%c", (atom[j].chnid == '-') ? space : atom[j].chnid);
printf("%4s/", brcode);
printf("%c", atom[j].chnid);
printf("%04d", atom[j].aanum);
if(OMLINSERT && atom[j].hetflg)
printf("h");
else
printf("%c", atom[j].inscode);
/* printf("%3s", acctyp); */
printf("%3s", atom[j].resnam);
printf("%4s", atom[j].atmnam);
printf("%c", atom[j].strucsum);
printf("%4.1f", sqrt(d));
printf(" %2s", bndtyp);
printf("%4d", gap);
printf("%4.1f", (ca_d <= 0.0) ? -1.0 : sqrt(ca_d));
printf("%6.1d", 180/3.1415927*angle(atom[i].p,nearest,atom[j].p) );
printf("%4.1d", d);
printf("%6d\n", nhb);
#endif
*o_str = 0 ;
/*This isn't neccessary if the longoutput format is used, but if the short
format is used, then the first output must (i) include a 'strlen' in case
it is part of a 'long format' output, and (ii) have some way of knowing
where to begin in case of a 'short format' output. */
if (longoutflg)
{
/* start to output the line, *.hhb format */
sprintf(o_str , "%4s", brcode);
sprintf(o_str+strlen(o_str), "%c", (atom[i].chnid == '-') ? space :
atom[i].chnid);
sprintf(o_str+strlen(o_str), "%4s/", brcode);
}
sprintf(o_str+strlen(o_str), "%c", atom[i].chnid);
sprintf(o_str+strlen(o_str), "%04d", atom[i].aanum);
if(OMLINSERT && atom[i].hetflg) /*v3.13*/
sprintf(o_str+strlen(o_str), "h");
else
sprintf(o_str+strlen(o_str), "%c", atom[i].inscode);
/* sprintf(o_str+strlen(o_str), "%3s", dontyp); */
/* Amendment. RAL 14 Jun 2012 --> */
// if (atom[i].aacode==Cys && atom[i].atmtyp == 5 & cssflg)
if (atom[i].aacode==Cys && atom[i].atmtyp == 5 && cssflg)
/* <-- Amendment. RAL 14 Jun 2012 */
{
if (atom[i].ssflg)
sprintf(o_str+strlen(o_str),"CSS");
else
sprintf(o_str+strlen(o_str),"CYH"); }
else
sprintf(o_str+strlen(o_str), "%3s", atom[i].resnam);
sprintf(o_str+strlen(o_str), "%4s", atom[i].atmnam);
if (longoutflg)
{
sprintf(o_str+strlen(o_str), "%c", atom[i].strucsum);
sprintf(o_str+strlen(o_str), "%c", (atom[j].chnid == '-') ? space :
atom[j].chnid);
sprintf(o_str+strlen(o_str), "%4s/", brcode);
}
else
sprintf(o_str+strlen(o_str), " ");
sprintf(o_str+strlen(o_str), "%c", atom[j].chnid);
sprintf(o_str+strlen(o_str), "%04d", atom[j].aanum);
if(OMLINSERT && atom[j].hetflg) /*v3.13 start*/
sprintf(o_str+strlen(o_str),"h");
else /*v3.13 end*/
sprintf(o_str+strlen(o_str), "%c", atom[j].inscode);
/* sprintf(o_str+strlen(o_str), "%3s", acctyp); */
if (atom[j].aacode==Cys && atom[j].atmtyp == 5 && cssflg)
{
if (atom[j].ssflg)
sprintf(o_str+strlen(o_str),"CSS");
else
sprintf(o_str+strlen(o_str),"CYH"); }
else
sprintf(o_str+strlen(o_str), "%3s", atom[j].resnam);
sprintf(o_str+strlen(o_str), "%4s", atom[j].atmnam);
if (longoutflg)
sprintf(o_str+strlen(o_str), "%c", atom[j].strucsum);
sprintf(o_str+strlen(o_str), "%5.2f", sqrt(d));
if (!longoutflg)
sprintf(o_str+strlen(o_str), " ");
sprintf(o_str+strlen(o_str), "%2s", bndtyp);
sprintf(o_str+strlen(o_str), "%4d", gap);
if (!longoutflg)
sprintf(o_str+strlen(o_str), " ");
sprintf(o_str+strlen(o_str), "%5.2f", (ca_d <= 0.0) ? -1.0 : sqrt(ca_d));
if (!longoutflg)
sprintf(o_str+strlen(o_str), " ");
if (atom[i].nh && h_atm[atom[i].h_ptr].typ)
{
/* if it has hydrogens and if they have been placed */
tmp = angle(atom[i].p,nearest,atom[j].p);
sprintf(o_str+strlen(o_str), "%5.1f", (180.0/3.1415927)* tmp );
if (!longoutflg)
sprintf(o_str+strlen(o_str), " ");
sprintf(o_str+strlen(o_str), "%5.2f", ha);
if (!longoutflg)
sprintf(o_str+strlen(o_str), " ");
if (haaa_ang>-50.0)
sprintf(o_str+strlen(o_str),"%5.1f", 180/3.1415927* (float) acos(haaa_ang) );
else
sprintf(o_str+strlen(o_str),"%5.1f", -1.0);
}
else
if (longoutflg)
sprintf(o_str+strlen(o_str), "%5.1f%5.2f%5.1f", -1.0, -1.0, -1.0);
else
sprintf(o_str+strlen(o_str), "%5.1f %5.2f %5.1f", -1.0, -1.0, -1.0);
if (daaa_ang>-50.0)
sprintf(o_str+strlen(o_str),"%6.1f", 180/3.1415927*acos(daaa_ang) );
else
sprintf(o_str+strlen(o_str),"%6.1f", -1.0);
if (debug)
{
sprintf(o_str+strlen(o_str), "D " );
sprintf(o_str+strlen(o_str), TF,VXYZ(atom[i].p) );
sprintf(o_str+strlen(o_str), " H " );
sprintf(o_str+strlen(o_str), TF,VXYZ(nearest) );
sprintf(o_str+strlen(o_str), " A " );
sprintf(o_str+strlen(o_str), TF, VXYZ(atom[j].p) );
}
/* sprintf(o_str+strlen(o_str), "%6d\n", nhb);
THIS IS DONE BY THE CALLING ROUTINE */
/* end of outputting the line, *.hhb format */
return;
}
template <typename T> struct point { T x, y; };template <typename T> T length_squared(const point<T> & p) { return p.x*p.x + p.y*p.y; }template <typename T> double length(const point<T> & p) { return sqrt(length_squared(p)); }template <typename T> bool operator == (const point<T> & p, const point<T> & q) { return p.x == q.x and p.y == q.y; }template <typename T> bool operator != (const point<T> & p, const point<T> & q) { return not (p == q); }template <typename T> point<T> operator + (const point<T> & a, const point<T> & b) { return (point<T>) { a.x+b.x, a.y+b.y }; }template <typename T> point<T> operator - (const point<T> & a, const point<T> & b) { return (point<T>) { a.x-b.x, a.y-b.y }; }template <typename T> point<T> operator - (const point<T> & a) { return (point<T>) { -a.x, -a.y }; }template <typename T> point<T> operator * (T a, const point<T> & b) { return (point<T>) { a*b.x, a*b.y }; }template <typename T> T dot(const point<T> & p, const point<T> & q) { return p.x * q.x + p.y * q.y; }template <typename T> T cross(const point<T> & p, const point<T> & q) { return p.x * q.y - p.y * q.x; }template <typename T> double atan(const point<T> & p) { return std::atan2(p.y, p.x); }template <typename T> std::istream & operator >> (std::istream & input, point<T> & p) { return input >> p.x >> p.y; }template <typename T> std::ostream & operator << (std::ostream & output, const point<T> & p) { return output << p.x << ' ' << p.y; }template <typename T> struct point_y_x_adapter { point<T> & data; };template <typename T> point_y_x_adapter<T> point_y_x(point<T> & data) { return (point_y_x_adapter<T>){ data }; }template <typename T> std::istream & operator >> (std::istream & input, point_y_x_adapter<T> p) { return input >> p.data.y >> p.data.x; }template <typename T> std::ostream & operator << (std::ostream & output, const point_y_x_adapter<T> p) { return output << p.data.y << ' ' << p.data.x; }template <typename T> std::pair<T,T> to_pair(const point<T> & a) { return std::make_pair(a.x, a.y); }template <typename T> bool operator < (const point<T> & a, const point<T> & b) { return to_pair(a) < to_pair(b); }template <typename T> bool operator <= (const point<T> & a, const point<T> & b) { return to_pair(a) <= to_pair(b); }template <typename T> bool operator >= (const point<T> & a, const point<T> & b) { return to_pair(a) >= to_pair(b); }template <typename T> bool operator > (const point<T> & a, const point<T> & b) { return to_pair(a) > to_pair(b); }template <typename T> bool on_board(const point<T> & p, T width, T height) { return 0 <= p.x and p.x < width and 0 <= p.y and p.y < height; }template <typename T> T manhattan(const point<T> & p) { return std::abs(p.x) + std::abs(p.y); }const point<int> directions[4] = { {0,-1}, {0,1}, {-1,0}, {1,0} };
constexpr bool is_unit() const noexcept {
return length_squared() == 1;
}
struct point { T x, y; };ttt point<T> operator + (cpa a, cpa b) { return { a.x+b.x, a.y+b.y }; }ttt point<T> operator - (cpa a, cpa b) { return { a.x-b.x, a.y-b.y }; }ttt point<T> operator - (cpa a) { return { -a.x, -a.y }; }ttt point<T> operator * (T a, cpa b) { return { a*b.x, a*b.y }; }ttt std::pair<T,T> to_pair(cpa a) { return { a.x, a.y }; }ttt bool operator == (cpa a, cpa b) { return to_pair(a) == to_pair(b); }ttt bool operator != (cpa a, cpa b) { return to_pair(a) != to_pair(b); }ttt bool operator < (cpa a, cpa b) { return to_pair(a) < to_pair(b); }ttt bool operator <= (cpa a, cpa b) { return to_pair(a) <= to_pair(b); }ttt bool operator >= (cpa a, cpa b) { return to_pair(a) >= to_pair(b); }ttt bool operator > (cpa a, cpa b) { return to_pair(a) > to_pair(b); }ttt T length_squared(cpa p) { return p.x*p.x + p.y*p.y; }ttt double length(cpa p) { return sqrt(length_squared(p)); }ttt T dot(cpa p, cpa q) { return p.x * q.x + p.y * q.y; }ttt T cross(cpa p, cpa q) { return p.x * q.y - p.y * q.x; }ttt int ccw(cpa a, cpa b, cpa c) { double x = cross(b - a, c - a); return x > 0 ? 1 : x < 0 ? -1 : 0; }
bool Vector2::is_normalized() const {
// use length_squared() instead of length() to avoid sqrt(), makes it more stringent.
return Math::is_equal_approx(length_squared(), 1.0, UNIT_EPSILON);
}
double Field::CalculateWlaplaceian(Vector3f r, float h) {
return -FACTOR / pow(h, 9) * (h * h - length_squared(r)) * (3 * h * h - 7 * length_squared(r));
}
double length() const noexcept {
return std::sqrt(length_squared());
}
void SteepestDescent::optimize_vertex_positions(PatchData &pd,
MsqError &err)
{
MSQ_FUNCTION_TIMER( "SteepestDescent::optimize_vertex_positions" );
const int SEARCH_MAX = 100;
const double c1 = 1e-4;
//std::vector<Vector3D> unprojected(pd.num_free_vertices());
std::vector<Vector3D> gradient(pd.num_free_vertices());
bool feasible=true;//bool for OF values
double min_edge_len, max_edge_len;
double step_size=0, original_value=0, new_value=0;
double norm_squared=0;
PatchDataVerticesMemento* pd_previous_coords;
TerminationCriterion* term_crit=get_inner_termination_criterion();
OFEvaluator& obj_func = get_objective_function_evaluator();
// get vertex memento so we can restore vertex coordinates for bad steps.
pd_previous_coords = pd.create_vertices_memento( err ); MSQ_ERRRTN(err);
// use auto_ptr to automatically delete memento when we exit this function
std::auto_ptr<PatchDataVerticesMemento> memento_deleter( pd_previous_coords );
// Evaluate objective function.
//
// Always use 'update' version when beginning optimization so that
// if doing block coordinate descent the OF code knows the set of
// vertices we are modifying during the optimziation (the subset
// of the mesh contained in the current patch.) This has to be
// done up-front because typically an OF will just store the portion
// of the OF value (e.g. the numeric contribution to the sum for an
// averaging OF) for the initial patch.
feasible = obj_func.update( pd, original_value, gradient, err ); MSQ_ERRRTN(err);
// calculate gradient dotted with itself
norm_squared = length_squared( gradient );
//set an error if initial patch is invalid.
if(!feasible){
MSQ_SETERR(err)("SteepestDescent passed invalid initial patch.",
MsqError::INVALID_ARG);
return;
}
// use edge length as an initial guess for for step size
pd.get_minmax_edge_length( min_edge_len, max_edge_len );
//step_size = max_edge_len / std::sqrt(norm_squared);
//if (!finite(step_size)) // zero-length gradient
// return;
// if (norm_squared < DBL_EPSILON)
// return;
if (norm_squared >= DBL_EPSILON)
step_size = max_edge_len / std::sqrt(norm_squared) * pd.num_free_vertices();
// The steepest descent loop...
// We loop until the user-specified termination criteria are met.
while (!term_crit->terminate()) {
MSQ_DBGOUT(3) << "Iteration " << term_crit->get_iteration_count() << std::endl;
MSQ_DBGOUT(3) << " o original_value: " << original_value << std::endl;
MSQ_DBGOUT(3) << " o grad norm suqared: " << norm_squared << std::endl;
// Save current vertex coords so that they can be restored if
// the step was bad.
pd.recreate_vertices_memento( pd_previous_coords, err ); MSQ_ERRRTN(err);
// Reduce step size until it satisfies Armijo condition
int counter = 0;
for (;;) {
if (++counter > SEARCH_MAX || step_size < DBL_EPSILON) {
MSQ_DBGOUT(3) << " o No valid step found. Giving Up." << std::endl;
return;
}
// Move vertices to new positions.
// Note: step direction is -gradient so we pass +gradient and
// -step_size to achieve the same thing.
pd.move_free_vertices_constrained( arrptr(gradient), gradient.size(), -step_size, err ); MSQ_ERRRTN(err);
// Evaluate objective function for new vertices. We call the
// 'evaluate' form here because we aren't sure yet if we want to
// keep these vertices. Until we call 'update', we have the option
// of reverting a block coordinate decent objective function's state
// to that of the initial vertex coordinates. However, for block
// coordinate decent to work correctly, we will need to call an
// 'update' form if we decide to keep the new vertex coordinates.
feasible = obj_func.evaluate( pd, new_value, err );
if (err.error_code() == err.BARRIER_VIOLATED)
err.clear(); // barrier violated does not represent an actual error here
MSQ_ERRRTN(err);
MSQ_DBGOUT(3) << " o step_size: " << step_size << std::endl;
MSQ_DBGOUT(3) << " o new_value: " << new_value << std::endl;
if (!feasible) {
// OF value is invalid, decrease step_size a lot
step_size *= 0.2;
}
else if (new_value > original_value - c1 * step_size * norm_squared) {
// Armijo condition not met.
step_size *= 0.5;
}
else {
// Armijo condition met, stop
break;
}
// undo previous step : restore vertex coordinates
pd.set_to_vertices_memento( pd_previous_coords, err ); MSQ_ERRRTN(err);
}
// Re-evaluate objective function to get gradient.
// Calling the 'update' form here incorporates the new vertex
// positions into the 'accumulated' value if we are doing a
// block coordinate descent optimization.
obj_func.update(pd, original_value, gradient, err ); MSQ_ERRRTN(err);
if (projectGradient) {
//if (cosineStep) {
// unprojected = gradient;
// pd.project_gradient( gradient, err ); MSQ_ERRRTN(err);
// double dot = inner_product( arrptr(gradient), arrptr(unprojected), gradient.size() );
// double lensqr1 = length_squared( gradient );
// double lensqr2 = length_squared( unprojected );
// double cossqr = dot * dot / lensqr1 / lensqr2;
// step_size *= sqrt(cossqr);
//}
//else {
pd.project_gradient( gradient, err ); MSQ_ERRRTN(err);
//}
}
// Update terination criterion for next iteration.
// This is necessary for efficiency. Some values can be adjusted
// for each iteration so we don't need to re-caculate the value
// over the entire mesh.
term_crit->accumulate_patch( pd, err ); MSQ_ERRRTN(err);
term_crit->accumulate_inner( pd, original_value, arrptr(gradient), err ); MSQ_ERRRTN(err);
// Calculate initial step size for next iteration using step size
// from this iteration
step_size *= norm_squared;
norm_squared = length_squared( gradient );
// if (norm_squared < DBL_EPSILON)
// break;
if (norm_squared >= DBL_EPSILON)
step_size /= norm_squared;
}
}
real_t Quat::length() const {
return Math::sqrt(length_squared());
}
void testAzElRotation()
{
mat3d M;
vec3d x, y, z;
const double threshold = 0.000001;
M = rotationFromAzElTwist(0.0, 0.0, 0.0);
x = M*unitX;
y = M*unitY;
z = M*unitZ;
assert(length_squared(unitX - x) < threshold);
assert(length_squared(unitY - y) < threshold);
assert(length_squared(unitZ - z) < threshold);
M = rotationFromAzElTwist(0.0, M_PI/2.0, 0.0);
x = M*unitX;
y = M*unitY;
z = M*unitZ;
assert(length_squared(unitX - x) < threshold);
assert(length_squared(unitZ - y) < threshold);
assert(length_squared(-unitY - z) < threshold);
M = rotationFromAzElTwist(M_PI, M_PI/2.0, 0.0);
x = M*unitX;
y = M*unitY;
z = M*unitZ;
assert(length_squared(-unitX - x) < threshold);
assert(length_squared(-unitZ - y) < threshold);
assert(length_squared(-unitY - z) < threshold);
M = rotationFromAzElTwist(M_PI, M_PI, 0.0);
x = M*unitX;
y = M*unitY;
z = M*unitZ;
assert(length_squared(-unitX - x) < threshold);
assert(length_squared(-unitY - y) < threshold);
assert(length_squared(unitZ - z) < threshold);
}
