unsigned int make_gaussian_cloud( cv::Mat_<T>& mat, 
                                  const T& point_color, const int n_points, 
                                  const double mean_x, const double mean_y, 
                                  const double var_x, const double var_y) {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<> dx(mean_x, var_x);
    std::normal_distribution<> dy(mean_y, var_y);
    unsigned int ret(0);
 
    for( unsigned int i=0; i<n_points; ++i) {
        int r = (int)dy(gen);
        int c = (int)dx(gen);
    
        if( r>=0 && r<mat.rows && c>=0 && c<mat.cols) {
            mat( r, c) = point_color;
            ++ret;
        }
    }
    return ret;
}

bool SVGFEOffsetElement::build(SVGResourceFilter* filterResource)
{
    FilterEffect* input1 = filterResource->builder()->getEffectById(in1());

    if (!input1)
        return false;

    RefPtr<FilterEffect> effect = FEOffset::create(input1, dx(), dy());
    filterResource->addFilterEffect(this, effect.release());

    return true;
}

bool Hyperboloid::IntersectP(const Ray &r) const {
    Float phi, v;
    Point3f pHit;
    // Transform _Ray_ to object space
    Vector3f oErr, dErr;
    Ray ray = (*WorldToObject)(r, &oErr, &dErr);

    // Compute quadratic hyperboloid coefficients

    // Initialize _EFloat_ ray coordinate values
    EFloat ox(ray.o.x, oErr.x), oy(ray.o.y, oErr.y), oz(ray.o.z, oErr.z);
    EFloat dx(ray.d.x, dErr.x), dy(ray.d.y, dErr.y), dz(ray.d.z, dErr.z);
    EFloat a = ah * dx * dx + ah * dy * dy - ch * dz * dz;
    EFloat b = 2.f * (ah * dx * ox + ah * dy * oy - ch * dz * oz);
    EFloat c = ah * ox * ox + ah * oy * oy - ch * oz * oz - 1.f;

    // Solve quadratic equation for _t_ values
    EFloat t0, t1;
    if (!Quadratic(a, b, c, &t0, &t1)) return false;

    // Check quadric shape _t0_ and _t1_ for nearest intersection
    if (t0.UpperBound() > ray.tMax || t1.LowerBound() <= 0) return false;
    EFloat tShapeHit = t0;
    if (t0.LowerBound() <= 0) {
        tShapeHit = t1;
        if (tShapeHit.UpperBound() > ray.tMax) return false;
    }

    // Compute hyperboloid inverse mapping
    pHit = ray((Float)tShapeHit);
    v = (pHit.z - p1.z) / (p2.z - p1.z);
    Point3f pr = (1 - v) * p1 + v * p2;
    phi = std::atan2(pr.x * pHit.y - pHit.x * pr.y,
                     pHit.x * pr.x + pHit.y * pr.y);
    if (phi < 0) phi += 2 * Pi;

    // Test hyperboloid intersection against clipping parameters
    if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) {
        if (tShapeHit == t1) return false;
        tShapeHit = t1;
        if (t1.UpperBound() > ray.tMax) return false;
        // Compute hyperboloid inverse mapping
        pHit = ray((Float)tShapeHit);
        v = (pHit.z - p1.z) / (p2.z - p1.z);
        Point3f pr = (1 - v) * p1 + v * p2;
        phi = std::atan2(pr.x * pHit.y - pHit.x * pr.y,
                         pHit.x * pr.x + pHit.y * pr.y);
        if (phi < 0) phi += 2 * Pi;
        if (pHit.z < zMin || pHit.z > zMax || phi > phiMax) return false;
    }
    return true;
}

int IsoparametricTransformation::TransformBack(const Vector &pt,
                                               IntegrationPoint &ip)
{
   const int    max_iter = 16;
   const double  ref_tol = 1e-15;
   const double phys_tol = 1e-15*pt.Normlinf();

   const int dim = FElem->GetDim();
   const int sdim = PointMat.Height();
   const int geom = FElem->GetGeomType();
   IntegrationPoint xip, prev_xip;
   double xd[3], yd[3], dxd[3], Jid[9];
   Vector x(xd, dim), y(yd, sdim), dx(dxd, dim);
   DenseMatrix Jinv(Jid, dim, sdim);
   bool hit_bdr = false, prev_hit_bdr;

   // Use the center of the element as initial guess
   xip = Geometries.GetCenter(geom);
   xip.Get(xd, dim); // xip -> x

   for (int it = 0; it < max_iter; it++)
   {
      // Newton iteration:    x := x + J(x)^{-1} [pt-F(x)]
      // or when dim != sdim: x := x + [J^t.J]^{-1}.J^t [pt-F(x)]
      Transform(xip, y);
      subtract(pt, y, y); // y = pt-y
      if (y.Normlinf() < phys_tol) { ip = xip; return 0; }
      SetIntPoint(&xip);
      CalcInverse(Jacobian(), Jinv);
      Jinv.Mult(y, dx);
      x += dx;
      prev_xip = xip;
      prev_hit_bdr = hit_bdr;
      xip.Set(xd, dim); // x -> xip
      // If xip is ouside project it on the boundary on the line segment
      // between prev_xip and xip
      hit_bdr = !Geometry::ProjectPoint(geom, prev_xip, xip);
      if (dx.Normlinf() < ref_tol) { ip = xip; return 0; }
      if (hit_bdr)
      {
         xip.Get(xd, dim); // xip -> x
         if (prev_hit_bdr)
         {
            prev_xip.Get(dxd, dim); // prev_xip -> dx
            subtract(x, dx, dx);    // dx = xip - prev_xip
            if (dx.Normlinf() < ref_tol) { return 1; }
         }
      }
   }
   ip = xip;
   return 2;
}

void FEOffset::apply(Filter* filter)
{
    m_in->apply(filter);
    if (!m_in->resultImage())
        return;

    GraphicsContext* filterContext = getEffectContext();
    if (!filterContext)
        return;

    if (filter->effectBoundingBoxMode()) {
        setDx(dx() * filter->sourceImageRect().width());
        setDy(dy() * filter->sourceImageRect().height());
    }

    FloatRect dstRect = FloatRect(dx() + m_in->subRegion().x() - subRegion().x(),
                                  dy() + m_in->subRegion().y() - subRegion().y(),
                                  m_in->subRegion().width(),
                                  m_in->subRegion().height());

    filterContext->drawImage(m_in->resultImage()->image(), dstRect);
}

void Simulation::calculateForceGradient(MatrixXd &TVk, SparseMatrix<double>& forceGradient){
	forceGradient.setZero();
	
	vector<Trip> triplets1;
	triplets1.reserve(12*12*M.tets.size());	
	for(unsigned int i=0; i<M.tets.size(); i++){
		//Get P(dxn), dx = [1,0, 0...], then [0,1,0,....], and so on... for all 4 vert's x, y, z
		//P is the compute Force Differentials blackbox fxn

		Vector12d dx(12);
		dx.setZero();
		Vector4i indices = M.tets[i].verticesIndex;
		int kj;
		for(unsigned int j=0; j<12; j++){
			dx(j) = 1;
			MatrixXd dForces = M.tets[i].computeForceDifferentials(TVk, dx);
			kj = j%3;
			//row in order for dfxi/dxi ..dfxi/dzl
			triplets1.push_back(Trip(3*indices[j/3]+kj, 3*indices[0], dForces(0,0)));
			triplets1.push_back(Trip(3*indices[j/3]+kj, 3*indices[0]+1, dForces(1,0)));
			triplets1.push_back(Trip(3*indices[j/3]+kj, 3*indices[0]+2, dForces(2,0)));

			triplets1.push_back(Trip(3*indices[j/3]+kj, 3*indices[1], dForces(0,1)));
			triplets1.push_back(Trip(3*indices[j/3]+kj, 3*indices[1]+1, dForces(1,1)));
			triplets1.push_back(Trip(3*indices[j/3]+kj, 3*indices[1]+2, dForces(2,1)));

			triplets1.push_back(Trip(3*indices[j/3]+kj, 3*indices[2], dForces(0,2)));
			triplets1.push_back(Trip(3*indices[j/3]+kj, 3*indices[2]+1, dForces(1,2)));
			triplets1.push_back(Trip(3*indices[j/3]+kj, 3*indices[2]+2, dForces(2,2)));

			triplets1.push_back(Trip(3*indices[j/3]+kj, 3*indices[3], dForces(0,3)));
			triplets1.push_back(Trip(3*indices[j/3]+kj, 3*indices[3]+1, dForces(1,3)));
			triplets1.push_back(Trip(3*indices[j/3]+kj, 3*indices[3]+2, dForces(2,3)));
			dx(j) = 0;
		}
	}
	forceGradient.setFromTriplets(triplets1.begin(), triplets1.end());
	return;
}

void rendervertwater(uint subdiv, int x, int y, int z, uint size, Texture *t)
{   
    float xf = 8.0f/t->xs;
    float yf = 8.0f/t->ys;
    float xs = subdiv*xf;
    float ys = subdiv*yf;
    float t1 = lastmillis/300.0f;
    float t2 = lastmillis/4000.0f;

    wx1 = x;
    wy1 = y;
    wx2 = wx1 + size,
    wy2 = wy1 + size;
    wsize = size;

    ASSERT((wx1 & (subdiv - 1)) == 0);
    ASSERT((wy1 & (subdiv - 1)) == 0);

    for(int xx = wx1; xx<wx2; xx += subdiv)
    {
        float xo = xf*(xx+t2);
        glBegin(GL_TRIANGLE_STRIP);
        for(int yy = wy1; yy<wy2; yy += subdiv)
        {
            float yo = yf*(yy+t2);
            if(yy==wy1)
            {
                vertw(xx, yy, z, dx(xo), dy(yo), t1);
                vertw(xx+subdiv, yy, z, dx(xo+xs), dy(yo), t1);
            };
            vertw(xx, yy+subdiv, z, dx(xo), dy(yo+ys), t1);
            vertw(xx+subdiv, yy+subdiv, z, dx(xo+xs), dy(yo+ys), t1);
        };
        glEnd();
        int n = (wy2-wy1-1)/subdiv;
        n = (n+2)*2; 
        xtraverts += n;
    };
};

Sacado::Fad::Vector< OrdinalType, Sacado::Fad::DVFad<ValueType> >::
~Vector()
{
  // Here we must destroy the value and derivative arrays
  if (vec_.size() > 0) {
    ValueType *v = vals();
    ds_array<ValueType>::destroy_and_release(v, vec_.size());
    if (deriv_size_ > 0) {
      v = dx();
      ds_array<ValueType>::destroy_and_release(v, vec_.size()*deriv_size_);
    }
  }
}

std::vector <double> evalDx(std::vector <double> x)
{
    std::vector <double> dx(nx);

    dx[0] = x[1] - x[0];
    for(int i = 1; i < nx - 1; i++)
    {
        dx[i] =  (x[i] - x[i - 1])/2  +  (x[i + 1] - x[i])/2 ;
    }
    dx[nx - 1] = x[x.size() - 1] - x[x.size() - 2];

    return dx;
}

// verify we can print intrinsics info
void TestDx::testPrintIntrinsics()
{
    ACE_SOCK_STREAM dummyStream;
    SseProxy proxy(dummyStream);

    Dx dx(&proxy);
    //DxDebug dx(&proxy);

    // test the dx
    dx.printIntrinsics();


}

// find distance x0 is from segment x1-x2
static float point_segment_distance(const Vec3f &x0, const Vec3f &x1, const Vec3f &x2)
{
   Vec3f dx(x2-x1);
   double m2=mag2(dx);
   // find parameter value of closest point on segment
   float s12=(float)(dot(x2-x0, dx)/m2);
   if(s12<0){
      s12=0;
   }else if(s12>1){
      s12=1;
   }
   // and find the distance
   return dist(x0, s12*x1+(1-s12)*x2);
}

void UnionOfBallsView::generate_circle(const Weighted_point &wp, std::list<Segment> &segments, int subdiv)
{
    segments.clear();
    if (wp.weight() <= 0) return;

    double r = sqrt(wp.weight());
    Vector dx(r, 0), dy(0, r);
    segments.push_back(Segment(wp+dx, wp+dy));
    segments.push_back(Segment(wp+dy, wp-dx));
    segments.push_back(Segment(wp-dx, wp-dy));
    segments.push_back(Segment(wp-dy, wp+dx));

    subdiv_circle(wp, segments, subdiv);
}

int main() {

  typedef Kokkos::DefaultNode::DefaultNodeType                 Node;
  typedef KokkosExamples::DummySparseKernel<Node>         SparseOps;
  typedef Kokkos::CrsGraph <       int,Node,SparseOps>        Graph;
  typedef Kokkos::CrsMatrix<double,int,Node,SparseOps>    DoubleMat;
  typedef Kokkos::CrsMatrix< float,int,Node,SparseOps>     FloatMat;
  typedef Kokkos::MultiVector<double,Node>                DoubleVec;
  typedef Kokkos::MultiVector<float,Node>                  FloatVec;

  std::cout << "Note, this class doesn't actually do anything. We are only testing that it compiles." << std::endl;

  // get a pointer to the default node
  Teuchos::RCP<Node> node = Kokkos::DefaultNode::getDefaultNode();

  // create the graph G
  const size_t numRows = 5;
  Graph G(numRows,node);

  // create a double-valued matrix dM using the graph G
  DoubleMat dM(G);
  // create a double-valued sparse kernel using the rebind functionality
  SparseOps::rebind<double>::other doubleKernel(node);
  // initialize it with G and dM
  doubleKernel.initializeStructure(G);
  doubleKernel.initializeValues(dM);
  // create double-valued vectors and initialize them
  DoubleVec dx(node), dy(node);
  // test the sparse kernel operator interfaces
  doubleKernel.multiply( Teuchos::NO_TRANS, 1.0, dx, dy);
  doubleKernel.multiply( Teuchos::NO_TRANS, 1.0, dx, 1.0, dy);
  doubleKernel.solve( Teuchos::NO_TRANS, Teuchos::UPPER_TRI, Teuchos::UNIT_DIAG, dy, dx);

  // create a float-valued matrix fM using the graph G
  FloatMat fM(G);
  // create a double-valued sparse kernel using the rebind functionality
  SparseOps::rebind<float>::other floatKernel(node);
  // initialize it with G and fM
  floatKernel.initializeStructure(G);
  floatKernel.initializeValues(fM);
  // create float-valued vectors and initialize them
  FloatVec fx(node), fy(node);
  // test the sparse kernel operator interfaces
  floatKernel.multiply( Teuchos::NO_TRANS, 1.0f, fx, fy);
  floatKernel.multiply( Teuchos::NO_TRANS, 1.0f, fx, 1.0f, fy);
  floatKernel.solve( Teuchos::NO_TRANS, Teuchos::UPPER_TRI, Teuchos::UNIT_DIAG, fy, fx);

  std::cout << "End Result: TEST PASSED" << std::endl;
  return 0;
}

/* >>> start tutorial code >>> */
int main( ){

    USING_NAMESPACE_ACADO

    // DEFINE VALRIABLES:
    // ---------------------------
       DifferentialState x, y;
       Function f;

       f << x*x + pow(y,2);

    // TEST THE FUNCTION f:
    // --------------------
       EvaluationPoint  z(f);
       EvaluationPoint dz(f);

       Vector xx(2);  Vector dx(2);

       xx(0) =  1.0;  dx(0) =  0.5;
       xx(1) =  1.0;  dx(1) =  0.1;

       z.setX( xx );  dz.setX( dx );


    // FORWARD DIFFERENTIATION:
    // ------------------------
       Vector ff = f.evaluate  ( z  );
       Vector df = f.AD_forward( dz );


    // PRINT THE RESULTS:
    // ------------------
       ff.print("result of evaluation      \n");
       df.print("result for the derivative \n");

    return 0;
}

void checkConsistency(const CTensor<float>& flow1, const CTensor<float>& flow2, CMatrix<float>& reliable, int argc, char** args) {
  int xSize = flow1.xSize(), ySize = flow1.ySize();
  int size = xSize * ySize;
  CTensor<float> dx(xSize,ySize,2);
  CTensor<float> dy(xSize,ySize,2);
  CDerivative<float> derivative(3);
  NFilter::filter(flow1,dx,derivative,1,1);
  NFilter::filter(flow1,dy,1,derivative,1);
  CMatrix<float> motionEdge(xSize,ySize,0);
  for (int i = 0; i < size; i++) {
    motionEdge.data()[i] += dx.data()[i]*dx.data()[i];
    motionEdge.data()[i] += dx.data()[size+i]*dx.data()[size+i];
    motionEdge.data()[i] += dy.data()[i]*dy.data()[i];
    motionEdge.data()[i] += dy.data()[size+i]*dy.data()[size+i];
  }

  for (int ay = 0; ay < flow1.ySize(); ay++)
    for (int ax = 0; ax < flow1.xSize(); ax++) {
      float bx = ax+flow1(ax, ay, 0);
      float by = ay+flow1(ax, ay, 1);
      int x1 = floor(bx);
      int y1 = floor(by);
      int x2 = x1 + 1;
      int y2 = y1 + 1;
      if (x1 < 0 || x2 >= xSize || y1 < 0 || y2 >= ySize)
      { reliable(ax, ay) = 0.0f; continue; }
      float alphaX = bx-x1; float alphaY = by-y1;
      float a = (1.0-alphaX) * flow2(x1, y1, 0) + alphaX * flow2(x2, y1, 0);
      float b = (1.0-alphaX) * flow2(x1, y2, 0) + alphaX * flow2(x2, y2, 0);
      float u = (1.0-alphaY)*a+alphaY*b;
      a = (1.0-alphaX) * flow2(x1, y1, 1) + alphaX * flow2(x2, y1, 1);
      b = (1.0-alphaX) * flow2(x1, y2, 1) + alphaX * flow2(x2, y2, 1);
      float v = (1.0-alphaY)*a+alphaY*b;
      float cx = bx+u;
      float cy = by+v;
      float u2 = flow1(ax,ay,0);
      float v2 = flow1(ax,ay,1);
      if (((cx-ax) * (cx-ax) + (cy-ay) * (cy-ay)) >= 0.01*(u2*u2 + v2*v2 + u*u + v*v) + 0.5f) {
        // Set to a negative value so that when smoothing is applied the smoothing goes "to the outside".
        // Afterwards, we clip values below 0.
        reliable(ax, ay) = -255.0f;
        continue;
      }
      if (motionEdge(ax, ay) > 0.01 * (u2*u2+v2*v2) + 0.002f) {
        reliable(ax, ay) = MOTION_BOUNDARIE_VALUE;
        continue;
      }
    }
}

bool MarginalizationFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    //printf("internal addr,%d, %d\n", (int)parameter_block_sizes().size(), num_residuals());
    //for (int i = 0; i < static_cast<int>(keep_block_size.size()); i++)
    //{
    //    //printf("unsigned %x\n", reinterpret_cast<unsigned long>(parameters[i]));
    //    //printf("signed %x\n", reinterpret_cast<long>(parameters[i]));
    //printf("jacobian %x\n", reinterpret_cast<long>(jacobians));
    //printf("residual %x\n", reinterpret_cast<long>(residuals));
    //}
    int n = marginalization_info->n;
    int m = marginalization_info->m;
    Eigen::VectorXd dx(n);
    for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
    {
        int size = marginalization_info->keep_block_size[i];
        int idx = marginalization_info->keep_block_idx[i] - m;
        Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(parameters[i], size);
        Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(marginalization_info->keep_block_data[i], size);
        if (size != 7)
            dx.segment(idx, size) = x - x0;
        else
        {
            dx.segment<3>(idx + 0) = x.head<3>() - x0.head<3>();
            dx.segment<3>(idx + 3) = 2.0 * Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
            if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).w() >= 0))
            {
                dx.segment<3>(idx + 3) = 2.0 * -Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
            }
        }
    }
    Eigen::Map<Eigen::VectorXd>(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx;
    if (jacobians)
    {

        for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
        {
            if (jacobians[i])
            {
                int size = marginalization_info->keep_block_size[i], local_size = marginalization_info->localSize(size);
                int idx = marginalization_info->keep_block_idx[i] - m;
                Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], n, size);
                jacobian.setZero();
                jacobian.leftCols(local_size) = marginalization_info->linearized_jacobians.middleCols(idx, local_size);
            }
        }
    }
    return true;
}

void
GradientsHdrCompression::hdrCompression(realType_t maxGradient_p, realType_t minGradient_p, realType_t expGradient_p,
			   bool bRescale01_p, imageType_t &rResultImage_p) const
{
  TimeMessage startHdrCompression("Gradient Domain HDR Compression");
  imageType_t li;
  {
    TimeMessage startClipValue("Determine clip values for gradients");
    imageType_t dx(m_imageDx);
    imageType_t dy(m_imageDy);
    // now clip values
    realType_t minGradientX,maxGradientX;
    minMaxValImage(dx,minGradientX,maxGradientX);
    realType_t minGradientY,maxGradientY;
    minMaxValImage(dy,minGradientY,maxGradientY);
    realType_t minValue=Min(minGradientX, minGradientY);
    realType_t maxValue=Max(maxGradientX, maxGradientY);
    double rangeValue=Max(-minValue, maxValue);
    realType_t const clipRange=rangeValue*maxGradient_p;
    realType_t const zeroRange=rangeValue*minGradient_p;
    startClipValue.Stop();

    TimeMessage startClip("Clipping Gradients");
    clipImage(dx,-clipRange,clipRange);
    zeroRangeImage(dx,-zeroRange,zeroRange);
    clipImage(dy,-clipRange,clipRange);
    zeroRangeImage(dy,-zeroRange,zeroRange);
    startClip.Stop();

    if(expGradient_p!=1.0){
      TimeMessage start("Pow() transformation of gradients");
      absPowImage(dx,expGradient_p);
      absPowImage(dy,expGradient_p);
    }

    TimeMessage startLaplace("Computing 2nd derivative");
    createLaplaceVonNeumannImage(dx,dy,li);
  }
  TimeMessage startSolve("Solving image");
  solveImage(li,rResultImage_p);
  startSolve.Stop();

  TimeMessage startRescale("Rescaling image");
  if(bRescale01_p){
    rResultImage_p.Rescale(0.0,1.0);
  } else {
    rResultImage_p.Rescale(m_dMinVal,m_dMaxVal);
  }
}

void R2Image::
GradientMagnitude(void) 
{
  // Compute squared gradient
  R2Image dx(*this); 
  R2Image dy(*this); 
  dx.XGradient();
  dx.Multiply(dx);
  dy.YGradient();
  dy.Multiply(dy);
  Clear();
  Add(dx);
  Add(dy);
  Sqrt();
}

Array<double,1> NewtonRaphson<F>::solve(Array<double,1>& currpos,const Array<double,1>& xtarget)
{
    unsigned long n;
    converged = false;
    Array<double,1> x = currpos.copy();
    Array<double,2> dx(x.extent(firstDim),1);
    target = xtarget;
    Array<double,1> y(target.extent(firstDim));
    Array<double,2> y_old(target.extent(firstDim),1);
    y_old(Range::all(),0) = target - f(x);
    for (n=0; n<maxit; n++) {
        Array<double,2> J = f.Jacobian(x);
        interfaceCLAPACK::SolveLinear(J,dx,y_old);
        x += dx(Range::all(),0);
        y  = target - f(x);
        if ((max(abs(dx))<eps)||(max(abs(y-y_old(Range::all(),0)))<eps)) {
            converged  = true;
            x_solution = x;
            return x;
        }
        y_old(Range::all(),0) = y;
    }
    if (!converged) throw std::runtime_error("Multidimensional Newton-Raphson failed to converge");
}

  // A Richardson approximation to the first derivative.  For
  // derivation, see
  // http://www2.math.umd.edu/~dlevy/classes/amsc466/lecture-notes/differentiation-chap.pdf
  double NumericalDerivatives::scalar_first_derivative(const Vector &x, int pos,
                                                       double h) const {
    Vector dx(x);
    dx[pos] = x[pos] + h;
    double fp1 = f_(dx);
    dx[pos] = x[pos] - h;
    double fm1 = f_(dx);
    dx[pos] = x[pos] + 2 * h;
    double fp2 = f_(dx);
    dx[pos] = x[pos] - 2 * h;
    double fm2 = f_(dx);

    double df = -fp2 + 8 * fp1 - 8 * fm1 + fm2;
    return df / (12 * h);
  }