bool rayIntersects(RayN<N, T> const & ray, T max_time = -1) const
    {
      if (max_time >= 0)
        return rayIntersectionTime(ray, max_time) >= 0;

      VectorT co = ray.getOrigin() - center;

      T c = co.squaredNorm() - radius * radius;
      if (c <= 0)  // origin is inside ball
        return true;

      T a = ray.getDirection().squaredNorm();
      T b = 2 * co.dot(ray.getDirection());

      // Solve quadratic a * t^2 + b * t + c = 0
      T b2 = b * b;
      T det = b2 - 4 * a * c;
      if (det < 0) return false;

      if (a > 0)
        return b <= 0 || det >= b2;
      else if (a < 0)
        return b >= 0 || det >= b2;
      else
        return false;
    }

Ipp32f BhattacharyyaDistance<Ipp32f>::doCalculate(const VectorT<Ipp32f>& v1, const VectorT<Ipp32f>& v2) const {

    if(v1.size()!=v2.size())
        fthrow(Exception, "Input vectors must have the same size.");

    Ipp32f B;

    #ifdef NICE_USELIB_IPP

        VectorT<Ipp32f> v1f(v1);
        v1f *= v2;

        ippsSqrt(v1f.getDataPointer(), v1f.getDataPointer(), v1f.size());
        ippsSum(v1f.getDataPointer(), v1f.size(), &B);
    
    #else // NICE_USELIB_IPP

        B = 0.0;
        for(uint i=0; i<v1.size(); ++i)
            B += std::sqrt(v1[i]*v2[i]);

    #endif // NICE_USELIB_IPP

    return std::sqrt(1-B);
}

      void operator()(LinPdeSysT const & pde_system,
                      SegmentT   const & segment,
                      StorageType      & storage,
                      MatrixT          & system_matrix,
                      VectorT          & load_vector)
      {
        typedef viennamath::equation                          equ_type;
        typedef viennamath::expr                              expr_type;
        typedef typename expr_type::interface_type            interface_type;
        typedef typename expr_type::numeric_type              numeric_type;

        typedef typename viennagrid::result_of::cell_tag<SegmentT>::type CellTag;

        std::size_t map_index = viennafvm::create_mapping(pde_system, segment, storage);

        system_matrix.clear();
        system_matrix.resize(map_index, map_index, false);
        load_vector.clear();
        load_vector.resize(map_index);


        for (std::size_t pde_index = 0; pde_index < pde_system.size(); ++pde_index)
        {
#ifdef VIENNAFVM_DEBUG
          std::cout << std::endl;
          std::cout << "//" << std::endl;
          std::cout << "//   Equation " << pde_index << std::endl;
          std::cout << "//" << std::endl;
#endif
          assemble(pde_system, pde_index,
                   segment, storage,
                   system_matrix, load_vector);

        } // for pde_index
      } // functor

void RowEchelon<T>::backSub(VectorT& x) const
{
  Assert(EB.n == 1);
  Assert(EB.m == R.m);
  x.resize(R.n);
  VectorT b;
  EB.getColRef(0,b);

  Assert((int)firstEntry.size() == R.m+1);
  x.setZero();
  int m=R.m,n=R.n;
  for(int i=m-1;i>=0;i--) {
    VectorT ri; R.getRowRef(i,ri);
    //solve to set R*x = b[i]
    //calculate alpha = dot between rest of x[>ji] and R[i]
    int ji=firstEntry[i];
    if(ji == n) continue;
    int ji2=firstEntry[i+1];  //(i+1==m?n:firstEntry[i+1]);
    T alpha;
    if(ji2 == n) alpha = Zero;
    else {
      VectorT rji2; rji2.setRef(ri,ji2,1,R.n-ji2);
      VectorT xji2; xji2.setRef(x,ji2,1,R.n-ji2);
      alpha = xji2.dot(rji2);
    }
    x[ji] = (b[i]-alpha)/ri[ji];
  }
}

void interval_set_element_iter_4_discrete_types()
{
    typedef IntervalSet<T> IntervalSetT;
    typedef typename IntervalSetT::interval_type IntervalT;
    typedef std::vector<T> VectorT;

    IntervalSetT set_a;
    set_a.add(I_I(1,3)).add(I_I(6,7));

    VectorT vec(5), cev(5);
    vec[0]=MK_v(1);vec[1]=MK_v(2);vec[2]=MK_v(3);vec[3]=MK_v(6);vec[4]=MK_v(7);
    cev[0]=MK_v(7);cev[1]=MK_v(6);cev[2]=MK_v(3);cev[3]=MK_v(2);cev[4]=MK_v(1);

    VectorT dest;
    std::copy(elements_begin(set_a), elements_end(set_a), std::back_inserter(dest));
    BOOST_CHECK_EQUAL( vec == dest, true );

    dest.clear();
    std::copy(elements_rbegin(set_a), elements_rend(set_a), std::back_inserter(dest));
    BOOST_CHECK_EQUAL( cev == dest, true );

    dest.clear();
    std::reverse_copy(elements_begin(set_a), elements_end(set_a), std::back_inserter(dest));
    BOOST_CHECK_EQUAL( cev == dest, true );

    dest.clear();
    std::reverse_copy(elements_rbegin(set_a), elements_rend(set_a), std::back_inserter(dest));
    BOOST_CHECK_EQUAL( vec == dest, true );
}

void EngineEntityT::Draw(bool FirstPersonView, const VectorT& ViewerPos) const
{
    MatSys::Renderer->PushMatrix(MatSys::RendererI::MODEL_TO_WORLD);
    MatSys::Renderer->PushLightingParameters();

    unsigned short Ent_Heading;
    unsigned short Ent_Pitch;
    unsigned short Ent_Bank;

    Entity->GetBodyOrientation(Ent_Heading, Ent_Pitch, Ent_Bank);

    // Get the currently set lighting parameters.
    const float* PosL=MatSys::Renderer->GetCurrentLightSourcePosition();
    VectorT      LightSourcePos   =VectorT(PosL[0], PosL[1], PosL[2]);
    float        LightSourceRadius=MatSys::Renderer->GetCurrentLightSourceRadius();

    const float* PosE=MatSys::Renderer->GetCurrentEyePosition();
    VectorT      EyePos=VectorT(PosE[0], PosE[1], PosE[2]);


    // Starting from world space, compute the position of the light source in model space.
    LightSourcePos=LightSourcePos-Entity->GetOrigin();         // Convert into unrotated model space.
    LightSourcePos=LightSourcePos.GetRotZ(-90.0+float(Ent_Heading)/8192.0*45.0);
    LightSourcePos=scale(LightSourcePos, 1.0/25.4);

    // Don't forget to scale the radius of the light source appropriately down (into model space), too.
    LightSourceRadius/=25.4f;


    // Do the same for the eye: Starting from world space, compute the position of the eye in model space.
    EyePos=EyePos-Entity->GetOrigin();         // Convert into unrotated model space.
    EyePos=EyePos.GetRotZ(-90.0+float(Ent_Heading)/8192.0*45.0);
    EyePos=scale(EyePos, 1.0/25.4);


    // Set the modified (now in model space) lighting parameters.
    MatSys::Renderer->SetCurrentLightSourcePosition(float(LightSourcePos.x), float(LightSourcePos.y), float(LightSourcePos.z));
    MatSys::Renderer->SetCurrentLightSourceRadius(LightSourceRadius);
    MatSys::Renderer->SetCurrentEyePosition(float(EyePos.x), float(EyePos.y), float(EyePos.z));


    // Set the ambient light color for this entity.
    // Paradoxically, this is not a global, but rather a per-entity value that is derived from the lightmaps that are close to that entity.
    const Vector3fT AmbientEntityLight=Entity->GetGameWorld()->GetAmbientLightColorFromBB(Entity->GetDimensions(), Entity->GetOrigin());
    MatSys::Renderer->SetCurrentAmbientLightColor(AmbientEntityLight.x, AmbientEntityLight.y, AmbientEntityLight.z);


    MatSys::Renderer->Translate(MatSys::RendererI::MODEL_TO_WORLD, float(Entity->GetOrigin().x), float(Entity->GetOrigin().y), float(Entity->GetOrigin().z));
    MatSys::Renderer->RotateZ  (MatSys::RendererI::MODEL_TO_WORLD, 90.0f-float(Ent_Heading)/8192.0f*45.0f);
    MatSys::Renderer->Scale    (MatSys::RendererI::MODEL_TO_WORLD, 25.4f);

    Entity->Draw(FirstPersonView, (float)length(ViewerPos-Entity->GetOrigin()));


    MatSys::Renderer->PopLightingParameters();
    MatSys::Renderer->PopMatrix(MatSys::RendererI::MODEL_TO_WORLD);
}

 void set_row(const Uint array_idx, const VectorT& row)
 {
   cf3_assert(row.size() == row_size());
 
   Row row_to_set = m_array[array_idx];
   
   for(Uint j=0; j<row.size(); ++j)
     row_to_set[j] = row[j];
 }

  void set_row(const Uint array_idx, const VectorT& row)
  {
    if (row.size() != row_size(array_idx))
      m_array[array_idx].resize(row.size());

    Uint j=0;
    boost_foreach( const typename VectorT::value_type& v, row)
      m_array[array_idx][j++] = v;
  }

void vector_test(const VectorT& A, const VectorT& B, Accumulator& Result)
{
    const Uint sizeA = A.size();
    const Uint sizeB = B.size();
    for(Uint i = 0, j = 0; i != sizeA && j != sizeB; ++i, ++j)
        test(A[i], B[j], Result);

    Result.exact(sizeA == sizeB);
};

int main()
{
    VectorT<int> vint;
    VectorT<double> vdouble;
    
    vint.Normalize();
    vdouble.Normalize();
    
    return 0;
}

void DiagonalMatrixTemplate<T>::mulPseudoInverse(const VectorT& a, VectorT& b) const
{
  if(BaseT::n != a.n) FatalError(MatrixError_ArgIncompatibleDimensions);
  if(b.n == 0)
    b.resize(this->n);
  else if(b.n != this->n) FatalError(MatrixError_DestIncompatibleDimensions);
  
  ItT v=this->begin();
  VectorIterator<T> va=a.begin(),vb=b.begin();
  for(int i=0; i<this->n; i++, v++,va++,vb++)
    *vb = *va * PseudoInv(*v);
}

    T rayIntersectionTime(RayN<N, T> const & ray, T max_time = -1) const
    {
      VectorT co = ray.getOrigin() - center;

      T c = co.squaredNorm() - radius * radius;
      if (c <= 0)  // origin is inside ball
        return 0;

      // We could do an early test to see if the distance from the ray origin to the ball is less than
      // max_time * ray.getDirection().norm(), but it would involve a square root so might as well solve the quadratic.

      T a = ray.getDirection().squaredNorm();
      T b = 2 * co.dot(ray.getDirection());

      // Solve quadratic a * t^2 + b * t + c = 0
      T det = b * b - 4 * a * c;
      if (det < 0) return -1;

      T d = std::sqrt(det);
      T t = -1;
      if (a > 0)
      {
        T s0 = -b - d;
        if (s0 >= 0)
          t = s0 / (2 * a);
        else
        {
          T s1 = -b + d;
          if (s1 >= 0)
            t = s1 / (2 * a);
        }
      }
      else if (a < 0)
      {
        T s0 = -b + d;
        if (s0 <= 0)
          t = s0 / (2 * a);
        else
        {
          T s1 = -b - d;
          if (s1 <= 0)
            t = s1 / (2 * a);
        }
      }

      if (max_time >= 0 && t > max_time)
        return -1;
      else
        return t;
    }

void MatrixTemplate<T>::copyCols(const VectorT* cols)
{
  CHECKEMPTY();
  for(int j=0; j<n; j++)
  {
    if(cols[j].n != m)
    {
      RaiseErrorFmt(WHERE_AM_I,MatrixError_IncompatibleDimensions,m,n,cols[j].n,-1);
    }
    VectorT tempcol;
    getColRef(j,tempcol);
    tempcol.copy(cols[j]);
  }
}

void SparseMatrixTemplate_RM<T>::mulTranspose(const VectorT& a,VectorT& x) const
{
  if(x.n == 0) x.resize(n);
  if(x.n != n) {
    FatalError("Destination vector has incorrect dimensions");
  }
  if(a.n != m) {
    FatalError("Source vector has incorrect dimensions");
  }
  x.setZero();
  for(int i=0;i<m;i++) {
    for(ConstRowIterator it=rows[i].begin();it!=rows[i].end();it++)
      x(it->first) += it->second*a(i);
  }
}

    MatrixT diag(VectorT const                 &v,
                 typename MatrixT::ScalarType  zero =
                     static_cast<typename MatrixT::ScalarType>(0))
    {
        MatrixT diag(v.size(), v.size(), zero);
        diag.set_zero(zero);
        //populate diagnals:
        std::vector<IndexType> indices;
        for (graphblas::IndexType ix = 0; ix < v.size(); ++ix) {
            indices.push_back(ix);
        }

        graphblas::buildmatrix(diag, indices.begin(), indices.begin(), v.begin(), v.size());
        return diag;
    }

      /**
       * Compute the search direction based on the current (inverse) Hessian
       * approximation and given gradient.
       *
       * @param[out] pk The negative product of the inverse Hessian and gradient
       * direction gk.
       * @param[in] gk Gradient direction.
       **/
      inline void search_direction(VectorT &pk, const VectorT &gk) const {
        std::vector<Scalar> alphas(_buf.size());
        typename
          boost::circular_buffer<UpdateT>::const_reverse_iterator buf_rit;
        typename boost::circular_buffer<UpdateT>::const_iterator buf_it;
        typename std::vector<Scalar>::const_iterator alpha_it;
        typename std::vector<Scalar>::reverse_iterator alpha_rit;

        pk.noalias() = -gk;
        for (buf_rit = _buf.rbegin(), alpha_rit = alphas.rbegin();
             buf_rit != _buf.rend();
             buf_rit++, alpha_rit++) {
          Scalar alpha;
          const Scalar &rhoi(boost::get<0>(*buf_rit));
          const VectorT &yi(boost::get<1>(*buf_rit));
          const VectorT &si(boost::get<2>(*buf_rit));

          alpha = rhoi * si.dot(pk);
          pk -= alpha * yi;
          *alpha_rit = alpha;
        }
        pk *= _gammak;
        for (buf_it = _buf.begin(), alpha_it = alphas.begin();
             buf_it != _buf.end();
             buf_it++, alpha_it++) {
          Scalar beta;
          const Scalar &rhoi(boost::get<0>(*buf_it));
          const VectorT &yi(boost::get<1>(*buf_it));
          const VectorT &si(boost::get<2>(*buf_it));

          beta = rhoi*yi.dot(pk);
          pk += (*alpha_it - beta)*si;
        }
      }

void QRDecomposition<T>::backSub(const VectorT& b, VectorT& x) const
{
  if(x.n == 0) x.resize(QR.n);
  Assert(QR.m == b.n);
  Assert(QR.n == x.n);

  /* compute rhs = Q^T b */
  VectorT rhs;
  QtMul(b,rhs);
  if(QR.m == QR.n) {
    /* Solve R x = rhs, storing x in-place */
    UBackSubstitute(QR,rhs,x);
    //UBackSubstitute(QR,x,x);
  }
  else if(QR.m > QR.n) {
    //solve the top part of R x = rhs
    MatrixT R1; R1.setRef(QR,0,0,1,1,QR.n,QR.n);
    VectorT rhs1; rhs1.setRef(rhs,0,1,QR.n);
    UBackSubstitute(R1,rhs1,x);
  }
  else {
    cerr<<"What do we do with m < n?"<<endl;
    MatrixPrinter mprint(QR); mprint.mode = MatrixPrinter::AsciiShade;
    cerr<<mprint<<endl;
    //solve the left part of R x = rhs
    MatrixT R1; R1.setRef(QR,0,0,1,1,QR.m,QR.m);
    VectorT x1; x1.setRef(x,0,1,QR.m);
    UBackSubstitute(R1,rhs,x1);
    getchar();
  }
}

      Requires_t<mtl::traits::is_distributed<VectorT>>
    initVector(VectorT& x) const
    {
#ifdef HAVE_PARALLEL_MTL4
      x.init_distribution(col_distribution(*fullMatrix), num_cols(*fullMatrix));
#endif
      set_to_zero(x);
    }

void
set_v(
  VectorT         vec,
  IndexObj const& obj)
{
  for (index_type p=0; p<vec.size(); ++p)
    vec(p) = obj(p);
}

void
check_v(
  VectorT         vec,
  IndexObj const& obj)
{
  for (index_type p=0; p<vec.size(); ++p)
    test_assert(vec(p) == obj(p));
}