Exemple #1
0
int main(int argc, char *argv[])
{
    SimpleMatrix m;
    assert (m.nrow() == 0);
    assert (m.ncol() == 0);

    m.resize(3, 2);
    assert (m.nrow() == 3);
    assert (m.ncol() == 2);

    m.zero();
    assert (m[0][0] == 0.0);
    assert (m[2][1] == 0.0);
    
    std::vector<double> col(3, 1.0);
    m.appendCol(col);
    assert (m[0][0] == 0.0);
    assert (m[2][2] == 1.0);

    m.resize(2,3);
    std::vector<double> row(3, 2.0);
    m.appendRow(row);
    assert (m[0][0] == 0.0);
    assert (m[2][2] == 2.0);

    
    return 0;
}
std::vector<SimpleMatrix*> readMatricesFromFile(std::string filename)
{
    std::ifstream infile(filename.c_str());
    CHECK(infile.good()) << "Failed to open pose file "
        << filename << std::endl;

    string hashtag;
    string matrix_name;

    std::vector<SimpleMatrix*> res;

    infile >> hashtag >> matrix_name;
    do {
        int rows, cols;
        infile >> rows >> cols;
        SimpleMatrix *a = new SimpleMatrix(rows, cols);
        for(int j = 0; j < rows; ++j)
            for(int i = 0; i < cols; ++i)
            {
                float val;
                infile >> val;
                a->val(j, i) = val;
            }
        //LOG(WARNING) << a->rows() << " " << a->cols();
        res.push_back(a);
    } while(infile >> hashtag >> matrix_name);

    return res;
}
Exemple #3
0
void adjointInput::computeJacglambda(double t, SiconosVector& x, SiconosVector& lambda, SiconosVector& z, SimpleMatrix& B)
{

  double *g = B.getArray();
#ifdef SICONOS_DEBUG
  std::cout << "computeJacglambda " << " at " << " " << t << std::endl;
#endif

  SP::SiconosVector K2P(new SiconosVector(2));
  SP::SiconosVector P(new SiconosVector(2));
  P->setValue(0, x(2));
  P->setValue(1, x(3));

  prod(*K2, *P, *K2P, true);

  SP::SiconosVector betatmp(new SiconosVector(2));
  beta(t, x, betatmp);

  g[0] = betatmp->getValue(0)  ;
  g[4] = 0.0;
  g[1] = betatmp->getValue(1)  ;
  g[5] = 0.0;
  g[2] = K2P->getValue(0)      ;
  g[6] = 0.0;
  g[3] = K2P->getValue(1)      ;
  g[7] = 0.0 ;



#ifdef SICONOS_DEBUG
  std::cout << "modif Jacglambda : \n";
  B.display();
#endif

}
void FirstOrderLinearR::computeB(double time, SiconosVector& z, SimpleMatrix& B)
{
  if (_pluginJacglambda->fPtr)
  {
    ((FOMatPtr1) _pluginJacglambda->fPtr)(time, B.size(0), B.size(1), &(B)(0, 0), z.size(), &(z)(0));
  }
}
	// ----------------------------------------------------------------------
	void 
		LocalizationDLSModule::
		estimate_position( void ) throw(){

			shawn::Vec* est_pos;
			double distance_r1,distance_r2;
			int count =0;
			distance_r1 = owner().estimate_distance(node(), *linearization_tool_);
			distance_r1 *=distance_r1;
			NeighborInfoList neighbors;
			collect_neighbors( neighborhood(), lat_anchors, neighbors );

			for( std::vector<shawn::Node*>::iterator iter = beacons_->begin(); iter!=beacons_->end();iter++,count++){
				for( ConstNeighborInfoListIterator iter1 = neighbors.begin(); iter1!=neighbors.end(); iter1++){
					if((*iter)->id() == (*iter1)->node().id() ){
						distance_r2 = (*iter1)->distance();
						//distance_r2 = owner().estimate_distance(node(), **iter);

						if(distance_r2 == std::numeric_limits<double>::max() ||distance_r2 == std::numeric_limits<double>::min())
							vector_r_->at(count,0) = 0;
						else    
							vector_r_->at(count,0) = 0.5*( distance_r1 - (distance_r2*distance_r2) + (vector_r_->at(count,0)));
						break;
					}
				}		
			}	
			SimpleMatrix<double>*  temp = new SimpleMatrix<double>(3,1);	
			*temp= *matrix_a_ * *vector_r_;
			est_pos = new shawn::Vec(temp->at(0,0),temp->at(1,0),temp->at(2,0));
			*est_pos += (linearization_tool_->has_est_position())?
				(linearization_tool_->est_position()):(linearization_tool_->real_position());
			node_w().set_est_position( *est_pos );
			delete temp;
	}
void FirstOrderLinearR::computeF(double time, SiconosVector& z, SimpleMatrix& F)
{
  if (_pluginf->fPtr)
  {
    ((FOMatPtr1)(_pluginf->fPtr))(time, F.size(0), F.size(1), &(F)(0, 0), z.size(), &(z)(0));
  }
}
void FirstOrderLinearR::computeD(double time, SiconosVector& z, SimpleMatrix& D)
{
  if (_pluginJachlambda->fPtr)
  {
    ((FOMatPtr1)(_pluginJachlambda->fPtr))(time, D.size(0), D.size(1), &(D)(0, 0), z.size(), &(z)(0));
  }
}
void FirstOrderLinearR::computeC(double time, SiconosVector& z, SimpleMatrix& C)
{
  if (_pluginJachx->fPtr)
  {
    ((FOMatPtr1)(_pluginJachx->fPtr))(time, C.size(0), C.size(1), &(C)(0, 0), z.size(), &(z)(0));
  }
}
void printMatrix(SimpleMatrix& m, PlinkInputFile& f1) {
  for (int i = 0; i < m.nrow(); i++ ) {
    for (int j = 0; j < m.ncol(); j++) {
      int geno = (int) m[i][j];
      char ref = f1.ref[j];
      char alt = f1.alt[j];
      switch(geno) {
        case 0:
          printf("%c %c\t", ref, ref);
          break;
        case 1:
          printf("%c %c\t", ref, alt);
          break;
        case 2:
          printf("%c %c\t", alt, alt);
          break;
        default:
          if ( geno < 0)
            printf(". .\t");
          break;
          printf("ERROR reading!\n");
      }
    }
    printf("\n");
  }
}
void NonlinearRelationReduced2::computeJacglambda(double t, SiconosVector& lambda, SimpleMatrix& B)
{

  B.setValue(0, 0, -40.0);
  B.setValue(1, 0,  0.0);
  B.setValue(0, 1,  0.0);
  B.setValue(1, 1, -40.0);
}
Exemple #11
0
void testSimpleMatrixDataAccess() {
    SimpleMatrix m = createSimpleMatrix(2, 2);

    BOOST_CHECK( m.getElement(0, 0) == 0 );
    BOOST_CHECK( m.getElement(0, 1) == 1 );
    BOOST_CHECK( m.getElement(1, 0) == 0 );
    BOOST_CHECK( m.getElement(1, 1) == 1 );
}
void FirstOrderType1R::computeJachx(double time, SiconosVector& x, SiconosVector& z, SimpleMatrix& C)
{
  //
  assert(_pluginJachx && "FirstOrderType1R::computeJacobianH() failed; not linked to a plug-in function.");

  ((Type1Ptr)(_pluginJachx->fPtr))(x.size(), &(x)(0), C.size(0), C.getArray(), z.size(), &(z)(0));

}
void NonlinearRelationReduced2::computeJachx(double t, SiconosVector& x, SiconosVector& lambda, SimpleMatrix& C)
{

  C.setValue(0, 0, -1);
  C.setValue(0, 1, 0);
  C.setValue(1, 0, 0);
  C.setValue(1, 1, -1);

}
Exemple #14
0
/**
 * Creates linearly initialized SimpleMatrix,
 * [0,1,2,..]
 * [0,1,2,..]
 * [..      ].
 */
SimpleMatrix createSimpleMatrix(int rows, int cols) {
    SimpleMatrix m = SimpleMatrix(rows, cols);

    for (int r = 0; r < rows; ++r) {
        for (int c = 0; c < cols; ++c) {
            m.setElement(r, c, c);
        }
    }

    return m;
}
/* XXX Find out if we can use an elementwise ublas operation */
SP::SiconosVector compareMatrices(const SimpleMatrix& data, const SimpleMatrix& ref)
{
  SimpleMatrix diff(data.size(0), data.size(1));
  SP::SiconosVector res(new SiconosVector(data.size(1)));
  diff = data - ref;
  for (unsigned int i = 0; i < data.size(0); ++i)
  {
    for (unsigned int j = 0; j < data.size(1); ++j)
      diff(i, j) /= 1 + fabs(ref(i, j));
  }
  diff.normInfByColumn(res);
  return res;

}
Exemple #16
0
/** default function to compute jacobianH
 *  \param double : current time
 *  \param index for jacobian (0: jacobian according to x, 1 according to lambda)
 */
void adjointInput::computeJachx(double t, SiconosVector& x, SiconosVector& lambda, SiconosVector& z, SimpleMatrix& C)
{

#ifdef SICONOS_DEBUG
  std::cout << "computeJachx " << " at " << " " << t << std::endl;
#endif

  SP::SiconosVector betatmp(new SiconosVector(2));
  beta(t, x, betatmp);

  SP::SiconosMatrix jacbetaXtmp(new SimpleMatrix(2, 2));

  JacobianXbeta(t, x, jacbetaXtmp);


  C(0, 0) = x(3);
  C(0, 1) = -x(2) ;
  C(0, 2) = (-x(1) + 1.0) ;
  C(0, 3) = x(0);
  C(1, 0) = 0.0;
  C(1, 1) = 0.0 ;
  C(1, 2) = 0.0 ;
  C(1, 3) = 0.0;



#ifdef SICONOS_DEBUG
  std::cout << "modif Jachx : \n";
  C.display();
#endif


}
Exemple #17
0
void toMatrix(const SimpleMatrix& from, Matrix* to) {
  const int nr = from.nrow();
  const int nc = from.ncol();
  to->Dimension(nr, nc);

  // copy value
  for (int i = 0; i < nr; ++i) {
    for (int j = 0; j < nc; ++j) {
      (*to)(i, j) = from[i][j];
    }
  }
  // copy col labels
  for (int i = 0; i < nc; ++i) {
    (*to).SetColumnLabel(i, from.getColName()[i].c_str());
  }
}
Exemple #18
0
void testSimpleMatrixMultiplyInequalSize() {
    SimpleVector v = createSimpleVector(3);
    SimpleMatrix m = createSimpleMatrix(2, 2);

    try {
        m.multiplyLeft(v);
        BOOST_ERROR( "Exception should have been thrown!" );
    } catch (const char* e) {
    }

    try {
        m.multiplyRight(v);
        BOOST_ERROR( "Exception should have been thrown!" );
    } catch (const char* e) {
    }
}
void NonlinearRelationWithSignInversed::computeJachlambda(double t, SiconosVector& x, SiconosVector& lambda, SimpleMatrix& D)
{
  //    double *h = &(*_jachlambda)(0,0);
#ifdef SICONOS_DEBUG
  std::cout << "NonlinearRelationWithSignInversed::computeJachlambda " << " at " << " " << t << std::endl;
#endif
  D.zero();

}
void NonlinearRelationWithSignInversed::computeJachx(double t, SiconosVector& x, SiconosVector& lambda, SimpleMatrix& C)
{
  C.setValue(0, 0, 1);
  C.setValue(0, 1, 0);
  C.setValue(1, 0, 0);
  C.setValue(1, 1, 1);
  C.setValue(2, 0, 1);
  C.setValue(2, 1, 0);
  C.setValue(3, 0, 0);
  C.setValue(3, 1, 1);

#ifdef SICONOS_DEBUG
  std::cout << "NonlinearRelationWithSignInversed::computeJachx computeJachx " << " at " << " " << t << ":" << std::endl;
  C.display();
  std::cout << std::endl;
#endif

}
Exemple #21
0
void testSimpleMatrixMultiplyRight() {
    SimpleVector v = SimpleVector(3);
    SimpleMatrix m = createSimpleMatrix(2, 3);

    v.setElement(0, 3);
    v.setElement(1, 2);
    v.setElement(2, 1);

    // [0,1,2] * [3] = [4]
    // [0,1,2]   [2]   [4]
    //           [1]
    Vector* res = m.multiplyRight(v);

    BOOST_CHECK( res->size() == 2 );
    BOOST_CHECK( res->getElement(0) == 4 );
    BOOST_CHECK( res->getElement(1) == 4 );

    delete res;
}
Exemple #22
0
void testSimpleMatrixMultiplyLeft() {
    SimpleVector v = SimpleVector(3);
    SimpleMatrix m = createSimpleMatrix(3, 2);

    v.setElement(0, 3);
    v.setElement(1, 2);
    v.setElement(2, 1);

    // [3,2,1] * [0,1] = [0,6]
    //           [0,1]
    //           [0,1]
    Vector* res = m.multiplyLeft(v);

    BOOST_CHECK( res->size() == 2 );
    BOOST_CHECK( res->getElement(0) == 0 );
    BOOST_CHECK( res->getElement(1) == 6 );

    delete res;
}
Exemple #23
0
SimpleMatrix Quaternion::getRotationMatrix() const 
{
	Quaternion tmp = *this;
	tmp.normalize();						// first normalize
											// matrix entries
	double vxSquare = 2 * tmp.v.x * tmp.v.x;
	double vySquare = 2 * tmp.v.y * tmp.v.y;
	double vzSquare = 2 * tmp.v.z * tmp.v.z;
	double vx_vy = 2 * tmp.v.x * tmp.v.y;
	double vx_vz = 2 * tmp.v.x * tmp.v.z;
	double vy_vz = 2 * tmp.v.y * tmp.v.z;
	double v_x = 2 * s * tmp.v.x;
	double v_y = 2 * s * tmp.v.y;
	double v_z = 2 * s * tmp.v.z;

	SimpleMatrix mat;						// generate the matrix
	mat.at(0,0) = 1 - vySquare -  vzSquare;
	mat.at(0,1) = vx_vy + v_z;
	mat.at(0,2) = vx_vz - v_y;
	mat.at(1,0) = vx_vy - v_z; 
	mat.at(1,1) = 1 - vxSquare - vzSquare;
	mat.at(1,2) = vy_vz + v_x;
	mat.at(2,0) = vx_vz + v_y;
	mat.at(2,1) = vy_vz - v_x;
	mat.at(2,2) = 1 - vxSquare - vySquare;

	return mat;
}
int main(int argc, char *argv[])
{
  // check loading all genotypes to memory
  PlinkInputFile f1 ("testPlinkInputFile");
  SimpleMatrix m;
  f1.readIntoMatrix(&m);
  for (int i = 0; i < m.nrow(); i++ ) {
    for (int j = 0; j < m.ncol(); j++) {
      int geno = (int) m[i][j];
      printf("%d ", geno);
    }
    printf("\n");
  }
  printMatrix(m, f1);

  // loading some people and marker
  printf("----------------------------------------------------------------------\n");
  m.clear();
  f1.readIntoMatrix(&m, NULL, NULL);
  std::vector<std::string> personName, markerName;
  f1.readIntoMatrix(&m, &personName, &markerName);      
  printMatrix(m, f1);


  // loading some people and marker
  printf("----------------------------------------------------------------------\n");
  m.clear();
  std::vector<std::string> people;
  std::vector<std::string> marker;
  people.push_back("1");
  f1.readIntoMatrix(&m, &people, &marker);
  printMatrix(m, f1);


  // loading some people and marker
  printf("----------------------------------------------------------------------\n");
  m.clear();
  people.push_back("6");
  marker.push_back("snp1");
  f1.readIntoMatrix(&m, &people, &marker);
  printMatrix(m, f1);

  // loading some people and marker
  printf("----------------------------------------------------------------------\n");
  m.clear();
  people.push_back("2");
  marker.push_back("snp2");
  f1.readIntoMatrix(&m, &people, &marker);
  printMatrix(m, f1);

  return 0;
}
Exemple #25
0
void adjointInput::computeJachlambda(double t, SiconosVector& x, SiconosVector& lambda, SiconosVector& z, SimpleMatrix& D)
{
#ifdef SICONOS_DEBUG
  std::cout << "computeJachlambda " << " at " << " " << t << std::endl;
#endif

  D(0, 0) = 0.0              ;
  D(0, 1) = 1.0 ;
  D(1, 0) = -1.0             ;
  D(1, 1) = 0.0 ;

#ifdef SICONOS_DEBUG
  std::cout << "modif Jachlambda : \n";
  D.display();
#endif

}
Exemple #26
0
T matrix_diff(const SimpleMatrix<T,0,0>& a, const SimpleMatrix<T,0,0>& b) {
    BOOST_CHECK_EQUAL(a.rows(), b.rows());
    BOOST_CHECK_EQUAL(a.cols(), b.cols());
    
    T residual = 0;
    for (index_t r = 0; r < a.rows(); ++r) {
        for (index_t c = 0; c < a.cols(); ++c) {
            T z = a[r][c] - b[r][c];
            residual += z * z;
        }
    }
    return std::sqrt(residual);
}
void NonlinearRelationWithSignInversed::computeJacglambda(double t, SiconosVector& lambda, SimpleMatrix& B)
{
  B.setValue(0, 0, 0);
  B.setValue(1, 0, -10.0 * (1 + lambda(3)));
  B.setValue(0, 1, -10.0 * (1 + lambda(2)));
  B.setValue(1, 1, 0);
  B.setValue(0, 2, 10.0 * (1 - lambda(1)));
  B.setValue(1, 2, 0);
  B.setValue(0, 3, 0);
  B.setValue(1, 3, 10.0 * (1 - lambda(0)));

#ifdef SICONOS_DEBUG
  std::cout << "NonlinearRelationWithSignInversed::computeJacgx " << " at " << " " << t << std::endl;
  B.display();
  std::cout << std::endl;
#endif

}
int main(int argc, char** argv) {
  double genotype[] = {0, 2, -9, 2, 2, 2, 2,  -9, 1,
                       2, 2, 2,  2, 1, 1, -9, -9, 0};

  SimpleMatrix m;  // marker by sample matrix
  m.resize(3, 6);
  int offset = 0;
  for (int i = 0; i < m.nrow(); ++i) {
    for (int j = 0; j < m.ncol(); ++j) {
      m[i][j] = genotype[offset];
      ++offset;
    }
  }

  std::vector<std::string> iid;
  char buf[128];
  for (int i = 0; i < m.ncol(); ++i) {
    sprintf(buf, "%d", i + 1);
    iid.push_back(buf);
  }

  std::vector<double> pheno;
  pheno.push_back(-9);
  pheno.push_back(-9);
  pheno.push_back(2);
  pheno.push_back(-9);
  pheno.push_back(2);
  pheno.push_back(2);

  PlinkOutputFile pout("testPlinkOutputFile.output");
  pout.writeFAM(iid, iid, pheno);
  pout.writeBIM("1", "snp1", 0, 1, "G", "A");
  pout.writeBIM("1", "snp2", 0, 2, "1", "2");
  pout.writeBIM("1", "snp3", 0, 3, "A", "C");
  pout.writeBED(&m, m.ncol(), m.nrow());

  fprintf(stderr, "Done\n");

  return 0;
}
Exemple #29
0
/** default function to compute jacobianG according to lambda
 *  \param double : current time
 *  \param index for jacobian: at the time only one possible jacobian => i = 0 is the default value .
 */
void adjointInput::computeJacgx(double t, SiconosVector& x, SiconosVector& lambda, SiconosVector& z, SimpleMatrix& K)
{

#ifdef SICONOS_DEBUG
  std::cout << "computeJacgx " << " at " << " " << t << std::endl;
#endif


  SP::SiconosMatrix jacbetaXtmp(new SimpleMatrix(2, 2));

  JacobianXbeta(t, x, jacbetaXtmp);

  K(0, 0) = jacbetaXtmp->getValue(0, 0) * (lambda(0) - 1.0) ;
  K(0, 1) = jacbetaXtmp->getValue(0, 1) * (lambda(0) - 1.0)  ;
  K(0, 2) = 0.0;
  K(0, 3) = 0.0;
  K(1, 0) = jacbetaXtmp->getValue(1, 0) * (lambda(0) - 1.0) ;
  K(1, 1) = jacbetaXtmp->getValue(1, 1) * (lambda(0) - 1.0)  ;
  K(1, 2) = 0.0  ;
  K(1, 3) = 0.0 ;
  K(2, 0) = 0.0;
  K(2, 1) = 0.0;
  K(2, 2) = K2->getValue(0, 0) * (lambda(0) - 1.0);
  K(2, 3) = K2->getValue(0, 1) * (lambda(0) - 1.0);
  K(3, 0) = 0.0;
  K(3, 1) = 0.0;
  K(3, 2) = K2->getValue(1, 0) * (lambda(0) - 1.0);
  K(3, 3) = K2->getValue(1, 1) * (lambda(0) - 1.0);

#ifdef SICONOS_DEBUG
  std::cout << "modif Jacgx : \n";
  K.display();
#endif


}
Exemple #30
0
/**
 * Load covariate from @param fn, using specified @param covNameToUse, for
 * given
 * @param includedSample
 * covariate will be stored in @param covariate, and column names will be
 * stored
 * in @colNames
 * if covariate file missed some samples, those sample names will be stored in
 * @sampleToDrop
 * NOTE: for missing values in a covariate, it will drop this covariate out of
 * the following anaylysis
 * @return number of samples have covariates.
 * Example:
 * includedSample = [A, B, C] and in covaraite file we have [B, C, C, D]
 * then output covariate have 3 rows corresponding to [A, B, C]
 * row C filled by the last C in covariate file
 * sample D will be in sampleToDrop
 */
int _loadCovariate(const std::string& fn,
                   const std::vector<std::string>& includedSample,
                   const std::vector<std::string>& covNameToUse,
                   DataLoader::HandleMissingCov handleMissingCov,
                   SimpleMatrix* covariate, std::vector<std::string>* colNames,
                   std::set<std::string>* sampleToDrop) {
  // load covariate
  SimpleMatrix mat;
  int ret = extractCovariate(fn, includedSample, covNameToUse, handleMissingCov,
                             &mat, sampleToDrop);
  if (ret < 0) {
    return -1;
  }

  // create covariate sample index
  // const int nr = mat.nrow();
  const int nc = mat.ncol();

  std::map<std::string, int> covIndex;
  makeMap(mat.getRowName(), &covIndex);
  int idx = 0;
  for (size_t i = 0; i < includedSample.size(); ++i) {
    if (covIndex.find(includedSample[i]) == covIndex.end()) {
      sampleToDrop->insert(includedSample[i]);
      continue;
    }
    const int match = covIndex[includedSample[i]];
    covariate->resize(idx + 1, nc);
    for (int j = 0; j < mat.ncol(); ++j) {
      (*covariate)[idx][j] = mat[match][j];
      // skip row label, as MathMatrix class does not have row label
    }
    ++idx;
  }
  // set col label
  for (int i = 0; i < mat.ncol(); ++i) {
    // (*covariate).SetColumnLabel(i, mat.getColName()[i].c_str());
    (*covariate).setColName(i, mat.getColName()[i]);
  }
  return 0;
}  // end _loadCovariate