void pendelum::euler_richardson()
{
int i;
double t_h,t_m;
double yout[2],y_h[2],y_m[2];
t_h=0;
y_h[0]=y[0]; //phi
y_h[1]=y[1]; //v
ofstream fout("er.out");
fout.setf(ios::scientific);
fout.precision(20);
for(i=1; i<=n; i++){
derivatives(t_h,y_h,yout);
y_m[1]=y_h[1]+0.5*yout[1]*delta_t_roof;
y_m[0]=y_h[0]+0.5*y_h[1]*delta_t_roof;
t_m=t_h+0.5*delta_t_roof;
derivatives(t_m,y_m,yout);
yout[1]=y_h[1]+yout[1]*delta_t_roof;
yout[0]=y_h[0]+y_m[1]*delta_t_roof;
fout<<i*delta_t<<"\t\t"<<yout[0]<<"\t\t"<<yout[1]<<"\n";
t_h+=delta_t_roof;
y_h[0]=yout[0];
y_h[1]=yout[1];
}
fout.close;
}
void BCS_wf::saveUpdate(Sample_point * sample, int e1, int e2,
Wavefunction_storage * wfstore)
{
BCS_wf_storage * store;
recast(wfstore, store);
jast.saveUpdate(sample,e1,e2,store->jast_store);
store->inverse=inverse;
store->detVal=detVal;
//
// unpaired electrons go into extra one-body orbitals, this
// functionality will be resurrected later
//
//int nmo=moVal.GetDim(1);
//int nd=moVal.GetDim(2);
//for(int m=0; m< nmo; m++) {
// for(int d=0; d< nd; d++) {
// store->moVal(m,d)=moVal(e,m,d);
// }
//}
// We assume the two electrons have opposite spins
assert( spin(e1) != spin(e2) );
int e_up, e_down;
e_up = spin(e1) ? e2 : e1;
e_down = spin(e1) ? e1 : e2;
{
int nj=derivatives.GetDim(1);
assert(nj==nelectrons(0));
int nd=derivatives.GetDim(2);
for(int j=0; j< nj; j++) {
for(int d=0; d< nd; d++) {
store->derivatives(j,d)=derivatives(e_up,j,d);
}
}
int ep=e_up+nelectrons(0);
for(int j=0; j< nj; j++) {
for(int d=0; d< nd; d++) {
store->derivatives(j+nelectrons(0),d)=derivatives(ep,j,d);
}
}
}
{
int ni=derivatives.GetDim(0);
assert(ni==2*nelectrons(0));
int nd=derivatives.GetDim(2);
int rede=e_down-nelectrons(0);
for(int i=0; i< ni; i++) {
for(int d=0; d< nd; d++) {
store->derivatives_2(i,d)=derivatives(i,rede,d);
}
}
}
}
void BCS_wf::restoreUpdate(Sample_point * sample, int e,
Wavefunction_storage * wfstore)
{
BCS_wf_storage * store;
recast(wfstore, store);
jast.restoreUpdate(sample,e,store->jast_store);
detVal=store->detVal;
inverse=store->inverse;
//
// unpaired electrons go into extra one-body orbitals, this
// functionality will be resurrected later
//
//int nmo=moVal.GetDim(1);
//int nd=moVal.GetDim(2);
//for(int m=0; m< nmo; m++) {
// for(int d=0; d< nd; d++) {
// moVal(e,m,d)=store->moVal(m,d);
// }
//}
if(spin(e)==0) {
int nj=derivatives.GetDim(1);
assert(nj==nelectrons(0));
int nd=derivatives.GetDim(2);
for(int j=0; j< nj; j++) {
for(int d=0; d< nd; d++) {
derivatives(e,j,d)=store->derivatives(j,d);
}
}
int ep=e+nelectrons(0);
for(int j=0; j< nj; j++) {
for(int d=0; d< nd; d++) {
derivatives(ep,j,d)=store->derivatives(j+nelectrons(0),d);
}
}
}
else {
int ni=derivatives.GetDim(0);
assert(ni==2*nelectrons(0));
int nd=derivatives.GetDim(2);
int rede=e-nelectrons(0);
for(int i=0; i< ni; i++) {
for(int d=0; d< nd; d++) {
derivatives(i,rede,d)=store->derivatives(i,d);
}
}
}
electronIsStaleLap=0;
electronIsStaleVal=0;
updateEverythingLap=0;
updateEverythingVal=0;
//calcLap(sample);
}
//Note that the matrix is almost symmetric--
//we can use that to halve the computational time
void BCS_wf::calcLap(Sample_point * sample)
{
int tote=nelectrons(0)+nelectrons(1);
Array3 <doublevar> temp_der;
Array2 <doublevar> temp_lap;
for(int e=0; e< tote; e++) {
sample->updateEIDist();
//
//parent->molecorb->updateLap(sample,e,0,updatedMoVal);
//for(int i=0; i< updatedMoVal.GetDim(0); i++) {
// for(int d=0; d< 5; d++) {
// moVal(e,i,d)=updatedMoVal(i,d);
// }
//}
}
Array2 <doublevar> onebody;
jast.updateLap(&parent->jastdata,sample);
jast.get_twobody(twobody);
jast.get_onebody_save(onebody);
int maxmatsize=max(nelectrons(0),nelectrons(1));
Array2 <doublevar> modet(maxmatsize, maxmatsize);
modet=0;
for(int det=0; det < ndet; det++ ) {
for(int i=0; i< nelectrons(0); i++) {
for(int j=0; j< nelectrons(1); j++) {
int jshift=nelectrons(0)+j;
Array2 <doublevar> ugrad;
jastcof->valGradLap(twobody,onebody,i,jshift,ugrad);
modet(i,j)=parent->magnification_factor*ugrad(0,0);
for(int d=0; d< 4; d++) {
derivatives(i,j,d)=ugrad(0,d+1);
derivatives(i+nelectrons(0),j,d)=ugrad(1,d+1);
}
}
//
// unpaired electrons go into extra one-body orbitals, this
// functionality will be resurrected later
//
//for(int j=nelectrons(1); j< nelectrons(0); j++) {
// modet(i,j)=moVal(i,parent->occupation(det,0)(j),0);
//}
}
detVal(det)=
TransposeInverseMatrix(modet,inverse(det), nelectrons(0)).val();
}
}
void MappingFunction3D::ideal( Sample location,
MsqMatrix<3,3>& J,
MsqError& err ) const
{
const Vector3D* coords = unit_element( element_topology(), true );
if (!coords) {
MSQ_SETERR(err)(MsqError::UNSUPPORTED_ELEMENT);
return;
}
const unsigned MAX_VERTS = 8;
MsqVector<3> d_coeff_d_xi[MAX_VERTS];
size_t indices[MAX_VERTS], num_vtx = 0;
derivatives( location, NodeSet(), indices,
d_coeff_d_xi, num_vtx, err ); MSQ_ERRRTN(err);
assert(num_vtx > 0 && num_vtx <= MAX_VERTS);
J.zero();
for (size_t r = 0; r < num_vtx; ++r) {
MsqMatrix<3,1> c( coords[indices[r]].to_array() );
J += c * transpose(d_coeff_d_xi[r]);
}
double size = Mesquite::cbrt(fabs(det(J)));
assert(size > -1e-15); // no negative jacobians for ideal elements!
divide( 1.0, size, size );
J *= size;
}
bool JacobianChecker::check_second_order_elements(
const Vector &new_parameterization,
std::vector<std::string> *error_messages) {
int dim = new_parameterization.size();
std::vector<Matrix> numeric_hessians;
for (int t = 0; t < dim; ++t) {
SubFunction ft(inverse_transformation_, t);
NumericalDerivatives derivatives(ft);
numeric_hessians.push_back(derivatives.Hessian(new_parameterization));
}
for (int r = 0; r < dim; ++r) {
for (int s = 0; s < dim; ++s) {
for (int t = 0; t < dim; ++t) {
double analytic_second_derivative =
analytic_jacobian_->second_order_element(r, s, t);
if (fabs(analytic_second_derivative - numeric_hessians[t](r, s)) >
epsilon_) {
if (error_messages) {
std::ostringstream err;
err << "Element (" << r << "," << s << "," << t << ")"
<< " had a numeric second derivative of "
<< numeric_hessians[t](r, s)
<< " but an analytic second derivative of "
<< analytic_second_derivative << "." << std::endl;
error_messages->push_back(err.str());
}
return false;
}
}
}
}
return true;
}
void MappingFunction3D::jacobian( const PatchData& pd,
size_t element_number,
NodeSet nodeset,
Sample location,
size_t* vertex_patch_indices_out,
MsqVector<3>* d_coeff_d_xi_out,
size_t& num_vtx_out,
MsqMatrix<3,3>& jacobian_out,
MsqError& err ) const
{
const MsqMeshEntity& elem = pd.element_by_index( element_number );
const size_t* conn = elem.get_vertex_index_array();
derivatives( location, nodeset, vertex_patch_indices_out,
d_coeff_d_xi_out, num_vtx_out, err ); MSQ_ERRRTN(err);
convert_connectivity_indices( elem.node_count(), vertex_patch_indices_out,
num_vtx_out, err ); MSQ_ERRRTN(err);
jacobian_out.zero();
size_t w = 0;
for (size_t r = 0; r < num_vtx_out; ++r) {
size_t i = conn[vertex_patch_indices_out[r]];
MsqMatrix<3,1> coords( pd.vertex_by_index( i ).to_array() );
jacobian_out += coords * transpose(d_coeff_d_xi_out[r]);
if (i < pd.num_free_vertices()) {
vertex_patch_indices_out[w] = i;
d_coeff_d_xi_out[w] = d_coeff_d_xi_out[r];
++w;
}
}
num_vtx_out = w;
}
void BCS_wf::getLap(Wavefunction_data * wfdata,
int e, Wf_return & lap)
{
getVal(wfdata,e,lap);
for(int d=1; d< 5; d++) {
if(spin(e)==0) {
for(int det=0; det < ndet; det++) {
doublevar temp=0;
for(int j=0; j< nelectrons(1); j++) {
temp+=derivatives(e,j,d-1)
*inverse(det)(e,j);
}
//
// unpaired electrons go into extra one-body orbitals, this
// functionality will be resurrected later
//
//for(int j=nelectrons(1); j < nelectrons(0); j++) {
// //error("need to do spin-polarized derivatives");
// temp+=moVal(e,parent->occupation(det,0)(j),d)
// *inverse(det)(e,j);
//}
if(ndet > 1) error("update BCS::getLap() for ndet > 1");
lap.amp(0,d)=parent->magnification_factor*temp;
}
}
else {
for(int det=0; det < ndet; det++) {
doublevar temp=0;
int red=parent->rede(e);
for(int i=0; i< nelectrons(0); i++) {
temp+=derivatives(i+nelectrons(0),red,d-1)
*inverse(det)(i,red);
}
lap.amp(0,d)=parent->magnification_factor*temp;
}
}
}
for(int d=1; d< 5; d++) {
lap.phase(0,d)=0.0;
}
}
int plant(plant_inputs_t *inputs, plant_outputs_t *outputs)
{
/* extra field needed by numerical recipes routines: */
static float plant_state[PLANT_STATE_SIZE+1] =
{
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0
};
float derivative_plant_state[PLANT_STATE_SIZE+1];
static int iteration = 0;
float time = iteration * TIME_STEP_SECONDS;
/* the plant state includes the four controller outputs,
because these are used in the calculation of derivatives: */
plant_state[19] = inputs->y19;
plant_state[20] = inputs->y20;
plant_state[21] = inputs->y21;
plant_state[22] = inputs->y22;
derivatives(time,
plant_state,
derivative_plant_state);
rk4(plant_state,
derivative_plant_state,
PLANT_STATE_SIZE,
time,
TIME_STEP_SECONDS,
plant_state,
derivatives);
outputs->y1 = plant_state[1];
outputs->y2 = plant_state[2];
outputs->y3 = plant_state[3];
outputs->y4 = plant_state[4];
outputs->y5 = plant_state[5];
outputs->y6 = plant_state[6];
outputs->y7 = plant_state[7];
outputs->y8 = plant_state[8];
outputs->y9 = plant_state[9];
outputs->y10 = plant_state[10];
outputs->y11 = plant_state[11];
printf("%.3f %.3f %.3f %.3f %.3f %.3f\n",
plant_state[6],
plant_state[7],
plant_state[8],
plant_state[9],
plant_state[10],
plant_state[11]);
iteration++;
return OK;
}
void TestLinearBasisFunction1d()
{
ChastePoint<1> zero(0);
ChastePoint<1> one(1);
std::vector<ChastePoint<1>*> evaluation_points;
evaluation_points.push_back(&zero);
evaluation_points.push_back(&one);
BasisFunctionsCheckers<1> checker;
checker.checkLinearBasisFunctions(evaluation_points);
// Derivatives
c_matrix<double, 1, 2> derivatives;
LinearBasisFunction<1>::ComputeBasisFunctionDerivatives(one,derivatives);
TS_ASSERT_DELTA(derivatives(0,0), -1, 1e-12);
TS_ASSERT_DELTA(derivatives(0,1), 1, 1e-12);
}
Matrix NumericJacobian::matrix(const Vector &z) {
int dim = z.size();
Matrix ans(dim, dim);
for (int i = 0; i < dim; ++i) {
SubFunction f(inverse_transformation_, i);
NumericalDerivatives derivatives(f);
ans.col(i) = derivatives.gradient(z);
}
return ans;
}
bool JacobianChecker::check_logdet_gradient(
const Vector &new_parameterization) {
analytic_jacobian_->evaluate_new_parameterization(new_parameterization);
Vector analytic_gradient = new_parameterization * 0;
analytic_jacobian_->add_logdet_gradient(analytic_gradient);
LogDet analytic_logdet(analytic_jacobian_);
NumericalDerivatives derivatives(analytic_logdet);
Vector numeric_gradient = derivatives.gradient(new_parameterization);
return (numeric_gradient - analytic_gradient).max_abs() < epsilon_;
}
bool JacobianChecker::check_logdet_Hessian(
const Vector &new_parameterization) {
analytic_jacobian_->evaluate_new_parameterization(new_parameterization);
int dim = new_parameterization.size();
Matrix analytic_hessian(dim, dim, 0.0);
analytic_jacobian_->add_logdet_Hessian(analytic_hessian);
LogDet analytic_logdet(analytic_jacobian_);
NumericalDerivatives derivatives(analytic_logdet);
Matrix numeric_hessian = derivatives.Hessian(new_parameterization);
return (numeric_hessian - analytic_hessian).max_abs() < epsilon_;
}
template <typename PointInT, typename IntensityT> void
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::downsample (const FloatImageConstPtr& input,
FloatImageConstPtr& output,
FloatImageConstPtr& output_grad_x,
FloatImageConstPtr& output_grad_y) const
{
downsample (input, output);
FloatImagePtr grad_x (new FloatImage (input->width, input->height));
FloatImagePtr grad_y (new FloatImage (input->width, input->height));
derivatives (*output, *grad_x, *grad_y);
output_grad_x = grad_x;
output_grad_y = grad_y;
}
void rk4TimeStep(StateMatrix& s, const ControlMatrix& c, const
SimulationParameters& params, const Map & m)
{
// CHANGED(AL): Now implementing RK4
StateMatrix k1, k2, k3, k4;
uint n_states = s.cols();
uint n = s.rows();
k1 = StateMatrix(n, n_states);
k2 = StateMatrix(n, n_states);
k3 = StateMatrix(n, n_states);
k4 = StateMatrix(n, n_states);
float dt = params.timeStep;
derivatives(s, k1, c, params, m);
derivatives(s + 0.5*k1*dt, k2, c, params, m);
derivatives(s + 0.5*k2*dt, k3, c, params, m);
derivatives(s + k3*dt, k4, c, params, m);
s += (k1 + 2.*k2 + 2.*k3 + k4) * dt/6.;
}
void pendelum::rk4_step(double t,double *yin,double *yout,double delta_t)
{
/*
The function calculates one step of fourth-order-runge-kutta-method
We will need it for the normal fourth-order-Runge-Kutta-method and
for RK-method with adaptive stepsize control
The function calculates the value of y(t + delta_t) using fourth-order-RK-method
Input: time t and the stepsize delta_t, yin (values of phi and v at time t)
Output: yout (values of phi and v at time t+delta_t)
*/
double k1[2],k2[2],k3[2],k4[2],y_k[2];
// Calculation of k1
derivatives(t,yin,yout);
k1[1]=yout[1]*delta_t;
k1[0]=yout[0]*delta_t;
y_k[0]=yin[0]+k1[0]*0.5;
y_k[1]=yin[1]+k1[1]*0.5;
/*Calculation of k2 */
derivatives(t+delta_t*0.5,y_k,yout);
k2[1]=yout[1]*delta_t;
k2[0]=yout[0]*delta_t;
y_k[0]=yin[0]+k2[0]*0.5;
y_k[1]=yin[1]+k2[1]*0.5;
/* Calculation of k3 */
derivatives(t+delta_t*0.5,y_k,yout);
k3[1]=yout[1]*delta_t;
k3[0]=yout[0]*delta_t;
y_k[0]=yin[0]+k3[0];
y_k[1]=yin[1]+k3[1];
/*Calculation of k4 */
derivatives(t+delta_t,y_k,yout);
k4[1]=yout[1]*delta_t;
k4[0]=yout[0]*delta_t;
/*Calculation of new values of phi and v */
yout[0]=yin[0]+1.0/6.0*(k1[0]+2*k2[0]+2*k3[0]+k4[0]);
yout[1]=yin[1]+1.0/6.0*(k1[1]+2*k2[1]+2*k3[1]+k4[1]);
}
void pendelum::rk2()
{
/*We are using the second-order-Runge-Kutta-algorithm
We have to calculate the parameters k1 and k2 for v and phi,
so we use to arrays k1[2] and k2[2] for this
k1[0], k2[0] are the parameters for phi,
k1[1], k2[1] are the parameters for v
*/
int i;
double t_h;
double yout[2],y_h[2],k1[2],k2[2],y_k[2];
t_h=0;
y_h[0]=y[0]; //phi
y_h[1]=y[1]; //v
ofstream fout("rk2.out");
fout.setf(ios::scientific);
fout.precision(20);
for(i=1; i<=n; i++){
/*Calculation of k1 */
derivatives(t_h,y_h,yout);
k1[1]=yout[1]*delta_t_roof;
k1[0]=yout[0]*delta_t_roof;
y_k[0]=y_h[0]+k1[0]*0.5;
y_k[1]=y_h[1]+k2[1]*0.5;
/*Calculation of k2 */
derivatives(t_h+delta_t_roof*0.5,y_k,yout);
k2[1]=yout[1]*delta_t_roof;
k2[0]=yout[0]*delta_t_roof;
yout[1]=y_h[1]+k2[1];
yout[0]=y_h[0]+k2[0];
fout<<i*delta_t<<"\t\t"<<yout[0]<<"\t\t"<<yout[1]<<"\n";
t_h+=delta_t_roof;
y_h[0]=yout[0];
y_h[1]=yout[1];
}
fout.close;
}
void pendelum::half_step()
{
/*We are using the half_step_algorith.
The algorithm is not self-starting, so we calculate
v_1/2 by using the Euler algorithm. */
int i;
double t_h;
double yout[2],y_h[2];
t_h=0;
y_h[0]=y[0]; //phi
y_h[1]=y[1]; //v
ofstream fout("half_step.out");
fout.setf(ios::scientific);
fout.precision(20);
/*At first we have to calculate v_1/2
For this we use Euler's method:
v_`1/2 = v_0 + 1/2*a_0*delta_t_roof
For calculating a_0 we have to start derivatives
*/
derivatives(t_h,y_h,yout);
yout[1]=y_h[1]+0.5*yout[1]*delta_t_roof;
yout[0]=y_h[0]+yout[1]*delta_t_roof;
fout<<delta_t<<"\t\t"<<yout[0]<<"\t\t"<<yout[1]<<"\n";
y_h[0]=yout[0];
y_h[1]=yout[1];
for(i=2; i<=n; i++){
derivatives(t_h,y_h,yout);
yout[1]=y_h[1]+yout[1]*delta_t_roof;
yout[0]=y_h[0]+yout[1]*delta_t_roof;
fout<<i*delta_t<<"\t\t"<<yout[0]<<"\t\t"<<yout[1]<<"\n";
t_h+=delta_t_roof;
y_h[0]=yout[0];
y_h[1]=yout[1];
}
fout.close;
}
void TestLinearBasisFunction3d()
{
ChastePoint<3> zero(0,0,0);
ChastePoint<3> zerozeroone(0,0,1);
ChastePoint<3> zeroonezero(0,1,0);
ChastePoint<3> onezerozero(1,0,0);
std::vector<ChastePoint<3>*> evaluation_points;
evaluation_points.push_back(&zero);
evaluation_points.push_back(&onezerozero);
evaluation_points.push_back(&zeroonezero);
evaluation_points.push_back(&zerozeroone);
BasisFunctionsCheckers<3> checker;
checker.checkLinearBasisFunctions(evaluation_points);
// Derivatives
c_matrix<double, 3, 4> derivatives;
LinearBasisFunction<3>::ComputeBasisFunctionDerivatives(onezerozero,derivatives);
TS_ASSERT_DELTA(derivatives(0,0), -1, 1e-12);
TS_ASSERT_DELTA(derivatives(0,1), 1, 1e-12);
TS_ASSERT_DELTA(derivatives(0,2), 0, 1e-12);
TS_ASSERT_DELTA(derivatives(0,3), 0, 1e-12);
}
void rk4Integrator(
const double alpha,
const double beta,
const double gamma,
const double gravParameter,
const double Wmagnitude,
Vector &Xcurrent,
const double t,
const double stepSize,
Vector &Xnext )
{
// Evaluate gravitational acceleration values at the current state values
Vector currentGravAcceleration( 3 );
computeEllipsoidGravitationalAcceleration(
alpha,
beta,
gamma,
gravParameter,
Xcurrent[ xPositionIndex ],
Xcurrent[ yPositionIndex ],
Xcurrent[ zPositionIndex ],
currentGravAcceleration );
// Create an object of the struct containing the equations of motion (in this case the eom
// for an orbiter around a uniformly rotating tri-axial ellipsoid) and initialize it to the
// values of the ellipsoidal asteroid's current gravitational accelerations
orbiterEquationsOfMotion derivatives( currentGravAcceleration, Wmagnitude );
// run the step integrator for using rk4 routine
Vector integratedState = Xcurrent;
rk4< Vector, orbiterEquationsOfMotion >( Xcurrent,
t,
stepSize,
integratedState,
derivatives );
// give back the final result
Xnext = integratedState;
}
Vector<double> BoundingLayer::calculate_derivative(const Vector<double>& inputs) const
{
const size_t bounding_neurons_number = get_bounding_neurons_number();
const Vector<double> outputs = calculate_outputs(inputs);
Vector<double> derivatives(bounding_neurons_number);
for(size_t i = 0; i < bounding_neurons_number; i++)
{
if(outputs[i] <= lower_bounds[i] || outputs[i] >= upper_bounds[i])
{
derivatives[i] = 0.0;
}
else
{
derivatives[i] = 1.0;
}
}
return(derivatives);
}
void multiple_roots ( int n, dcmplx p[n], double eps, int maxit,
dcmplx r[n-1], double tol, int m[n-1] )
{
int i,nit;
dcmplx *dp[n-1];
roots(n,p,eps,maxit,r);
multiplicities(n-1,tol,r,m);
derivatives(n,n-1,p,dp);
/* write_derivatives(n,n-1,dp); */
for(i=0; i<n-1; i++)
{
if (m[i] == 1)
nit = Newton(n,p,dp[0],&r[i],eps,8);
else
nit = Newton(n-m[i]+1,dp[m[i]-2],dp[m[i]-1],&r[i],eps,8);
/* printf("refined root %d : ", i);
writeln_dcmplx(r[i]); */
}
}
void pendelum::midpoint()
{
int i;
double t_h;
double yout[2],y_h[2];
t_h=0;
y_h[0]=y[0]; //phi
y_h[1]=y[1]; //v
ofstream fout("midpoint.out");
fout.setf(ios::scientific);
fout.precision(20);
for(i=1; i<=n; i++){
derivatives(t_h,y_h,yout);
yout[1]=y_h[1]+yout[1]*delta_t_roof;
yout[0]=y_h[0]+0.5*(yout[1]+y_h[1])*delta_t_roof;
fout<<i*delta_t<<"\t\t"<<yout[0]<<"\t\t"<<yout[1]<<"\n";
t_h+=delta_t_roof;
y_h[0]=yout[0];
y_h[1]=yout[1];
}
fout.close;
}
void pendelum::euler()
{ //using simple euler-method
int i;
double yout[2],y_h[2];
double t_h;
y_h[0]=y[0];
y_h[1]=y[1];
t_h=0;
ofstream fout("euler.out");
fout.setf(ios::scientific);
fout.precision(20);
for(i=1;i<=n;i++){
derivatives(t_h,y_h,yout);
yout[1]=y_h[1]+yout[1]*delta_t_roof;
yout[0]=y_h[0]+yout[0]*delta_t_roof;
// Calculation with dimensionless values
fout<<i*delta_t<<"\t\t"<<yout[0]<<"\t\t"<<yout[1]<<"\n";
t_h+=delta_t_roof;
y_h[1]=yout[1];
y_h[0]=yout[0];
}
fout.close;
}
void pendelum::euler_cromer()
{
int i;
double t_h;
double yout[2],y_h[2];
t_h=0;
y_h[0]=y[0]; //phi
y_h[1]=y[1]; //v
ofstream fout("ec.out");
fout.setf(ios::scientific);
fout.precision(20);
for(i=1; i<=n; i++){
derivatives(t_h,y_h,yout);
yout[1]=y_h[1]+yout[1]*delta_t_roof;
yout[0]=y_h[0]+yout[1]*delta_t_roof;
// The new calculated value of v is used for calculating phi
fout<<i*delta_t<<"\t\t"<<yout[0]<<"\t\t"<<yout[1]<<"\n";
t_h+=delta_t_roof;
y_h[0]=yout[0];
y_h[1]=yout[1];
}
fout.close;
}
void backward_pass()
{
int k,i,j,u,v;
double sum,
e[max_units],
ec[max_blocks][max_size],
eo[max_blocks],
es[max_blocks][max_size];
/* output units */
for (k=cell_mod,j=0;k<ges_mod;k++,j++)
{
e[k]=error[j]*(1.0-Yk_mod_new[k])*Yk_mod_new[k];
/* weight update contribution */
for (i=in_mod;i<hi_in_mod;i++)
{
DW[k][i] += alpha*e[k]*Yk_mod_new[i];
};
i=hi_in_mod;
for (u=0;u<num_blocks;u++)
{
i++;
for (v=0;v<block_size[u];v++)
{
i++;
DW[k][i] += alpha*e[k]*Yk_mod_new[i];
};
i++;
}
};
/* hidden units */
for (i=in_mod;i<hi_in_mod;i++)
{
sum=0;
for (k=cell_mod;k<ges_mod;k++)
sum+= W_mod[k][i]*e[k];
e[i]=sum*(1.0-Yk_mod_new[i])*Yk_mod_new[i];
/* weight update contribution */
for (j=0;j<cell_mod;j++)
{
DW[i][j] += alpha*e[i]*Yk_mod_old[j];
};
}
/* error to memory cells ec[][] and internal states es[][] */
i=hi_in_mod;
for (u=0;u<num_blocks;u++)
{
i++;
for (v=0;v<block_size[u];v++)
{
i++;
sum = 0;
for (k=cell_mod;k<ges_mod;k++)
sum+= W_mod[k][i]*e[k];
ec[u][v]=sum;
es[u][v]=Y_out[u]*(0.5*(1.0+H[u][v])*(1.0-H[u][v]))*sum;
};
i++;
};
/* output gates */
for (u=0;u<num_blocks;u++)
{
sum=0;
for (v=0;v<block_size[u];v++)
{
sum+= H[u][v]*ec[u][v];
};
eo[u]=sum*(1.0-Y_out[u])*Y_out[u];
};
/* Derivatives of the internal state */
derivatives();
/* updates for weights to input and output gates and memory cells */
i=hi_in_mod;
for (u=0;u<num_blocks;u++)
{
for (j=0;j<cell_mod;j++)
{
sum = 0;
for (v=0;v<block_size[u];v++)
{
sum += es[u][v]*SI[u][v][j];
}
DW[i][j] += alpha*sum;
}
i++;
for (j=0;j<cell_mod;j++)
{
DW[i][j]+= alpha*eo[u]*Yk_mod_old[j];
}
for (v=0;v<block_size[u];v++)
{
i++;
for (j=0;j<cell_mod;j++)
DW[i][j]+= alpha*es[u][v]*SC[u][v][j];
};
i++;
}
}
void main()
{
int i, j, k,
trialnr;
/* input pars */
getpars();
/* input training set and test set */
getsets();
if (maxtrials>20)
maxtrials=20;
if (bias1==1)
in_mod++;
if (bias2==1)
hid_mod++;
hi_in_mod = in_mod+hid_mod;
cell_mod=hi_in_mod;
for (i=0;i<num_blocks;i++)
cell_mod+=(2+block_size[i]);
ges_mod = cell_mod+out_mod;
if (ges_mod>max_units)
{
printf("Program terminated!\n");
printf("You have to set the constant max_units at begin\n");
printf("of the program file greater or equal %d and then\n",ges_mod);
printf("compile the program again.\n");
exit(0);
}
srand(ran_sta);
for (trialnr=0;trialnr<maxtrials;trialnr++)
{
outfile = outf[trialnr];
weightfile = weig[trialnr];
fp1 = fopen(outfile, "w");
fprintf(fp1,"Trial Nr.:%.1d\n",trialnr);
fclose(fp1);
fp2 = fopen(weightfile, "w");
fprintf(fp2,"Trial Nr.:%.1d\n",trialnr);
fclose(fp2);
initia();
examples=0;
epoch=0;
maxepoch=maxepoch_init;
stop_learn=0;
learn = 1;
while (learn == 1)
{
/* executing the environment
and setting the input
*/
execute_act();
/* forward pass */
forward_pass();
if (targ==1) /* only if target for this input */
{
/* compute error */
for (k=cell_mod,j=0;k<ges_mod;k++,j++)
{
error[j]= target_a[j] - Yk_mod_new[k];
};
/* Training error */
comp_err();
}
/* backward pass */
if (targ==1) /* only if target for this input */
{
backward_pass();
}
else
{
derivatives();
}
/* set old activations */
for (i=0;i<ges_mod;i++)
{
Yk_mod_old[i] = Yk_mod_new[i];
}
/* update weights */
if (weight_up==1)
{
weight_up=0;
weight_update();
}
/* stop if maxepoch reached */
if (epoch>maxepoch)
learn=0;
}
weight_out();
test();
}
exit(0);
}
int main()
{
printf("Welcome to Chiu Industries. Pendulum Calculator. NO Driving Force Mode. \n");
FILE *output_file;
//declarations of variables
int number_of_steps;
double initial_x, initial_v, final_t, E_initial;
double h;
double y[2], dydt[2], yout[2];
double t;
double gamma;
double period_initial = 10.0;
int printInterval = 1;
int printCounter = 0;
double count;
initial_v = -0.1;
//output_file = fopen("finalq1a_rk4_vinitial_0pt1_gamma_neg0pt1_v1.dat", "w");
output_file = fopen("dragonfly.dat", "w");
//for(gamma=-10; gamma<11; gamma++)
gamma = -0.1;
for(count = -10; count<10; count++)
{
//fprintf(output_file, "#%12.10E \n", gamma);
final_t = period_initial*10.0;
h = 0.05;
number_of_steps = final_t/h;
initial_x = 0.0;
//initial_v = 0.1;
assert(number_of_steps >0);
//initialise position and velocity
y[0] = initial_x;
y[1] = initial_v;
E_initial = 0.5*y[0]*y[0] + 0.5*y[1]*y[1];
t=0.0;
//output(output_file, h, t, y, E_initial, initial_v);
while(t<=final_t)
{
derivatives(t,y,dydt, gamma);
//euler(y, dydt, 2, t, h, yout);
runge_kutta_4(y, dydt, 2, t, h, yout, derivatives);
y[0]=yout[0];
y[1]=yout[1];
if(printCounter == printInterval)
{
output(output_file, h, t, y, E_initial, initial_v);
printCounter = 0;
}
else{}
printCounter++;
t+=h;
}
initial_v = initial_v+0.01;
}
fclose(output_file);
return 0;
} //end main program
void pendelum::asc()
{
/*
We are using the Runge-Kutta-algorithm with adaptive stepsize control
according to "Numerical Recipes in C", S. 574 ff.
At first we calculate y(x+h) using rk4-method => y1
Then we calculate y(x+h) using two times rk4-method at x+h/2 and x+h => y2
The difference between these values is called "delta" If it is smaller than a given value,
we calculate y(x+h) by y2 + (delta)/15 (page 575, Numerical R.)
If delta is not smaller than ... we calculate a new stepsize using
h_new=(Safety)*h_old*(.../delta)^(0.25) where "Safety" is constant (page 577 N.R.)
and start again with calculating y(x+h)...
*/
int i;
double t_h,h_alt,h_neu,hh,errmax;
double yout[2],y_h[2],y_m[2],y1[2],y2[2], delta[2], yscal[2];
const double eps=1.0e-6;
const double safety=0.9;
const double errcon=6.0e-4;
const double tiny=1.0e-30;
t_h=0;
y_h[0]=y[0]; //phi
y_h[1]=y[1]; //v
h_neu=delta_t_roof;
ofstream fout("asc.out");
fout.setf(ios::scientific);
fout.precision(20);
for(i=0;i<=n;i++){
/* The error is scaled against yscal
We use a yscal of the form yscal = fabs(y[i]) + fabs(h*derivatives[i])
(N.R. page 567)
*/
derivatives(t_h,y_h,yout);
yscal[0]=fabs(y[0])+fabs(h_neu*yout[0])+tiny;
yscal[1]=fabs(y[1])+fabs(h_neu*yout[1])+tiny;
/* the do-while-loop is used until the */
do{
/* Calculating y2 by two half steps */
h_alt=h_neu;
hh=h_alt*0.5;
rk4_step(t_h, y_h, y_m, hh);
rk4_step(t_h+hh,y_m,y2,hh);
/* Calculating y1 by one normal step */
rk4_step(t_h,y_h,y1,h_alt);
/* Now we have two values for phi and v at the time t_h + h in y2 and y1
We can now calculate the delta for phi and v
*/
delta[0]=fabs(y1[0]-y2[0]);
delta[1]=fabs(y1[1]-y2[1]);
errmax=(delta[0]/yscal[0] > delta[1]/yscal[1] ? delta[0]/yscal[0] : delta[1]/yscal[1]);
/*We scale delta against the constant yscal
Then we take the biggest one and call it errmax */
errmax=(double)errmax/eps;
/*We divide errmax by eps and have only */
h_neu=safety*h_alt*exp(-0.25*log(errmax));
}while(errmax>1.0);
/*Now we are outside the do-while-loop and have a delta which is small enough
So we can calculate the new values of phi and v
*/
yout[0]=y_h[0]+delta[0]/15.0;
yout[1]=y_h[1]+delta[1]/15.0;
fout<<(double)(t_h+h_alt)/omega_0<<"\t\t"<<yout[0]<<"\t\t"<<yout[1]<<"\n";
// Calculating of the new stepsize
h_neu=(errmax > errcon ? safety*h_alt*exp(-0.20*log(errmax)) : 4.0*h_alt);
y_h[0]=yout[0];
y_h[1]=yout[1];
t_h+=h_neu;
}
}
template <typename PointInT, typename IntensityT> void
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::computePyramids (const PointCloudInConstPtr& input,
std::vector<FloatImageConstPtr>& pyramid,
pcl::InterpolationType border_type) const
{
int step = 3;
pyramid.resize (step * nb_levels_);
FloatImageConstPtr previous;
FloatImagePtr tmp (new FloatImage (input->width, input->height));
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
for (int i = 0; i < static_cast<int> (input->size ()); ++i)
tmp->points[i] = intensity_ (input->points[i]);
previous = tmp;
FloatImagePtr img (new FloatImage (previous->width + 2*track_width_,
previous->height + 2*track_height_));
pcl::copyPointCloud (*tmp, *img, track_height_, track_height_, track_width_, track_width_,
border_type, 0.f);
pyramid[0] = img;
// compute first level gradients
FloatImagePtr g_x (new FloatImage (input->width, input->height));
FloatImagePtr g_y (new FloatImage (input->width, input->height));
derivatives (*img, *g_x, *g_y);
// copy to bigger clouds
FloatImagePtr grad_x (new FloatImage (previous->width + 2*track_width_,
previous->height + 2*track_height_));
pcl::copyPointCloud (*g_x, *grad_x, track_height_, track_height_, track_width_, track_width_,
pcl::BORDER_CONSTANT, 0.f);
pyramid[1] = grad_x;
FloatImagePtr grad_y (new FloatImage (previous->width + 2*track_width_,
previous->height + 2*track_height_));
pcl::copyPointCloud (*g_y, *grad_y, track_height_, track_height_, track_width_, track_width_,
pcl::BORDER_CONSTANT, 0.f);
pyramid[2] = grad_y;
for (int level = 1; level < nb_levels_; ++level)
{
// compute current level and current level gradients
FloatImageConstPtr current;
FloatImageConstPtr g_x;
FloatImageConstPtr g_y;
downsample (previous, current, g_x, g_y);
// copy to bigger clouds
FloatImagePtr image (new FloatImage (current->width + 2*track_width_,
current->height + 2*track_height_));
pcl::copyPointCloud (*current, *image, track_height_, track_height_, track_width_, track_width_,
border_type, 0.f);
pyramid[level*step] = image;
FloatImagePtr gradx (new FloatImage (g_x->width + 2*track_width_, g_x->height + 2*track_height_));
pcl::copyPointCloud (*g_x, *gradx, track_height_, track_height_, track_width_, track_width_,
pcl::BORDER_CONSTANT, 0.f);
pyramid[level*step + 1] = gradx;
FloatImagePtr grady (new FloatImage (g_y->width + 2*track_width_, g_y->height + 2*track_height_));
pcl::copyPointCloud (*g_y, *grady, track_height_, track_height_, track_width_, track_width_,
pcl::BORDER_CONSTANT, 0.f);
pyramid[level*step + 2] = grady;
// set the new level
previous = current;
}
}