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;
}
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);
}
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);
}
/**
* 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;
}
/** 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
}
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());
}
}
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
}
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;
}
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;
}
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;
}
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
}
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;
}
/** 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
}
/**
* 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
