void SimMathcalBongo::myintegrate() {
double h = 1.0 * mTimestep;
double h_2 = 0.5 * h;
Eigen::VectorXd x = mState;
Eigen::VectorXd u = mTorque;
// LOG(INFO) << "math.x = " << x.transpose();
// LOG(INFO) << "math.u = " << u.transpose();
Eigen::VectorXd k1 = deriv(x, u);
Eigen::VectorXd k2 = deriv(x + h_2 * k1, u);
Eigen::VectorXd k3 = deriv(x + h_2 * k2, u);
Eigen::VectorXd k4 = deriv(x + h * k3, u);
Eigen::VectorXd dx = (k1 + 2 * k2 + 2 * k3 + k4) / 6.0;
// LOG(INFO) << "dx = " << dx.transpose();
mState = x + h * dx;
for (int i = 0; i < mState.size(); i++) {
double x = mState(i);
if (x < -2.0 * PI) x = -2.0 * PI;
if (x > 2.0 * PI) x = 2.0 * PI;
mState(i) = x;
}
// mHistory.push_back(mState);
// // Hard coded constraint..
mState(3) = mState(2);
mState(4) = 0.5 * PI - mState(2);
mState(5) = -0.5 * PI - mState(2);
}
inline void him::runge_kutta_4(t_float dt)
{
t_float k1[NUMB_EQ],k2[NUMB_EQ],k3[NUMB_EQ],k4[NUMB_EQ];
t_float temp1[NUMB_EQ], temp2[NUMB_EQ], temp3[NUMB_EQ];
for(int i=0; i<=NUMB_EQ-1; i++) // iterate over equations
{
k1[i] = dt * deriv(data,i);
temp1[i] = data[i] + 0.5*k1[i];
}
for(int i=0; i<=NUMB_EQ-1; i++)
{
k2[i] = dt * deriv(temp1,i);
temp2[i] = data[i] + 0.5*k2[i];
}
for(int i=0; i<=NUMB_EQ-1; i++)
{
k3[i] = dt * deriv(temp2,i);
temp3[i] = data[i] + k3[i];
}
for(int i=0; i<=NUMB_EQ-1; i++)
{
k4[i] = dt * deriv(temp3,i);
data[i] = data[i] + (k1[i] + (2.*(k2[i]+k3[i])) + k4[i])/6.;
// we don't want to experience denormals in the next step */
if(fabs((data[i]))<1e-5)
data[i]=0;
}
/*
the system might become unstable ... in this case, we'll request a new system
*/
for(int i=0; i<=NUMB_EQ-1; i++)
{
if(data[i]>2)
{
xfade = newsystem = true;
data[i] = 2;
}
if(data[i]<-2)
{
xfade = newsystem = true;
data[i] = -2;
}
}
}
inline
Eigen::Matrix<fvar<T>, R1, C2>
mdivide_right_tri_low(const Eigen::Matrix<double, R1, C1> &A,
const Eigen::Matrix<fvar<T>, R2, C2> &b) {
check_square("mdivide_right_tri_low", "b", b);
check_multiplicable("mdivide_right_tri_low", "A", A, "b", b);
Eigen::Matrix<T, R1, C2>
A_mult_inv_b(A.rows(), b.cols());
Eigen::Matrix<T, R2, C2> deriv_b_mult_inv_b(b.rows(), b.cols());
Eigen::Matrix<T, R2, C2> val_b(b.rows(), b.cols());
Eigen::Matrix<T, R2, C2> deriv_b(b.rows(), b.cols());
val_b.setZero();
deriv_b.setZero();
for (int j = 0; j < b.cols(); j++) {
for (int i = j; i < b.rows(); i++) {
val_b(i, j) = b(i, j).val_;
deriv_b(i, j) = b(i, j).d_;
}
}
A_mult_inv_b = mdivide_right(A, val_b);
deriv_b_mult_inv_b = mdivide_right(deriv_b, val_b);
Eigen::Matrix<T, R1, C2>
deriv(A.rows(), b.cols());
deriv = -multiply(A_mult_inv_b, deriv_b_mult_inv_b);
return to_fvar(A_mult_inv_b, deriv);
}
Result zero_newton(Params* p, TFloat (*fn)(Params *, TFloat), TFloat (*deriv) (Params *, TFloat)){
TFloat current = p->x, previous = 0.0;
Result res;
int i;
for(i = p->max_iterations; i > 0 && !stopping_criteria(previous, current, p->tol_newton); i--){
previous = current;
current = previous - (fn(p, previous)/deriv(p, previous));
}
res.speed = deriv(p,current);
res.zero = current;
res.iterations = p->max_iterations - i;
return res;
}
int main(void){
long input[6] = {1,2,3,4,5,6};
long size_in = 6;
long out[6] = {0,0,0,0,0};
long stopien = 1;
int i;
printf("Tablica in:\n");
for( i=0;i<6;i++)
printf("%d = %ld\n", i ,input[i]);
printf("\nStopien %d\n", stopien);
deriv(input, size_in, out, stopien);
printf("\nTablica out\n");
for(i = 0; i<6;i++)
printf("el %d = %ld\n", i, out[i]);
printf("\nsuma trzech doubli\n");
printf(" 6.5 + 4.5 + 5.5 = %f\n", sum3(6.5,5.5,4.5));
return 0;
}
static void deriv(const InputType& y, OutputType& x)
{
x = y;
for (size_t i = 0; i < y.n_elem; i++)
x(i) = deriv(y(i));
}
void Newton_Rhapson(double (*func)(double), double (*deriv)(double), double *X, double x0, unsigned N){
double x = x0;
X[0] = x0;
for(unsigned u = 1; u < N; u++){
x -= func(x)/deriv(x);
X[u] = x;
}
}
void RTransform::deriv_array(double* t, double* d, int n) {
while (n > 0) {
*d = deriv(*t);
n--;
t++;
d++;
}
}
void integ(Trick::Integrator *I, BALL *ball ) {
do {
deriv( ball);
I->state_in( &ball->pos[0], &ball->pos[1], &ball->vel[0], &ball->vel[1], NULL);
I->deriv_in( &ball->vel[0], &ball->vel[1], &ball->acc[0], &ball->acc[1], NULL);
I->integrate();
I->state_out( &ball->pos[0], &ball->pos[1], &ball->vel[0], &ball->vel[1], NULL);
} while ( I->intermediate_step);
}
PoseVelocityState RK4Integrator::calcStates(const PoseVelocityState &states, const base::Vector6d &control_input)
{
checkInputs(states, control_input);
PoseVelocityState system_states = states;
// Runge-Kuta coefficients
PoseVelocityState k1 = deriv(system_states, control_input);
PoseVelocityState k2 = deriv(system_states + ((integration_step/2)*k1), control_input);
PoseVelocityState k3 = deriv(system_states + ((integration_step/2)*k2), control_input);
PoseVelocityState k4 = deriv(system_states + (integration_step*k3), control_input);
// Calculating the system states
system_states += (integration_step/6)*(k1 + 2*k2 + 2*k3 + k4);
//Brute force normalization of quaternions due the RK4 integration.
system_states.orientation.normalize();
return system_states;
}
// calculator
void CoordinationBase::calculate()
{
double ncoord=0.;
Tensor virial;
vector<Vector> deriv(getNumberOfAtoms());
// deriv.resize(getPositions().size());
if(nl->getStride()>0 && invalidateList){
nl->update(getPositions());
}
unsigned stride=comm.Get_size();
unsigned rank=comm.Get_rank();
if(serial){
stride=1;
rank=0;
}else{
stride=comm.Get_size();
rank=comm.Get_rank();
}
for(unsigned int i=rank;i<nl->size();i+=stride) { // sum over close pairs
Vector distance;
unsigned i0=nl->getClosePair(i).first;
unsigned i1=nl->getClosePair(i).second;
if(getAbsoluteIndex(i0)==getAbsoluteIndex(i1)) continue;
if(pbc){
distance=pbcDistance(getPosition(i0),getPosition(i1));
} else {
distance=delta(getPosition(i0),getPosition(i1));
}
double dfunc=0.;
ncoord += pairing(distance.modulo2(), dfunc,i0,i1);
deriv[i0] = deriv[i0] + (-dfunc)*distance ;
deriv[i1] = deriv[i1] + dfunc*distance ;
virial=virial+(-dfunc)*Tensor(distance,distance);
}
if(!serial){
comm.Sum(ncoord);
if(!deriv.empty()) comm.Sum(&deriv[0][0],3*deriv.size());
comm.Sum(virial);
}
for(unsigned i=0;i<deriv.size();++i) setAtomsDerivatives(i,deriv[i]);
setValue (ncoord);
setBoxDerivatives (virial);
}
/*
* Make one R-K step
*/
void
RungeStep(void *CTX, /* context for extra parameters */
/* derivative function takes independent and array of
dependent variables, last argument is returned derivates */
void (*deriv)(void *, double, double *, double*),
int nDep, /* number of dependent variables */
double t, /* independent variable */
double *xin, /* array of input */
double *xout, /* array of output */
double dDelta) /* step size */
{
double k1[MAXDEP];
double k2[MAXDEP];
double k3[MAXDEP];
double k4[MAXDEP];
double xtemp[MAXDEP];
const double onesixth = 1.0/6.0;
const double onethird = 1.0/3.0;
int i;
assert(nDep <= MAXDEP);
deriv(CTX, t, xin, k1);
for(i = 0; i < nDep; i++) {
k1[i] *= dDelta;
xtemp[i] = xin[i] + 0.5*k1[i];
}
deriv(CTX, t + 0.5*dDelta, xtemp, k2);
for(i = 0; i < nDep; i++) {
k2[i] *= dDelta;
xtemp[i] = xin[i] + 0.5*k2[i];
}
deriv(CTX, t + 0.5*dDelta, xtemp, k3);
for(i = 0; i < nDep; i++) {
k3[i] *= dDelta;
xtemp[i] = xin[i] + k3[i];
}
deriv(CTX, t + dDelta, xtemp, k4);
for(i = 0; i < nDep; i++) {
k4[i] *= dDelta;
xout[i] = xin[i] + onesixth*(k1[i] + k4[i]) + onethird*(k2[i] + k3[i]);
}
}
NOX::Abstract::Group::ReturnType
LOCA::Epetra::ModelEvaluatorInterface::
computeDfDp(LOCA::MultiContinuation::AbstractGroup& grp,
const vector<int>& param_ids,
NOX::Abstract::MultiVector& result,
bool isValidF) const
{
// Break result into f and df/dp
NOX::Epetra::Vector& f = dynamic_cast<NOX::Epetra::Vector&>(result[0]);
Epetra_Vector& epetra_f = f.getEpetraVector();
std::vector<int> dfdp_index(result.numVectors()-1);
for (unsigned int i=0; i<dfdp_index.size(); i++)
dfdp_index[i] = i+1;
Teuchos::RefCountPtr<NOX::Epetra::MultiVector> dfdp =
Teuchos::rcp_dynamic_cast<NOX::Epetra::MultiVector>(result.subView(dfdp_index));
Epetra_MultiVector& epetra_dfdp = dfdp->getEpetraMultiVector();
// Create inargs
EpetraExt::ModelEvaluator::InArgs inargs = model_->createInArgs();
const NOX::Epetra::Vector& x =
dynamic_cast<const NOX::Epetra::Vector&>(grp.getX());
const Epetra_Vector& epetra_x = x.getEpetraVector();
inargs.set_x(Teuchos::rcp(&epetra_x, false));
inargs.set_p(0, Teuchos::rcp(¶m_vec, false));
if (inargs.supports(EpetraExt::ModelEvaluator::IN_ARG_x_dot)) {
// Create x_dot, filled with zeros
if (x_dot == NULL)
x_dot = new Epetra_Vector(epetra_x.Map());
inargs.set_x_dot(Teuchos::rcp(x_dot, false));
}
// Create outargs
EpetraExt::ModelEvaluator::OutArgs outargs = model_->createOutArgs();
if (!isValidF) {
EpetraExt::ModelEvaluator::Evaluation<Epetra_Vector> eval_f;
Teuchos::RefCountPtr<Epetra_Vector> F = Teuchos::rcp(&epetra_f, false);
eval_f.reset(F, EpetraExt::ModelEvaluator::EVAL_TYPE_EXACT);
outargs.set_f(eval_f);
}
Teuchos::RefCountPtr<Epetra_MultiVector> DfDp =
Teuchos::rcp(&epetra_dfdp, false);
Teuchos::Array<int> param_indexes(param_ids.size());
for (unsigned int i=0; i<param_ids.size(); i++)
param_indexes[i] = param_ids[i];
EpetraExt::ModelEvaluator::DerivativeMultiVector dmv(DfDp, EpetraExt::ModelEvaluator::DERIV_MV_BY_COL,
param_indexes);
EpetraExt::ModelEvaluator::Derivative deriv(dmv);
outargs.set_DfDp(0, deriv);
model_->evalModel(inargs, outargs);
return NOX::Abstract::Group::Ok;
}
bool Target2DShapeOrientBarrier::evaluate_with_hess( const MsqMatrix<2,2>& A,
const MsqMatrix<2,2>& W,
double& result,
MsqMatrix<2,2>& deriv,
MsqMatrix<2,2> second[3],
MsqError& err )
{
const MsqMatrix<2,2> Winv = inverse(W);
const MsqMatrix<2,2> T = A * Winv;
const double norm = Frobenius(T);
const double invroot = 1.0/MSQ_SQRT_TWO;
const double tau = det(T);
if (invalid_determinant(tau)) { // barrier
result = 0.0;
return false;
}
const double inv_tau = 1.0/tau;
const double invnorm = 1.0/norm;
const double f = norm - invroot * trace(T);
result = 0.5 * inv_tau * f;
const MsqMatrix<2,2> adjt = transpose_adj(T);
deriv = invnorm * T;
deriv(0,0) -= invroot;
deriv(1,1) -= invroot;
deriv *= 0.5;
deriv -= result * adjt;
deriv *= inv_tau;
deriv = deriv * transpose(Winv);
const double a = 0.5 * inv_tau * invnorm;
set_scaled_outer_product( second, -a*invnorm*invnorm, T );
pluseq_scaled_I( second, a );
pluseq_scaled_outer_product( second, f*inv_tau*inv_tau*inv_tau, adjt );
pluseq_scaled_2nd_deriv_of_det( second, -0.5*f*inv_tau*inv_tau );
pluseq_scaled_sum_outer_product( second, -0.5*inv_tau*inv_tau*invnorm, T, adjt );
pluseq_scaled_sum_outer_product_I( second, 0.5*inv_tau*inv_tau*invroot, adjt );
second_deriv_wrt_product_factor( second, Winv );
return true;
}
void WLSQFitter::calculate_beta_and_alpha(){
//calculate beta and alpha matrix
//clear lower left corner including diagonal
for(int i = 0; i<alpha.rows(); ++i)
{
for(int j = 0; j<=i; ++j)
{
alpha(i,j)=0;
}
}
//clear beta
for (size_t j=0;j<modelptr->getnroffreeparameters();j++){
beta[j]=0.0;
}
for (size_t i=0;i<(modelptr->getnpoints());i++){
double expdata=(modelptr->getHLptr())->getcounts(i);
double modeldata=modelptr->getcounts(i);
if (!(modelptr->isexcluded(i))){ //don't count points that are excluded
double weight=getweight(i);
for (size_t j=0;j<modelptr->getnroffreeparameters();j++){
beta[j] += weight*(expdata-modeldata)*deriv(j,i);
for (size_t k=0; k<=j; k++){
alpha(j,k) += weight*deriv(j,i)*deriv(k,i);
}
}
}
}
//copy the one triangle to the other side because of symmetry
for (size_t j=1; j<modelptr->getnroffreeparameters(); j++){
//was j=0 but first row needs not to be copied because is already full
for (size_t k=0; k<j; k++){
//was k<=j but you don't need to copy the diagonal terms
alpha(k,j) = alpha(j,k);
}
}
}
// output input->hidden layer weights
NeuralNetwork updateweights_IH(NeuralNetwork netwk)
{
double wtchg = 0.0;
int i, j;
for (i=0; i<netwk.data.hiddenNodes; ++i)
for (j=0; j<netwk.data.inputNodes; ++j)
{
wtchg = netwk.lr_ih * netwk.data.patternerr * deriv(netwk.data.hiddenLayer[i], netwk.activationFunction, netwk.fnpars, 1);
wtchg *= netwk.data.weights_HO[i] * netwk.data.inputLayer[j];
netwk.data.weights_IH[i][j] += wtchg;
}
return netwk;
}
std::vector<CurveIntersection> Curve::intersectSelf(Coord eps) const
{
std::vector<CurveIntersection> result;
// Monotonic segments cannot have self-intersections.
// Thus, we can split the curve at roots and intersect the portions.
std::vector<Coord> splits;
std::auto_ptr<Curve> deriv(derivative());
splits = deriv->roots(0, X);
if (splits.empty()) {
return result;
}
deriv.reset();
splits.push_back(1.);
boost::ptr_vector<Curve> parts;
Coord previous = 0;
for (unsigned i = 0; i < splits.size(); ++i) {
if (splits[i] == 0.) continue;
parts.push_back(portion(previous, splits[i]));
previous = splits[i];
}
Coord prev_i = 0;
for (unsigned i = 0; i < parts.size()-1; ++i) {
Interval dom_i(prev_i, splits[i]);
prev_i = splits[i];
Coord prev_j = 0;
for (unsigned j = i+1; j < parts.size(); ++j) {
Interval dom_j(prev_j, splits[j]);
prev_j = splits[j];
std::vector<CurveIntersection> xs = parts[i].intersect(parts[j], eps);
for (unsigned k = 0; k < xs.size(); ++k) {
// to avoid duplicated intersections, skip values at exactly 1
if (xs[k].first == 1. || xs[k].second == 1.) continue;
Coord ti = dom_i.valueAt(xs[k].first);
Coord tj = dom_j.valueAt(xs[k].second);
CurveIntersection real(ti, tj, xs[k].point());
result.push_back(real);
}
}
}
return result;
}
static real_t second_deriv(real_t y) {
return -deriv(y) * 0.5 * y;
}
//----------------------------------------------------------------------
void Wannier_method::optimize_rotation(Array3 <dcomplex> & eikr,
Array2 <doublevar> & R ) {
int norb=eikr.GetDim(1);
Array2 <doublevar> gvec(3,3);
sys->getPrimRecipLattice(gvec);
Array1 <doublevar> gnorm(3);
gnorm=0;
for(int d=0; d < 3; d++) {
for(int d1=0; d1 < 3; d1++) {
gnorm(d)+=gvec(d,d1)*gvec(d,d1);
}
gnorm(d)=sqrt(gnorm(d));
}
for(int i=0; i< norb; i++) {
cout << "rloc2 " << i << " ";
for(int d=0; d< 3; d++) {
cout << -log(norm(eikr(d,i,i)))/(gnorm(d)*gnorm(d)) << " ";
}
cout << endl;
}
Array2 <doublevar> Rgen(norb,norb),Rgen_save(norb,norb);
//R(norb,norb);
R.Resize(norb,norb);
//Array2 <dcomplex> tmp(norb,norb),tmp2(norb,norb);
//Shake up the angles, since often the original orbitals
//are at a maximum and derivatives are zero.
Array2 <doublevar> deriv(norb,norb);
Rgen=0.0;
for(int ii=0; ii< norb; ii++) {
for(int jj=ii+1; jj< norb; jj++) {
Rgen(ii,jj)=rng.gasdev()*pi*shake;
}
}
for(int step=0; step < max_opt_steps; step++) {
doublevar fbase=evaluate_local(eikr,Rgen,R);
for(int ii=0; ii <norb; ii++) {
cout << "deriv ";
for(int jj=ii+1; jj < norb; jj++) {
doublevar save_rgeniijj=Rgen(ii,jj);
doublevar h=1e-6;
Rgen(ii,jj)+=h;
doublevar func=evaluate_local(eikr,Rgen,R);
deriv(ii,jj)=(func-fbase)/h;
Rgen(ii,jj)=save_rgeniijj;
cout << deriv(ii,jj) << " ";
}
cout << endl;
}
doublevar rloc_thresh=0.0001;
Rgen_save=Rgen;
doublevar best_func=1e99, best_tstep=0.0;
doublevar bracket_tstep=0.0;
doublevar last_func=fbase;
for(doublevar tstep=0.01; tstep < 20.0; tstep*=2.0) {
doublevar func=eval_tstep(eikr,Rgen,Rgen_save,deriv,tstep,R);
cout << "tstep " << tstep << " func " << func << endl;
if(func > fbase or func > last_func) {
bracket_tstep=tstep;
break;
}
else last_func=func;
}
cout << "bracket_tstep " << bracket_tstep << endl;
doublevar resphi=2.-(1.+sqrt(5.))/2.;
doublevar a=0, b=resphi*bracket_tstep, c=bracket_tstep;
doublevar af=fbase, bf=eval_tstep(eikr,Rgen,Rgen_save,deriv,b,R), cf=eval_tstep(eikr,Rgen,Rgen_save,deriv,bracket_tstep,R);
cout << "first step a,b,c " << a << " " << b << " " << c
<< " funcs " << af << " " << bf << " " << cf << endl;
for(int it=0; it < 20; it++) {
doublevar d,df;
if( (c-b) > (b-a))
d=b+resphi*(c-b);
else
d=b-resphi*(b-a);
df=eval_tstep(eikr,Rgen,Rgen_save,deriv,d,R);
if(df < bf) {
if( (c-b) > (b-a) ) {
a=b;
af=bf;
b=d;
bf=df;
}
else {
c=b;
cf=bf;
b=d;
bf=df;
}
}
else {
if( (c-b) > (b-a) ) {
c=d;
cf=df;
}
else {
a=d;
af=df;
}
}
cout << "step " << it << " a,b,c " << a << " " << b << " " << c
<< " funcs " << af << " " << bf << " " << cf << endl;
}
best_tstep=b;
/*
bool made_move=false;
while (!made_move) {
for(doublevar tstep=0.00; tstep < max_tstep; tstep+=0.1*max_tstep) {
for(int ii=0; ii< norb;ii++) {
for(int jj=ii+1; jj < norb; jj++) {
Rgen(ii,jj)=Rgen_save(ii,jj)-tstep*deriv(ii,jj);
}
}
doublevar func=evaluate_local(eikr,Rgen,R);
if(func < best_func) {
best_func=func;
best_tstep=tstep;
}
cout << " tstep " << tstep << " " << func << endl;
}
if(abs(best_tstep) < 0.2*max_tstep)
max_tstep*=0.5;
else if(abs(best_tstep-max_tstep) < 1e-14)
max_tstep*=2.0;
else made_move=true;
}
*/
for(int ii=0; ii< norb; ii++) {
for(int jj=ii+1; jj < norb; jj++) {
Rgen(ii,jj)=Rgen_save(ii,jj)-best_tstep*deriv(ii,jj);
}
}
doublevar func2=evaluate_local(eikr,Rgen,R);
doublevar max_change=0;
for(int ii=0; ii < norb; ii++) {
for(int jj=ii+1; jj< norb; jj++) {
doublevar change=abs(Rgen(ii,jj)-Rgen_save(ii,jj));
if(change > max_change) max_change=change;
}
}
cout << "tstep " << best_tstep << " rms " << sqrt(func2) << " bohr max change " << max_change <<endl;
doublevar threshold=0.0001;
if(max_change < threshold) break;
if(abs(best_func-fbase) < rloc_thresh) break;
/*
bool moved=false;
for(int ii=0; ii< norb; ii++) {
for(int jj=ii+1; jj< norb; jj++) {
doublevar save_rgeniijj=Rgen(ii,jj);
doublevar best_del=0;
doublevar best_f=1e99;
for(doublevar del=-0.5; del < 0.5; del+=0.05) {
cout << "############ for del = " << del << endl;
Rgen(ii,jj)=save_rgeniijj+del;
doublevar func=evaluate_local(eikr,Rgen,R);
if(func < best_f) {
best_f=func;
best_del=del;
}
cout << "func " << func << endl;
}
Rgen(ii,jj)=save_rgeniijj+best_del;
if(abs(best_del) > 1e-12) moved=true;
}
}
if(!moved) break;
*/
}
make_rotation_matrix(Rgen,R);
}
NOX::Abstract::Group::ReturnType
LOCA::Thyra::Group::computeDfDpMulti(const std::vector<int>& paramIDs,
NOX::Abstract::MultiVector& fdfdp,
bool isValidF)
{
// Currently this does not work because the thyra modelevaluator is not
// setting the parameter names correctly in the epetraext modelevalator,
// so we are disabling this for now
implement_dfdp = false;
// Use default implementation if we don't want to use model evaluator, or
// it doesn't support it
if (!implement_dfdp ||
!out_args_.supports(::Thyra::ModelEvaluatorBase::OUT_ARG_DfDp,
param_index).supports(::Thyra::ModelEvaluatorBase::DERIV_MV_BY_COL)) {
NOX::Abstract::Group::ReturnType res =
LOCA::Abstract::Group::computeDfDpMulti(paramIDs, fdfdp, isValidF);
return res;
}
// Split fdfdp into f and df/dp
int num_vecs = fdfdp.numVectors()-1;
std::vector<int> index_dfdp(num_vecs);
for (int i=0; i<num_vecs; i++)
index_dfdp[i] = i+1;
NOX::Thyra::Vector& f = dynamic_cast<NOX::Thyra::Vector&>(fdfdp[0]);
Teuchos::RCP<NOX::Abstract::MultiVector> dfdp =
fdfdp.subView(index_dfdp);
// Right now this isn't very efficient because we have to compute
// derivatives with respect to all of the parameters, not just
// paramIDs. Will have to work out with Ross how to selectively get
// parameter derivatives
int np = params.length();
Teuchos::RCP<NOX::Thyra::MultiVector> dfdp_full =
Teuchos::rcp_dynamic_cast<NOX::Thyra::MultiVector>(dfdp->clone(np));
::Thyra::ModelEvaluatorBase::DerivativeMultiVector<double> dmv(dfdp_full->getThyraMultiVector(), ::Thyra::ModelEvaluatorBase::DERIV_MV_BY_COL);
::Thyra::ModelEvaluatorBase::Derivative<double> deriv(dmv);
in_args_.set_x(x_vec_->getThyraRCPVector().assert_not_null());
if (in_args_.supports(::Thyra::ModelEvaluatorBase::IN_ARG_x_dot))
in_args_.set_x_dot(x_dot_vec);
in_args_.set_p(param_index, param_thyra_vec);
if (!isValidF)
out_args_.set_f(f.getThyraRCPVector().assert_not_null());
out_args_.set_DfDp(param_index, deriv);
// Evaluate model
model_->evalModel(in_args_, out_args_);
// Copy back dfdp
for (int i=0; i<num_vecs; i++)
(*dfdp)[i] = (*dfdp_full)[paramIDs[i]];
// Reset inargs/outargs
in_args_.set_x(Teuchos::null);
in_args_.set_p(param_index, Teuchos::null);
out_args_.set_f(Teuchos::null);
out_args_.set_DfDp(param_index,
::Thyra::ModelEvaluatorBase::Derivative<double>());
if (out_args_.isFailed())
return NOX::Abstract::Group::Failed;
return NOX::Abstract::Group::Ok;
}
void leapfrog(double *x, double *v, double *t, double step){
*x+=step*(*v);
*v+=step*deriv(*x);
*t+=step;
}
void advance_vel(real x, real *v, real dt2)
{
*v += dt2*deriv(x); /* offset the velocity by half a
time step using the Euler method */
}
int main(int argc, char **argv) {
printf("\n======= GCOR v1.1 - Automatic Gain Correction =======\n\tR. Lica, IFIN-HH, Dec2013 \n\n");
int chNum, detNum, runstart, runstop, irun, idet;
int minWIDTH, maxWIDTH, SHIFT, SWEEP, degree;
int low, high;
char name[20];
FILE *settings, *fi, *fo;
initialize(settings, &chNum, &detNum, name, &runstart, &runstop, &minWIDTH, &maxWIDTH, &SHIFT, &SWEEP, °ree, &low, &high);
degree++; // in the program degree represents the number of coefficients of the polynomial and NOT the degree of the polynomial. i know...
int regions=(high-low)/SHIFT;
struct Data2Fit shData[regions];
int i, j, k, nData=0;
double refSpec[chNum], Spec[chNum];
double coeff[degree], chisq, norm;
FILE * gnuplotPipe = popen ("gnuplot", "w");
char outfile[20], infile[20], answer;
sprintf(outfile, "gcor.cal");
fo=fopen(outfile, "wt");
for(idet=0; idet<detNum; idet++)
{
fprintf(fo, "%5d%5d%5d%9.3f%10.6f\n", runstart, idet, 2, 0.0, 1.0); //each detector from the first run is set as reference
for(irun=runstart+1; irun<=runstop; irun++)
{
for (i=0; i<regions; i++)
{
shData[i].ch=0;
shData[i].chShift=0;
shData[i].err=0;
}
sprintf(infile, "%2s.%04d", name, runstart);
if(fopen(infile, "rb")) fi=fopen(infile, "rb");
else {
printf("Cannot open %2s.%04d\n", name, runstart);
exit(0);
}
readBin(fi, refSpec, idet, chNum, low, high);
sprintf(infile, "%2s.%04d", name, irun);
if(fopen(infile, "rb")) fi=fopen(infile, "rb");
else continue;
readBin(fi, Spec, idet, chNum, low, high);
normalize(refSpec, Spec, chNum, &norm);
smooth(refSpec, Spec, minWIDTH/5, maxWIDTH/2, low, high);
deriv(refSpec, Spec, low, high, 5);
autoshift(refSpec, Spec, shData, low, high, minWIDTH, maxWIDTH, SHIFT, SWEEP, regions);
nData = performFit(shData, degree, &chisq, coeff, regions);
if (nData == 0) {
printf("Warning! %2s#%02d.%04d: Could not extract data suitable for fit. Change settings!\n", name, idet, irun);
goto skip;
}
gnuplot(gnuplotPipe, irun, idet, shData, nData, degree, chisq, coeff, regions, norm);
writeCal(fo, degree, coeff, irun, idet);
if (answer=='a') goto skip;
printf("Going to %2s#%02d.%04d ([y]/n)? (Type 'a' for automatic fit)", name, idet, irun+1);
answer = getchar();
skip:
if (answer=='n') exit(0);
}
printf("---------------------\nGoing to Detector #%02d\n---------------------\n", idet+1);
}
exit(0);
}
// calculator
void CoordinationBase::calculate()
{
double ncoord=0.;
Tensor virial;
vector<Vector> deriv(getNumberOfAtoms());
// deriv.resize(getPositions().size());
if(nl->getStride()>0 && invalidateList) {
nl->update(getPositions());
}
unsigned stride=comm.Get_size();
unsigned rank=comm.Get_rank();
if(serial) {
stride=1;
rank=0;
} else {
stride=comm.Get_size();
rank=comm.Get_rank();
}
unsigned nt=OpenMP::getNumThreads();
const unsigned nn=nl->size();
if(nt*stride*10>nn) nt=nn/stride/10;
if(nt==0)nt=1;
#pragma omp parallel num_threads(nt)
{
std::vector<Vector> omp_deriv(getPositions().size());
Tensor omp_virial;
#pragma omp for reduction(+:ncoord) nowait
for(unsigned int i=rank; i<nn; i+=stride) {
Vector distance;
unsigned i0=nl->getClosePair(i).first;
unsigned i1=nl->getClosePair(i).second;
if(getAbsoluteIndex(i0)==getAbsoluteIndex(i1)) continue;
if(pbc) {
distance=pbcDistance(getPosition(i0),getPosition(i1));
} else {
distance=delta(getPosition(i0),getPosition(i1));
}
double dfunc=0.;
ncoord += pairing(distance.modulo2(), dfunc,i0,i1);
Vector dd(dfunc*distance);
Tensor vv(dd,distance);
if(nt>1) {
omp_deriv[i0]-=dd;
omp_deriv[i1]+=dd;
omp_virial-=vv;
} else {
deriv[i0]-=dd;
deriv[i1]+=dd;
virial-=vv;
}
}
#pragma omp critical
if(nt>1) {
for(int i=0; i<getPositions().size(); i++) deriv[i]+=omp_deriv[i];
virial+=omp_virial;
}
}
if(!serial) {
comm.Sum(ncoord);
if(!deriv.empty()) comm.Sum(&deriv[0][0],3*deriv.size());
comm.Sum(virial);
}
for(unsigned i=0; i<deriv.size(); ++i) setAtomsDerivatives(i,deriv[i]);
setValue (ncoord);
setBoxDerivatives (virial);
}
static void deriv(const InputType& y, OutputType& x)
{
x = y;
x.transform( [](double y) { return deriv(y); } );
}
static void integrate(state y0, state *yp, double *xp, double x1, double x2, int *steps)
{
int i;
int stepnum = 0; // Track number of steps the integrator has run
double x = x1; // Current time, begin at x1
state s; // Current state
// Integrator memory, each position in an array is a DOF of the system
double y[n]; // array of integrator outputs, y = integral(y' dx)
double dydx[n]; // array of RHS, dy/dx
double yscale[n]; // array of yscale factors (integraion error tolorence)
double h; // timestep
double hdid; // stores actual timestep taken by RK45
double hnext; // guess for next timestep
// stop the integrator
double time_to_stop = x2;
// First guess for timestep
h = x2 - x1;
// Inital conditions
y[0] = y0.x.v.i;
y[1] = y0.v.v.i;
y[2] = y0.x.v.j;
y[3] = y0.v.v.j;
y[4] = y0.x.v.k;
y[5] = y0.v.v.k;
dydx[1] = y0.a.v.i;
dydx[3] = y0.a.v.j;
dydx[5] = y0.a.v.k;
y[6] = y0.m;
while (stepnum <= MAXSTEPS)
{
// First RHS call
deriv(y, dydx, x);
s = rk2state(y, dydx);
// Store current state
xp[stepnum] = x;
yp[stepnum] = s;
// Y-scaling. Holds down fractional errors
for (i=0;i<n;i++) {
yscale[i] = 0.000000001;//dydx[i]+TINY;
}
// Check for stepsize overshoot
if ((x + h) > time_to_stop)
h = time_to_stop - x;
// One quality controled integrator step
rkqc(y, dydx, &x, h, eps, yscale, &hdid, &hnext, n, deriv);
s = rk2state(y, dydx);
// Are we finished?
// hit ground
if (underground(s))
{
deriv(y, dydx, x);
s = rk2state(y, dydx);
xp[stepnum] = x;
yp[stepnum] = s;
stepnum++;
break;
}
// Passed requested integration time
if ( (time_to_stop - x) <= 0.0001 )
{
deriv(y, dydx, x);
s = rk2state(y, dydx);
xp[stepnum] = x;
yp[stepnum] = s;
stepnum++;
break;
}
// set timestep for next go around
h = hnext;
// Incement step counter
stepnum++;
}
(*steps) = stepnum;
return;
}
inline Color
CurveGradient::color_func(const Point &point_, int quality, float supersample)const
{
Point origin=param_origin.get(Point());
Real width=param_width.get(Real());
std::vector<synfig::BLinePoint> bline(param_bline.get_list().begin(), param_bline.get_list().end());
Gradient gradient=param_gradient.get(Gradient());
bool loop=param_loop.get(bool());
bool zigzag=param_zigzag.get(bool());
bool perpendicular=param_perpendicular.get(bool());
bool fast=param_fast.get(bool());
Vector tangent;
Vector diff;
Point p1;
Real thickness;
Real dist;
float perp_dist;
bool edge_case = false;
if(bline.size()==0)
return Color::alpha();
else if(bline.size()==1)
{
tangent=bline.front().get_tangent1();
p1=bline.front().get_vertex();
thickness=bline.front().get_width();
}
else
{
float t;
Point point(point_-origin);
std::vector<synfig::BLinePoint>::const_iterator iter,next;
// Figure out the BLinePoints we will be using,
// Taking into account looping.
if(perpendicular)
{
next=find_closest(fast,bline,point,t,bline_loop,&perp_dist);
perp_dist/=curve_length_;
}
else // not perpendicular
{
next=find_closest(fast,bline,point,t,bline_loop);
}
iter=next++;
if(next==bline.end()) next=bline.begin();
// Setup the curve
etl::hermite<Vector> curve(
iter->get_vertex(),
next->get_vertex(),
iter->get_tangent2(),
next->get_tangent1()
);
// Setup the derivative function
etl::derivative<etl::hermite<Vector> > deriv(curve);
int search_iterations(7);
/*if(quality==0)search_iterations=8;
else if(quality<=2)search_iterations=10;
else if(quality<=4)search_iterations=8;
*/
if(perpendicular)
{
if(quality>7)
search_iterations=4;
}
else // not perpendicular
{
if(quality<=6)search_iterations=7;
else if(quality<=7)search_iterations=6;
else if(quality<=8)search_iterations=5;
else search_iterations=4;
}
// Figure out the closest point on the curve
if (fast)
t = curve.find_closest(fast, point,search_iterations);
// Calculate our values
p1=curve(t); // the closest point on the curve
tangent=deriv(t); // the tangent at that point
// if the point we're nearest to is at either end of the
// bline, our distance from the curve is the distance from the
// point on the curve. we need to know which side of the
// curve we're on, so find the average of the two tangents at
// this point
if (t<0.00001 || t>0.99999)
{
bool zero_tangent = (tangent[0] == 0 && tangent[1] == 0);
if (t<0.5)
{
if (iter->get_split_tangent_angle() || iter->get_split_tangent_radius() || zero_tangent)
{
// fake the current tangent if we need to
if (zero_tangent) tangent = curve(FAKE_TANGENT_STEP) - curve(0);
// calculate the other tangent
Vector other_tangent(iter->get_tangent1());
if (other_tangent[0] == 0 && other_tangent[1] == 0)
{
// find the previous blinepoint
std::vector<synfig::BLinePoint>::const_iterator prev;
if (iter != bline.begin()) (prev = iter)--;
else if (loop) (prev = bline.end())--;
else prev = iter;
etl::hermite<Vector> other_curve(prev->get_vertex(), iter->get_vertex(), prev->get_tangent2(), iter->get_tangent1());
other_tangent = other_curve(1) - other_curve(1-FAKE_TANGENT_STEP);
}
// normalise and sum the two tangents
tangent=(other_tangent.norm()+tangent.norm());
edge_case=true;
}
}
else
{
if (next->get_split_tangent_angle() || next->get_split_tangent_radius() || zero_tangent)
{
// fake the current tangent if we need to
if (zero_tangent) tangent = curve(1) - curve(1-FAKE_TANGENT_STEP);
// calculate the other tangent
Vector other_tangent(next->get_tangent2());
if (other_tangent[0] == 0 && other_tangent[1] == 0)
{
// find the next blinepoint
std::vector<synfig::BLinePoint>::const_iterator next2(next);
if (++next2 == bline.end())
{
if (loop) next2 = bline.begin();
else next2 = next;
}
etl::hermite<Vector> other_curve(next->get_vertex(), next2->get_vertex(), next->get_tangent2(), next2->get_tangent1());
other_tangent = other_curve(FAKE_TANGENT_STEP) - other_curve(0);
}
// normalise and sum the two tangents
tangent=(other_tangent.norm()+tangent.norm());
edge_case=true;
}
}
}
tangent = tangent.norm();
if(perpendicular)
{
tangent*=curve_length_;
p1-=tangent*perp_dist;
tangent=-tangent.perp();
}
else // not perpendicular
// the width of the bline at the closest point on the curve
thickness=(next->get_width()-iter->get_width())*t+iter->get_width();
}
if(perpendicular)
{
if(quality>7)
{
dist=perp_dist;
/* diff=tangent.perp();
const Real mag(diff.inv_mag());
supersample=supersample*mag;
*/
supersample=0;
}
else
{
diff=tangent.perp();
//p1-=diff*0.5;
const Real mag(diff.inv_mag());
supersample=supersample*mag;
diff*=mag*mag;
dist=(point_-origin - p1)*diff;
}
}
else // not perpendicular
{
if (edge_case)
{
diff=(p1-(point_-origin));
if(diff*tangent.perp()<0) diff=-diff;
diff=diff.norm()*thickness*width;
}
else
diff=tangent.perp()*thickness*width;
p1-=diff*0.5;
const Real mag(diff.inv_mag());
supersample=supersample*mag;
diff*=mag*mag;
dist=(point_-origin - p1)*diff;
}
if(loop)
dist-=floor(dist);
if(zigzag)
{
dist*=2.0;
supersample*=2.0;
if(dist>1)dist=2.0-dist;
}
if(loop)
{
if(dist+supersample*0.5>1.0)
{
float left(supersample*0.5-(dist-1.0));
float right(supersample*0.5+(dist-1.0));
Color pool(gradient(1.0-(left*0.5),left).premult_alpha()*left/supersample);
if (zigzag) pool+=gradient(1.0-right*0.5,right).premult_alpha()*right/supersample;
else pool+=gradient(right*0.5,right).premult_alpha()*right/supersample;
return pool.demult_alpha();
}
if(dist-supersample*0.5<0.0)
{
float left(supersample*0.5-dist);
float right(supersample*0.5+dist);
Color pool(gradient(right*0.5,right).premult_alpha()*right/supersample);
if (zigzag) pool+=gradient(left*0.5,left).premult_alpha()*left/supersample;
else pool+=gradient(1.0-left*0.5,left).premult_alpha()*left/supersample;
return pool.demult_alpha();
}
}
return gradient(dist,supersample);
}
void advance_vel(double x, double *v, double step){
*v += step*deriv(x); /* offset the velocity by half a
time step using the Euler method */
}
void leapfrog(real *x, real *v, real *t, real step)
{
*x+=step*(*v);
*v+=step*deriv(*x);
*t+=step;
}
int main(int argc, char* argv[]) {
if(argc != 2) {
std::cerr << "Usage : " << argv[0] << "<0,1>" <<std::endl;
std::cerr << "with : " << std::endl;
std::cerr << "0 : quadratic loss" << std::endl;
std::cerr << "1 : cross entropy loss" << std::endl;
return -1;
}
bool quadratic_loss = (atoi(argv[1]) == 0);
srand(time(NULL));
// We compare our computation of the gradient to
// a finite difference approximation
// The loss is also involved
std::cout << "---------------------------------" << std::endl;
std::cout << "Comparing the analytical gradient and numerical approximation " << std::endl;
auto input = gaml::mlp::input<X>(INPUT_DIM, fillInput);
auto l1 = gaml::mlp::layer(input, HIDDEN_LAYER_SIZE, gaml::mlp::mlp_sigmoid(), gaml::mlp::mlp_dsigmoid());
auto l2 = gaml::mlp::layer(l1, HIDDEN_LAYER_SIZE, gaml::mlp::mlp_identity(), gaml::mlp::mlp_didentity());
auto l3 = gaml::mlp::layer(l2, HIDDEN_LAYER_SIZE, gaml::mlp::mlp_tanh(), gaml::mlp::mlp_dtanh());
auto l4 = gaml::mlp::layer(l3, OUTPUT_DIM, gaml::mlp::mlp_sigmoid(), gaml::mlp::mlp_dsigmoid());
auto mlp = gaml::mlp::perceptron(l4, output_of);
std::cout << "We use the following architecture : " << std::endl;
std::cout << mlp << std::endl;
std::cout << "which has a total of " << mlp.psize() << " parameters"<< std::endl;
gaml::mlp::parameters_type params(mlp.psize());
gaml::mlp::parameters_type paramsph(mlp.psize());
gaml::mlp::values_type derivatives(mlp.psize());
gaml::mlp::values_type forward_sweep(mlp.size());
X x;
auto loss_ce = gaml::mlp::loss::CrossEntropy();
auto loss_quadratic = gaml::mlp::loss::Quadratic();
auto f = [&mlp, ¶ms] (const typename decltype(mlp)::input_type& x) -> gaml::mlp::values_type {
auto output = mlp(x, params);
gaml::mlp::values_type voutput(mlp.output_size());
fillOutput(voutput.begin(), output);
return voutput;
};
auto df = [&mlp, &forward_sweep, ¶ms] (const typename decltype(mlp)::input_type& x, unsigned int parameter_dim) -> gaml::mlp::values_type {
return mlp.deriv(x, params, forward_sweep, parameter_dim);
};
unsigned int nbtrials = 100;
unsigned int nbfails = 0;
std::cout << "I will compare " << nbtrials << " times a numerical approximation and the analytical gradient we compute" << std::endl;
for(unsigned int t = 0 ; t < nbtrials ; ++t) {
randomize_data(params, -1.0, 1.0);
randomize_data(x, -1.0, 1.0);
// Compute the output at params
auto output = mlp(x, params);
gaml::mlp::values_type raw_output(OUTPUT_DIM);
fillOutput(raw_output.begin(), output);
gaml::mlp::values_type raw_outputph(OUTPUT_DIM);
// For computing the loss, we need a target
gaml::mlp::values_type raw_target(OUTPUT_DIM);
randomize_data(raw_target);
double norm_dh = 0.0;
for(unsigned int i = 0 ; i < mlp.psize() ; ++i) {
// Let us compute params + h*[0 0 0 0 0 0 1 0 0 0 0 0], the 1 at the ith position
std::copy(params.begin(), params.end(), paramsph.begin());
double dh = (sqrt(DBL_EPSILON) * paramsph[i]);
paramsph[i] += dh;
norm_dh += dh*dh;
// Compute the output at params + h
auto outputph = mlp(x, paramsph);
fillOutput(raw_outputph.begin(), outputph);
// We now compute the approximation of the derivative
if(quadratic_loss)
derivatives[i] = (loss_quadratic(raw_target, raw_outputph) - loss_quadratic(raw_target, raw_output))/dh;
else
derivatives[i] = (loss_ce(raw_target, raw_outputph) - loss_ce(raw_target, raw_output))/dh;
}
// We now compute the analytical derivatives
mlp(x, params);
std::copy(mlp.begin(), mlp.end(), forward_sweep.begin());
gaml::mlp::values_type our_derivatives(mlp.psize());
for(unsigned int i = 0 ; i < mlp.psize() ; ++i) {
if(quadratic_loss)
our_derivatives[i] = loss_quadratic.deriv(x, raw_target, forward_sweep, f, df, i);
else
our_derivatives[i] = loss_ce.deriv(x, raw_target, forward_sweep, f, df, i);
}
// We finally compute the norm of the difference
double error = 0.0;
auto diter = derivatives.begin();
for(auto& ourdi : our_derivatives) {
error = (ourdi - *diter) * (ourdi - *diter);
diter++;
}
error = sqrt(error);
std::cout << "Error between the analytical and numerical gradients " << error << " with a step size of " << sqrt(norm_dh) << " in norm" << std::endl;
if(error > 1e-7)
++nbfails;
/*
std::cout << "numerical " << std::endl;
for(auto & di : derivatives)
std::cout << di << " ";
std::cout << std::endl;
std::cout << "our :" << std::endl;
for(auto& di : our_derivatives)
std::cout << di << " ";
std::cout << std::endl;
*/
}
std::cout << nbfails << " / " << nbtrials << " with an error higher than 1e-7" << std::endl;
}
