kernel/Operations.h

Go to the documentation of this file.

#ifndef OPERATIONS_H
#define OPERATIONS_H

#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <string>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include "Point.h"
#include "Connection.h"

#define verbose 1

inline void toCPT3(BPType& val, PType& point ) {
  point(0) = val(0);   point(1) = val(1);   point(2) = val(2);
}

inline void toBoost3( PType& val, BPType& point ) {
  point(0) = val(0);   point(1) = val(1);   point(2) = val(2);
}

inline void toBoost3( const PType& val, BPType& point ) {
  point(0) = val(0);   point(1) = val(1);   point(2) = val(2);
}


inline int sgn(DataType a)  {
  if ( fabs(a) < TOL ) return 0;
  else if (a < 0) return -1;
  else if (a > 0) return 1;
  else { assert(0); }
  return -2;
}

inline int sgn2(DataType a)  {
  if (a < 0) return -1;
  if (a >= 0) return 1;
}

inline DataType norm_2(PType val) {
  DataType result;
  BPType boostTmp(3,0.);
  toBoost3(val,boostTmp);

  result = boost::numeric::ublas::norm_2(boostTmp);
  return result;
}

BPType normalize( BPType& a ) {
  BPType result(a);

  if (norm_2(a) > TOL ) result /= norm_2(a);
  else result.resize(a.size(),0.);

  return result;
}

PType normalize( PType& a ) {
  PType result(a);
  BPType boostTmp(3,0);
  toBoost3(result,boostTmp);
  boostTmp = normalize(boostTmp);
  toCPT3(boostTmp,result);
  return result;
}

inline int octant(BPType a)  {
  if      (sgn(a(0)) >= 0 &&  sgn(a(1)) >= 0 && sgn(a(2)) >= 0  ) { return 0; }
  else if (sgn(a(0)) <= 0 &&  sgn(a(1)) >= 0 && sgn(a(2)) >= 0  ) { return 1; }
  else if (sgn(a(0)) <= 0 &&  sgn(a(1)) <= 0 && sgn(a(2)) >= 0  ) { return 2; }
  else if (sgn(a(0)) >= 0 &&  sgn(a(1)) <= 0 && sgn(a(2)) >= 0  ) { return 3; }
  else if (sgn(a(0)) >= 0 &&  sgn(a(1)) >= 0 && sgn(a(2)) <= 0  ) { return 4; }
  else if (sgn(a(0)) <= 0 &&  sgn(a(1)) >= 0 && sgn(a(2)) <= 0  ) { return 5; }
  else if (sgn(a(0)) <= 0 &&  sgn(a(1)) <= 0 && sgn(a(2)) <= 0  ) { return 6; }
  else if (sgn(a(0)) >= 0 &&  sgn(a(1)) <= 0 && sgn(a(2)) <= 0  ) { return 7; }
}

inline int octant(PType a)  {
  BPType boostTmp(3,0);
  toBoost3(a,boostTmp);
  return octant(boostTmp);
}

inline int octant(BPType a, BPType b)  {
  if (sgn(a(0)) == sgn(b(0)) || sgn(a(0)) == 0 || sgn(b(0)) == 0 ) {
    if (sgn(a(1)) == sgn(b(1)) || sgn(a(1)) == 0 || sgn(b(1)) == 0 ) {
      if (sgn(a(2)) == sgn(b(2)) || sgn(a(2)) == 0 || sgn(b(2)) == 0 )
        return 1;
    }
  }
  return 0;
}

inline int octant(PType a, PType b)  {
  BPType Ba(3,0), Bb(3,0.);
  toBoost3(a,Ba);
  toBoost3(b,Bb);
  return octant(Ba,Bb);
}

inline int octant2(BPType a, BPType b)  {
  if (sgn(a(0)) == sgn(b(0)) || sgn(a(0)) == 0 || sgn(b(0)) == 0 ) {
    if (sgn(a(1)) == sgn(b(1)) || sgn(a(1)) == 0 || sgn(b(1)) == 0 ) {
      if (sgn(a(2)) == sgn(b(2)) || sgn(a(2)) == 0 || sgn(b(2)) == 0 )
        return 1;
    }
  }
  return 0;
}

inline int octant2(PType a, PType b)  {
  BPType Ba(3,0), Bb(3,0.);
  toBoost3(a,Ba);
  toBoost3(b,Bb);
  return octant2(Ba,Bb);
}

BPType sin3D( BPType& a ) {
  BPType result(a);

  result(0) = std::sin(a(0));
  result(1) = std::sin(a(1));
  result(2) = std::sin(a(2));

  return result;
}

BPType multElem( BPType& a, BPType& b) {
  BPType result(3,0.);

  result(0) = a(0)*b(0);
  result(1) = a(1)*b(1);
  result(2) = a(2)*b(2);

  return result;
}

DataType innerProd3D(BPType& a, BPType& b) {
  DataType result = 0.;

  result = a(0)*b(0) + a(1)*b(1) + a(2)*b(2);

  return result;
}

DataType innerProd3D(PType& a, PType& b) {
  DataType result = 0.;

  result = a(0)*b(0) + a(1)*b(1) + a(2)*b(2);

  return result;
}

DataType innerProd3D(PType a, PType b) {
  DataType result = 0.;

  result = a(0)*b(0) + a(1)*b(1) + a(2)*b(2);

  return result;
}

DataType cross( BPType& a, BPType& b) {
  PType result;

  result(0) = a(1)*b(2) - a(2)*b(1);
  result(1) = a(2)*b(0) - a(0)*b(2);

  return norm_2(result);
}

BPType cross3D(BPType& a, BPType& b) {
  BPType result(3,0.);

  result(0) = a(1)*b(2) - a(2)*b(1);
  result(1) = a(2)*b(0) - a(0)*b(2);
  result(2) = a(0)*b(1) - a(1)*b(0);

  return result;
}

PType cross3D(PType& a, PType& b) {
  PType result;

  result(0) = a(1)*b(2) - a(2)*b(1);
  result(1) = a(2)*b(0) - a(0)*b(2);
  result(2) = a(0)*b(1) - a(1)*b(0);

  return result;
}

BPType cross3D(BPType& a, BPType& b, int& par) {
  BPType result(3,0.);

  result(0) = a(1)*b(2) - a(2)*b(1);
  result(1) = a(2)*b(0) - a(0)*b(2);
  result(2) = a(0)*b(1) - a(1)*b(0);

  par = 0;   // intersecting

  if ( norm_2(result) < TOL ) {
    if (octant(a,b) == 1 ) {
      par = 1;   // parallel
    }
    else {
      par = -1;   // anti-parallel
    }
  }

  return result;
}


PType cross3D(PType& a, PType& b, int& par) {
  PType result;
  BPType tmp(3,0.);

  result(0) = a(1)*b(2) - a(2)*b(1);
  result(1) = a(2)*b(0) - a(0)*b(2);
  result(2) = a(0)*b(1) - a(1)*b(0);

  par = 0;   // intersecting

  toBoost3(result,tmp);
  if ( norm_2(tmp) < TOL ) {
    if (octant(a,b) == 1 ) {
      par = 1;   // parallel
    }
    else {
      par = -1;   // anti-parallel
    }
  }

  return result;
}

DataType angle3D_axis( BPType a, BPType b, BPType n)  {
  //std::cout << "angle3D_axis\n";
  BPType c(3,0.), d(3,0.);
  DataType theta;

  a = normalize(a);
  b = normalize(b);
  n = normalize(n);

  //std::cout << "   angle3D_axis a=" << a << "   b=" << b << "   n=" << n << std::endl;

  if ( norm_2(a - n) < TOL || norm_2(b - n) < TOL ) {
    theta = 0.;
    return theta;
  }

  c = a - n * inner_prod(a,n);
  d = b - n * inner_prod(b,n);

  theta = std::acos( inner_prod(c,d) / ( norm_2(c)*norm_2(d) ) )/d2r;

  //std::cout << "   angle3D_axis theta (deg)=" << theta << std::endl;

  return theta;
}

DataType angle3D_axis( PType a, PType b, PType n)  {
  BPType Ba(3,0), Bb(3,0),Bn(3,0);
  toBoost3(a,Ba);
  toBoost3(b,Bb);
  toBoost3(n,Bn);
  return angle3D_axis(Ba,Bb,Bn);
}

DataType angle3D2_axis( BPType a, BPType b, BPType n)  {
  //std::cout << "angle3D2_axis\n";
  BPType c(3,0.), d(3,0.), ntmp(3,0.);
  DataType theta;

  a = normalize(a);
  b = normalize(b);
  n = normalize(n);

  if ( norm_2(a - n) < TOL ||
       norm_2(b - n) < TOL ||
       norm_2(a + n) < TOL ||
       norm_2(b + n) < TOL ||
       norm_2(a - b) < TOL ||
       norm_2(a + b) < TOL ) {
    theta = 0.;
    return theta;
  }

  c = a - n * inner_prod(a,n);
  d = b - n * inner_prod(b,n);

  theta = std::acos( inner_prod(c,d) / ( norm_2(c)*norm_2(d) ) )/d2r;

  ntmp = cross3D(c,d);

  if ( octant(n,ntmp) == 0 ) {
    theta *= -1.;
  }

  return theta;
}

DataType angle3D2_axis( PType a, PType b, PType n)  {
  BPType Ba(3,0), Bb(3,0),Bn(3,0);
  toBoost3(a,Ba);
  toBoost3(b,Bb);
  toBoost3(n,Bn);
  return angle3D2_axis(Ba,Bb,Bn);
}

DataType angle3D3_axis( BPType a, BPType b, BPType n, int& flip)  {
  //std::cout << "angle3D3_axis\n";
  BPType c(3,0.), d(3,0.), ntmp(3,0.);
  DataType theta;

  a = normalize(a);
  b = normalize(b);
  n = normalize(n);

  //std::cout << "   angle3D3_axis a=" << a << "   b=" << b << "   n=" << n << std::endl;

  if ( norm_2(a - n) < TOL ||
       norm_2(b - n) < TOL ||
       norm_2(a + n) < TOL ||
       norm_2(b + n) < TOL ||
       norm_2(a - b) < TOL ||
       norm_2(a + b) < TOL ) {
    theta = 0.;
    return theta;
  }

  c = a - n * inner_prod(a,n);
  d = b - n * inner_prod(b,n);

  theta = std::acos( inner_prod(c,d) / ( norm_2(c)*norm_2(d) ) )/d2r;

  ntmp = cross3D(a,b);

  //std::cout << "   angle3D3_axis c=" << c << "   d=" << d << "   ntmp=" << ntmp << std::endl;

  if ( inner_prod(n,ntmp) > 0 ) {
    ( mklog() << "FLIP!!!!!!!!!\t" ).flush();
    flip = 1;
    theta *= -1.;
  }
  else {
    flip = 0;
  }

  //std::cout << "   angle3D3_axis theta (deg)=" << theta << std::endl;

  return theta;
}

DataType angle3D3_axis( PType a, PType b, PType n, int& f)  {
  BPType Ba(3,0), Bb(3,0),Bn(3,0);
  toBoost3(a,Ba);
  toBoost3(b,Bb);
  toBoost3(n,Bn);
  return angle3D3_axis(Ba,Bb,Bn,f);
}

BPType rotate3D( BPType& u, BPType& theta)  {
  BPType result(3,0.), utmp(4,1.), rtmp(4,1.);
  MType m(4,4,0.);
  m(0,0) = 1.;   m(1,1) = 1.;   m(2,2) = 1.;  m(3,3) = 1.;

  theta *= d2r;

  DataType ct1 = std::cos(theta(2));
  DataType ct2 = std::cos(theta(0));
  DataType ct3 = std::cos(theta(1));
  DataType st1 = std::sin(theta(2));
  DataType st2 = std::sin(theta(0));
  DataType st3 = std::sin(theta(1));

  m(0,0) = (ct3*ct1 + st3*st2*st1 );
  m(0,1) = (-ct3*st1 + st3*st2*ct1 );
  m(0,2) = (st3*ct2 );
  m(1,0) = (ct2*st1 );
  m(1,1) = (ct2*ct1 );
  m(1,2) = (-st2 );
  m(2,0) = (-st3*ct1 + ct3*st2*st1 );
  m(2,1) = (st3*st1 + ct3*st2*ct1 );
  m(2,2) = (ct3*ct2 );

  utmp(0) = u(0);   utmp(1) = u(1);   utmp(2) = u(2);
  rtmp = prod(m,utmp);
  result(0) = rtmp(0);  result(1) = rtmp(1);   result(2) = rtmp(2);

  return result;
}

bool InvertMatrix(const MType& input, MType& inverse)
{
  typedef boost::numeric::ublas::permutation_matrix<std::size_t> pmatrix;
  typedef boost::numeric::ublas::identity_matrix<std::size_t> imatrix;

  // create a working copy of the input
  MType A(input);

  // create a permutation matrix for the LU-factorization
  pmatrix pm(A.size1());

  // perform LU-factorization
  int res = 1.;
  res = boost::numeric::ublas::lu_factorize(A, pm);

  if (res != 0)
    return false;

  // create identity matrix of "inverse"
  inverse.assign(imatrix (A.size1()));

  // backsubstitute to get the inverse
  boost::numeric::ublas::lu_substitute(A, pm, inverse);

  return true;
}

DataType determinant3(const MType& A) {

  return ( A(0,0)*A(1,1)*A(2,2) +  A(0,2)*A(1,2)*A(2,0) + A(0,2)*A(1,0)*A(2,1)
           - A(0,2)*A(1,1)*A(2,0) - A(0,1)*A(1,0)*A(2,2) - A(0,0)*A(1,2)*A(2,1) );
}


BPType pointFinM( BPType p, MType A) {
  BPType point(3,0.), dtemp(4,1.), ptemp(4,1.);
  MType Atransp(A);

  dtemp(0) = A(0,3); dtemp(1) = A(1,3); dtemp(2) = A(2,3);
  Atransp(0,3) = 0.;
  Atransp(1,3) = 0.;
  Atransp(2,3) = 0.;
  Atransp = boost::numeric::ublas::trans(Atransp);

  dtemp = DataType(-1.)*boost::numeric::ublas::prod(Atransp,dtemp);

  Atransp(0,3) = dtemp(0);
  Atransp(1,3) = dtemp(1);
  Atransp(2,3) = dtemp(2);

  ptemp(0) = p(0); ptemp(1) = p(1); ptemp(2) = p(2);
  ptemp = boost::numeric::ublas::prod(Atransp,ptemp);

  point(0) = ptemp(0);   point(1) = ptemp(1);   point(2) = ptemp(2);

  return point;
}

MType FinM( MType A) {
  BPType dtemp(4,1.);
  MType Atransp(A);

  dtemp(0) = A(0,3); dtemp(1) = A(1,3); dtemp(2) = A(2,3);
  Atransp(0,3) = 0.;
  Atransp(1,3) = 0.;
  Atransp(2,3) = 0.;
  Atransp = boost::numeric::ublas::trans(Atransp);

  dtemp = DataType(-1.)*boost::numeric::ublas::prod(Atransp,dtemp);

  Atransp(0,3) = dtemp(0);
  Atransp(1,3) = dtemp(1);
  Atransp(2,3) = dtemp(2);

  return Atransp;
}

MType tranMat3D( BPType move ) {
  typedef boost::numeric::ublas::identity_matrix<std::size_t> imatrix;
  MType m(4,4);

  m.assign(imatrix (4));
  m(0,3) = move(0);
  m(1,3) = move(1);
  m(2,3) = move(2);

  return m;
}

MType tranMat3D( PType move ) {
  BPType Bmove(3,0.);
  toBoost3(move,Bmove);
  MType m(4,4);
  m = tranMat3D(Bmove);

  return m;
}

MType scalMat3D( BPType scale ) {
  typedef boost::numeric::ublas::identity_matrix<std::size_t> imatrix;
  MType m(4,4);

  m.assign(imatrix (4));

  m(0,0) = scale(0);
  m(1,1) = scale(1);
  m(2,2) = scale(2);

  return m;
}

MType scalMat3D( PType scale ) {
  BPType Bscale(3,0.);
  toBoost3(scale,Bscale);
  return scalMat3D(Bscale);
}

MType rotMat3D( BPType theta ) {
  typedef boost::numeric::ublas::identity_matrix<std::size_t> imatrix;
  MType m(4,4);

  m.assign(imatrix (4));
  DataType ct1 = std::cos(theta(2)*d2r);
  DataType ct2 = std::cos(theta(0)*d2r);
  DataType ct3 = std::cos(theta(1)*d2r);
  DataType st1 = std::sin(theta(2)*d2r);
  DataType st2 = std::sin(theta(0)*d2r);
  DataType st3 = std::sin(theta(1)*d2r);

  m(0,0) = (ct3*ct1 + st3*st2*st1 );
  m(0,1) = (-ct3*st1 + st3*st2*ct1 );
  m(0,2) = (st3*ct2 );
  m(1,0) = (ct2*st1 );
  m(1,1) = (ct2*ct1 );
  m(1,2) = (-st2 );
  m(2,0) = (-st3*ct1 + ct3*st2*st1 );
  m(2,1) = (st3*st1 + ct3*st2*ct1 );
  m(2,2) = (ct3*ct2 );

  return m;
}

MType rotMat3D( PType theta ) {
  BPType Btheta(3,0.);
  toBoost3(theta,Btheta);
  return rotMat3D(Btheta);
}

MType axisAngleMat3D( BPType axis, DataType angle ) {
  typedef boost::numeric::ublas::identity_matrix<std::size_t> imatrix;
  MType m(3,3);
  DataType ct, st, ct1;

  ct = std::cos(angle);
  st = std::sin(angle);
  ct1 = DataType(1.) - std::cos(angle);

  m.assign(imatrix (3));
  m(0,0) = ct1*axis(0)*axis(0) + ct;
  m(0,1) = ct1*axis(0)*axis(1) - axis(2)*st;
  m(0,2) = ct1*axis(0)*axis(2) + axis(1)*st;

  m(1,0) = ct1*axis(1)*axis(0) + axis(2)*st;
  m(1,1) = ct1*axis(1)*axis(1) + ct;
  m(1,2) = ct1*axis(1)*axis(2) - axis(0)*st;

  m(2,0) = ct1*axis(2)*axis(0) - axis(2)*st;
  m(2,1) = ct1*axis(2)*axis(1) + axis(1)*st;
  m(2,2) = ct1*axis(2)*axis(2) + ct;

  return m;
}

MType axisAngleMat3D( PType axis, DataType angle ) {
  BPType tmp(3,0.);
  toBoost3(axis,tmp);
  return axisAngleMat3D(tmp,angle);
}

MType axisAngleMat3DH( BPType axis, DataType angle ) {
  typedef boost::numeric::ublas::identity_matrix<std::size_t> imatrix;
  MType m(4,4);
  DataType ct, st, ct1;

  ct = std::cos(angle*d2r);
  st = std::sin(angle*d2r);
  ct1 = DataType(1.) - std::cos(angle*d2r);

  m.assign(imatrix (4));
  m(0,0) = ct1*axis(0)*axis(0) + ct;
  m(0,1) = ct1*axis(0)*axis(1) - axis(2)*st;
  m(0,2) = ct1*axis(0)*axis(2) + axis(1)*st;

  m(1,0) = ct1*axis(1)*axis(0) + axis(2)*st;
  m(1,1) = ct1*axis(1)*axis(1) + ct;
  m(1,2) = ct1*axis(1)*axis(2) - axis(0)*st;

  m(2,0) = ct1*axis(2)*axis(0) - axis(1)*st;
  m(2,1) = ct1*axis(2)*axis(1) + axis(0)*st;
  m(2,2) = ct1*axis(2)*axis(2) + ct;

  return m;
}

MType axisAngleMat3DH( PType axis, DataType angle ) {
  BPType tmp(3,0.);
  toBoost3(axis,tmp);
  return axisAngleMat3DH(tmp,angle);
}

BPType matTaitBryant( MType A) {
  BPType rotation(3,0.);

  if ( ( std::fabs(A(2,0)) - DataType(1.) ) < TOL ) {
    rotation(1) = std::asin( -A(2,0) )/d2r;
    if ( rotation(1) < DataType(90.) ) {
      rotation(0) = std::atan2( A(2,1), A(2,2) )/d2r;
      rotation(2) = std::atan2( A(1,0), A(0,0) )/d2r;
    }
    else {
      rotation(0) = std::atan2( -A(2,1), -A(2,2) )/d2r;
      rotation(2) = std::atan2( -A(1,0), -A(0,0) )/d2r;
    }
  }
  else if ( ( A(2,0) + DataType(1.) ) < TOL ) {
    rotation(0) = std::atan2( A(0,1), A(0,2) )/d2r;
    rotation(1) = DataType(90.);
    rotation(2) = 0.;
  }
  else if ( ( A(2,0) - DataType(1.) ) < TOL ) {
    rotation(0) = std::atan2( -A(0,1), -A(0,2) )/d2r;
    rotation(1) = DataType(-90.);
    rotation(2) = 0.;
  }

  return rotation;
}

BPType angle3D2( BPType& u, BPType& v)  {
  MType rmat(4,4);
  BPType utmp(u), vtmp(v), result(3,0.);
  BPType ntmp(3,0.);
  DataType atmp;

  int par;

  //std::cout << "   angle3D2 u=" << u << "   v=" << v << std::endl;

  ntmp = cross3D(u,v,par);
  ntmp = normalize(ntmp);

  //std::cout << "   ." << std::endl;

  if ( par == 0 ) {
    atmp = angle3D_axis(u,v,ntmp);
    rmat = axisAngleMat3DH(ntmp,atmp);
    result = matTaitBryant(rmat);
    ( mklog() << "   angle3D2 called on intersecting lines result (deg)=" << result << std::endl ).flush();
    return result;
  }
  else if ( par == 1 ) {
    ( mklog() << "   angle3D2 called on parallel lines result (deg)=" << result << std::endl ).flush();
    return result;
  }
  else if ( par == -1 ) {
    ntmp(0) = 1.;   ntmp(1) = 0.;   ntmp(2) = 0.;
    if ( boost::numeric::ublas::norm_2(u - ntmp) < TOL || boost::numeric::ublas::norm_2(v - ntmp) < TOL ) {
      ntmp(0) = ((DataType)rand()/(DataType)RAND_MAX);
      ntmp(1) = ((DataType)rand()/(DataType)RAND_MAX);
      ntmp(2) = ((DataType)rand()/(DataType)RAND_MAX);
      ntmp = normalize(ntmp);
      ( mklog() << "      ntmp " << ntmp << std::endl ).flush();
    }
    atmp = angle3D_axis(u,v,ntmp);
    rmat = axisAngleMat3DH(ntmp,atmp);
    result = matTaitBryant(rmat);
    ( mklog() << "   angle3D2 called on anti-parallel lines result (deg)=" << result << std::endl ).flush();
    return result;
  }
  else { assert(0); }
  return (BPType) 0;
}

void dist3D_Line_to_Line( BPType L0P0, BPType L0P1, BPType L1P0, BPType L1P1, BPType& L0I, BPType& L1I, BPType& CN, DataType& dist, int& lcase)
{
  BPType u(3,0.), v(3,0.), w(3,0.), dP(3,0.);
  DataType a, b, c, d, e, D, sc, tc;
  u = L0P1 - L0P0;
  v = L1P1 - L1P0;
  w = L0P0 - L1P0;
  a = boost::numeric::ublas::inner_prod(u,u);         // always >= 0
  b = boost::numeric::ublas::inner_prod(u,v);
  c = boost::numeric::ublas::inner_prod(v,v);         // always >= 0
  d = boost::numeric::ublas::inner_prod(u,w);
  e = boost::numeric::ublas::inner_prod(v,w);
  D = a*c - b*b;        // always >= 0

  if (D < TOL) {          // the lines are almost parallel
    sc = 0.0;
    tc = (b>c ? d/b : e/c);    // use the larger denominator
  }
  else {
    sc = (b*e - c*d) / D;
    tc = (a*e - b*d) / D;
  }

  dP = w + (sc * u) - (tc * v);  // =  L1(sc) - L2(tc)
  CN = normalize(dP);

  L0I = L0P0 + (sc * u);
  L1I = L1P0 + (tc * v);

  dist = boost::numeric::ublas::norm_2(dP);   // set the closest distance

  if (norm_2(CN) < TOL && dist < TOL && norm_2(L0P0-L0I) < TOL && norm_2(L0P0-L1I) < TOL) {
    lcase = 2; 
  }
  else if (norm_2(L0P0-L0I) < TOL && dist > TOL) {
    lcase = 1; 
  }
  else if (norm_2(CN) < TOL && dist < TOL) {
    lcase = 0; 
  }
  else if (norm_2(CN) > TOL && dist > TOL) {
    lcase = 3; 
  }
}

void dist3D_Line_to_Line( PType L0P0, PType L0P1, PType L1P0, PType L1P1, PType& L0I,
                          PType& L1I, PType& CN, DataType& dist, int& lcase) {
  BPType BL0P0(3,0.), BL0P1(3,0.), BL1P0(3,0.), BL1P1(3,0.), BL0I(3,0.), BL1I(3,0.), BCN(3,0.);
  toBoost3(L0P0,BL0P0);
  toBoost3(L0P1,BL0P1);
  toBoost3(L1P0,BL1P0);
  toBoost3(L1P1,BL1P1);
  toBoost3(L0I,BL0I);
  toBoost3(L1I,BL1I);
  toBoost3(CN,BCN);
  dist3D_Line_to_Line(BL0P0,BL0P1,BL1P0,BL1P1,BL0I,BL1I,BCN,dist,lcase);

}

BPType project( BPType& u, BPType& v )  {
  BPType result(3,0.);
  MType Pv(3,3);

  Pv.assign(imatrix(3));
  Pv(0,0) = v(0)*v(0);
  Pv(0,1) = v(0)*v(1);
  Pv(0,2) = v(0)*v(2);

  Pv(1,0) = v(0)*v(1);
  Pv(1,1) = v(1)*v(1);
  Pv(1,2) = v(1)*v(2);

  Pv(2,0) = v(0)*v(2);
  Pv(2,1) = v(1)*v(2);
  Pv(2,2) = v(2)*v(2);

  result = prod(Pv,u);

  return result;

}

PType project( PType& u, PType& v )  {
  BPType Bu(3,0.), Bv(3,0.), result(3,0.);
  PType answer;//(0.);
  toBoost3(u,Bu);
  toBoost3(v,Bv);
  result = project(Bu,Bv);
  toCPT3(result,answer);
  return answer;

}

DataType projectScalar( BPType& u, BPType& v )  {
  DataType result;
  result = inner_prod(u,v)/norm_2(v);
  return result;
}

DataType projectScalar( PType& u, PType& v )  {
  BPType Bu(3,0.), Bv(3,0.);
  DataType answer;
  toBoost3(u,Bu);
  toBoost3(v,Bv);
  answer = projectScalar(Bu,Bv);
  return answer;
}

DataType distance_point_to_line3D(BPType a, BPType b, BPType p) {
  DataType dist = -1., length;
  BPType n(3,0.), v(3,0), w(3,0.);
  n = b - a;
  n = normalize(n);
  v = a - p;
  w = p - a;

  length = boost::numeric::ublas::norm_2( (a-b) );
  dist = boost::numeric::ublas::norm_2( (a-p)-( boost::numeric::ublas::inner_prod((a-p),n) )*n );

  if ( projectScalar(w,n) > length ) {
    dist = -1;
  }
  else if ( projectScalar(w,n) < 0 ) {
    dist = -1;
  }

  return dist;
}

DataType distance_point_to_line3D(PType a, PType b, PType p) {
  BPType Ba(3,0.), Bb(3,0.), Bp(3,0.);
  DataType answer;
  toBoost3(a,Ba);
  toBoost3(b,Bb);
  toBoost3(p,Bp);
  answer = distance_point_to_line3D(Ba,Bb,Bp);
  return answer;
}

MType DenavitHartenbergMat( DataType radius, DataType alpha, DataType depth, DataType theta ) {
  MType DH(4,4);
  DataType ca, sa, ct, st;

  ca = std::cos(alpha*d2r);
  sa = std::sin(alpha*d2r);
  ct = std::cos(theta*d2r);
  st = std::sin(theta*d2r);

  DH(0,0) = ct;
  DH(0,1) = -st*ca;
  DH(0,2) = st*sa;
  DH(0,3) = radius*ct;

  DH(1,0) = st;
  DH(1,1) = ct*ca;
  DH(1,2) = -ct*sa;
  DH(1,3) = radius*st;

  DH(2,0) = 0.;
  DH(2,1) = sa;
  DH(2,2) = ca;
  DH(2,3) = depth;

  DH(3,0) = 0.;
  DH(3,1) = 0.;
  DH(3,2) = 0.;
  DH(3,3) = 1.;

  return DH;
}

MType DenavitHartenbergMat_spericalWrist( DataType t4, DataType t5, DataType t6, DataType d6 ) {
  MType DH(4,4);
  DataType ct4, st4, ct5, st5, st6, ct6;

  ct4 = std::cos(t4*d2r);
  st4 = std::sin(t4*d2r);
  ct5 = std::cos(t5*d2r);
  st5 = std::sin(t5*d2r);
  ct6 = std::cos(t6*d2r);
  st6 = std::sin(t6*d2r);

  DH(0,0) = ct4*ct5*ct6 - st4*st6;
  DH(0,1) = -ct4*ct5*st6 - st4*ct6;
  DH(0,2) = ct4*st5;
  DH(0,3) = ct4*st5*d6;

  DH(1,0) = st4*ct5*ct6 + ct4*st6;
  DH(1,1) = -st4*ct5*st6 + ct4*ct6;
  DH(1,2) = st4*st5;
  DH(1,3) = st4*st5*d6;

  DH(2,0) = -st5*ct6;
  DH(2,1) = st5*st6;
  DH(2,2) = ct5;
  DH(2,3) = ct5*d6;

  DH(3,0) = 0.;
  DH(3,1) = 0.;
  DH(3,2) = 0.;
  DH(3,3) = 1.;

  return DH;
}

void prodMVP( MType Tnet, std::vector<BPType>& in, std::vector<BPType>& out) {
  BPType v(in[0].size()+1,1.);
  out.resize(in.size());
  for (unsigned int i = 0; i < in.size(); i++) {
    v(0) = in[i](0);    v(1) = in[i](1);  v(2) = in[i](2);
    v = prod(Tnet, v);
    out[i](0) = v(0);    out[i](1) = v(1);  out[i](2) = v(2);
  }
}

void prodMVP( MType Tnet, std::vector<PType>& in, std::vector<PType>& out) {
  BPType v(in[0].size()+1,1.);
  out.resize(in.size());
  for (unsigned int i = 0; i < in.size(); i++) {
    v(0) = in[i](0);    v(1) = in[i](1);  v(2) = in[i](2);
    v = prod(Tnet, v);
    out[i](0) = v(0);    out[i](1) = v(1);  out[i](2) = v(2);
  }
}

BPType prodMP( MType Tnet, BPType in) {
  BPType v(in.size()+1,1.), result(3,0.);
  v(0) = in(0);    v(1) = in(1);  v(2) = in(2);
  v = prod(Tnet, v);
  result(0) = v(0);   result(1) = v(1);  result(2) = v(2);
  return result;
}

PType prodMP( MType Tnet, PType in) {
  BPType v(in.size()+1,1.), tmp(3,0.);
  PType answer; //(0.);
  v(0) = in(0);    v(1) = in(1);  v(2) = in(2);
  v = prod(Tnet, v);
  tmp(0) = v(0);   tmp(1) = v(1);  tmp(2) = v(2);
  toCPT3(tmp,answer);
  return answer;
}

MType SkewSymmetric( BPType alpha ) {
  MType SS(4,4);

  SS(0,0) = 0.;
  SS(0,1) = -alpha(2);
  SS(0,2) = alpha(1);
  SS(0,3) = 0.;

  SS(1,0) = alpha(2);
  SS(1,1) = 0.;
  SS(1,2) = -alpha(0);
  SS(1,3) = 0.;

  SS(2,0) = -alpha(1);
  SS(2,1) = alpha(0);
  SS(2,2) = 0.;
  SS(2,3) = 0.;

  SS(3,0) = 0.;
  SS(3,1) = 0.;
  SS(3,2) = 0.;
  SS(3,3) = 1.;

  return SS;
}

void calcVelMVP( MType Tnet, std::vector<BPType>& origins, std::vector<BPType>& axis, std::vector<BPType>& angVel, std::vector<BPType>& linVel, std::vector<BPType>& in, std::vector<BPType>& out) {
  BPType v(in[0].size()+1,1.);
  out.resize(in.size());

  MType skew(4,4);
  BPType rtmp(4,0.);
  for (unsigned int i = 0; i < in.size(); i++) {

    v.resize(4,0.);
    for (unsigned int j = 0; j < angVel.size(); j++ ) {
      rtmp(0) = in[i](0) - angVel[j](0);    rtmp(1) = in[i](1) - angVel[j](1);  rtmp(2) = in[i](2) - angVel[j](2);
      skew = SkewSymmetric(angVel[j]);
      v = v + prod(skew,rtmp) + linVel[j];
    }
    out[i](0) = v(0);    out[i](1) = v(1);  out[i](2) = v(2);
  }
}

void numVelMVP( MType Tnet0, MType Tnet1, std::vector<BPType>& in, std::vector<BPType>& out, DataType dt) {
  BPType v(in[0].size()+1,1.);
  out.clear();
  std::copy (in.begin (), in.end (), std::back_inserter (out));

  MType vtmp(4,4);

  vtmp = (Tnet1 - Tnet0)*(DataType(1.)/dt);
  vtmp(3,3) = 1.;
  for (unsigned int i = 0; i < in.size(); i++) {
    v(0) = in[i](0);    v(1) = in[i](1);  v(2) = in[i](2);
    v = prod(vtmp,v);
    out[i](0) = v(0);    out[i](1) = v(1);  out[i](2) = v(2);
  }
}

void numVelMVP( MType Tnet0, MType Tnet1, std::vector<PType>& in, std::vector<PType>& out, DataType dt) {
  BPType v(in[0].size()+1,1.);
  out.clear();
  std::copy (in.begin (), in.end (), std::back_inserter (out));

  MType mtmp(4,4);

  mtmp = (Tnet1 - Tnet0)*(DataType(1.)/dt);
  mtmp(3,3) = 1.;
  for (unsigned int i = 0; i < in.size(); i++) {
    v(0) = in[i](0);    v(1) = in[i](1);  v(2) = in[i](2);
    v = prod(mtmp,v);
    out[i](0) = v(0);    out[i](1) = v(1);  out[i](2) = v(2);
  }
}

void printID(std::vector<int> tag) {
  for (unsigned int i = 0; i < tag.size(); i++) {
    ( mklog() << tag[i] ).flush();
    if (i < tag.size()-1) ( mklog() << ":" ).flush();
  }
  ( mklog() << std::endl ).flush();
}

std::string stringID(std::vector<int> tag) {
  std::string id;
  for (unsigned int i = 0; i < tag.size(); i++) {
    std::stringstream ss;
    ss << tag[i];
    id += ss.str();
    if (i < tag.size()-1) id += ":";
  }
  return id;
}

void trimID(std::vector<int> &tag) {
  for (int i = tag.size()-1; i > 0; i--) {
    if (tag[i-1] == -1) {
      tag.pop_back();
    }
    else {
      if (tag[i-1] < 0) tag.pop_back();
      ( mklog() << "\nDone Trimming Id Tag\n" ).flush();
    }
  }
  printID(tag);
  ( mklog() << std::endl ).flush();
}

int checkTag(std::vector<int> tag) {
  int type;
  if ( tag[tag.size()-1] < 0 ) {
    type = 0;
  }
  else {
    type = 1;
  }
  return type;
}

void assignVertices3D(const std::vector<BPType>& in, ads::FixedArray<3,DataType> * out) {
  ( mklog() << " assigning Points\n" ).flush();
  typedef ads::FixedArray<3,DataType> point_type;

  point_type* piter = out;
  for (unsigned int i = 0; i < in.size(); i++) {
    piter[0] = in[i][0];  piter[1] = in[i][1];  piter[2] = in[i][2];
    piter++;
  }
}

void assignConnections3D(std::vector<Facet<DataType> >& in, ads::FixedArray<3,int> * out) {
  ( mklog() << " assigning Conections\n" ).flush();
  typedef ads::FixedArray<3,int> multi_index_type;
  multi_index_type* citer = out;
  for (unsigned int i = 0; i < in.size(); i++) {
    citer[0] = in[i].getNthCon(0);  citer[1] = in[i].getNthCon(1);  citer[2] = in[i].getNthCon(2);
    citer++;
  }

}

PType intersection_line2facet(PType I_a, PType I_b, int& nump, int& numc, ads::FixedArray<3,DataType> * verts, ads::FixedArray<3,int> * conns , int& flag) {
  PType intPt;

  PType p_0, p_1, p_2;
  MType X(3,3),  Xinv(3,3);
  PType tuv(-1.), delta;
  DataType detCheck=0., distance=0., dtmp=0., r=20.0;

  ( mklog() << " : I_a= " << I_a <<  " I_b= " << I_b << " nump = " << nump
            << " numc = " << numc << std::endl ).flush();

  for ( register int j = 0; j < numc; j++ ) {

    p_0 = verts[ conns[j](0) ];
    p_1 = verts[ conns[j](1) ];
    p_2 = verts[ conns[j](2) ];

    r = norm_2(p_0-p_1);
    if ( r >  norm_2(p_0-p_2) ) r = norm_2(p_0-p_2);
    if ( r >  norm_2(p_1-p_2) ) r = norm_2(p_1-p_2);


    distance = distance_point_to_line3D(I_a, I_b, p_0);
    dtmp = distance_point_to_line3D(I_a, I_b, p_1);
    if (dtmp < distance) distance = dtmp;
    dtmp = distance_point_to_line3D(I_a, I_b, p_2);
    if (dtmp < distance) distance = dtmp;

    if ( distance >= 0. && distance <= r ) {
      ( mklog() << " distance " << distance << " r = " << r << std::endl ).flush();

      X(0,0) = ( I_a(0) - I_b(0) );   X(0,1) = ( p_1(0) - p_0(0) );    X(0,2) = ( p_2(0) - p_0(0) );
      X(1,0) = ( I_a(1) - I_b(1) );   X(1,1) = ( p_1(1) - p_0(1) );    X(1,2) = ( p_2(1) - p_0(1) );
      X(2,0) = ( I_a(2) - I_b(2) );   X(2,1) = ( p_1(2) - p_0(2) );    X(2,2) = ( p_2(2) - p_0(2) );

      delta(0) = ( I_a(0) - p_0(0) );
      delta(1) = ( I_a(1) - p_0(1) );
      delta(2) = ( I_a(2) - p_0(2) );

      detCheck = determinant3(X);

      if (detCheck > TOL) {

        if (InvertMatrix(X,Xinv)) {
          ( mklog() << "   detCheck == " << detCheck << std::endl ).flush();
          InvertMatrix(X,Xinv);
          BPType boostTmp(3,0.);
          toBoost3(delta,boostTmp);
          boostTmp = prod(Xinv,boostTmp);
          toCPT3(boostTmp,tuv);

        }
        else {
          ( mklog() << "InvertMatrix false\n" ).flush();
          tuv(0) = 3.;   tuv(1) = 3.;   tuv(2) = 3.;

        }
      }
      ( mklog() << " norm_2(tuv) = " << norm_2(tuv) << " tuv " << tuv << std::endl
                << " p_0 " << p_0
                << " p_1 " << p_1
                << " p_2 " << p_2 << std::endl ).flush();
      if (norm_2(tuv) <= 2) {
        if (tuv(0) >= 0. && tuv(0) <= 1. ) {
          if (tuv(1) >= 0. && tuv(1) <= 1. ) {
            if (fabs(tuv(2)) <=1 ) {
              if ( (tuv(1)+tuv(2)) <= 1. ) {
                ( mklog() << "==== tuv" << tuv << std::endl
                          << " ````` flagging facetB[" << j << "]\n"
                          << " INTERSECTION = " << tuv(0)*( I_b - I_a ) + I_a
                          << std::endl ).flush();
                intPt =  tuv(0)*( I_b - I_a ) + I_a;

                ( mklog() << " facetPtB->getNthCon(0) " << conns[j](0)
                          << " facetPtB->getNthCon(1) " << conns[j](1)
                          << " facetPtB->getNthCon(2) " << conns[j](2) << std::endl
                          << " p_0 " << p_0
                          << " p_1 " << p_1
                          << " p_2 " << p_2 << std::endl ).flush();
                flag = 1;
                return intPt;
              }
            }
          }
        }
      }
    }
  }

  flag = 0;
  return intPt;
}

void reflect(std::vector<PType>& in, std::vector<PType>& out, PType n ) {
  MType ref(4,4);
  DataType a = n(0), b = n(1), c = n(2);

  BPType v(in[0].size()+1,1.);
  out.clear();
  out.resize(in.size());\

  ref(0,0) = 1. - 2.*a*a;
  ref(0,1) = -2.*a*b;
  ref(0,2) = -2.*a*c;
  ref(0,3) = 0.;

  ref(1,0) = -2.*a*b;
  ref(1,1) = 1. - 2.*b*b;
  ref(1,2) = -2*b*c;
  ref(1,3) = 0.;

  ref(2,0) = -2*a*c;
  ref(2,1) = -2*b*c;
  ref(2,2) = 1-2*c*c;
  ref(2,3) = 0.;

  ref(3,0) = 0.;
  ref(3,1) = 0.;
  ref(3,2) = 0.;
  ref(3,3) = 1.;

  ( mklog() << " REFLECTION MATRIX " << ref << std::endl ).flush();

  for (unsigned int i = 0; i < in.size(); i++) {
    v(0) = in[i](0);    v(1) = in[i](1);  v(2) = in[i](2);
    v = prod(ref,v);
    out[i](0) = v(0);    out[i](1) = v(1);  out[i](2) = v(2);
  }

}

MType reflMat3D( BPType n ) {
  MType ref(4,4);
  DataType a = n(0), b = n(1), c = n(2);

  ref(0,0) = 1. - 2.*a*a;
  ref(0,1) = -2.*a*b;
  ref(0,2) = -2.*a*c;
  ref(0,3) = 0.;

  ref(1,0) = -2.*a*b;
  ref(1,1) = 1. - 2.*b*b;
  ref(1,2) = -2*b*c;
  ref(1,3) = 0.;

  ref(2,0) = -2*a*c;
  ref(2,1) = -2*b*c;
  ref(2,2) = 1.-2*c*c;
  ref(2,3) = 0.;

  ref(3,0) = 0.;
  ref(3,1) = 0.;
  ref(3,2) = 0.;
  ref(3,3) = 1.;

  ( mklog() << "reflection matrix " << ref << std::endl ).flush();

  return ref;
}

MType reflMat3D( PType n ) {
  BPType Bn(3,0.);
  toBoost3(n,Bn);
  return reflMat3D(Bn);
}

bool fexists(const char *filename)
{
  std::ifstream ifile(filename);
  return ifile;
}

#endif // OPERATIONS_H

---