Example #1
0
void igl::sort_triangles(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedFF> & FF,
  Eigen::PlainObjectBase<DerivedI> & I)
{
  using namespace Eigen;
  using namespace igl;
  using namespace std;
  // Put model, projection, and viewport matrices into double arrays
  Matrix4d MV;
  Matrix4d P;
  glGetDoublev(GL_MODELVIEW_MATRIX,  MV.data());
  glGetDoublev(GL_PROJECTION_MATRIX, P.data());
  if(V.cols() == 3)
  {
    Matrix<typename DerivedV::Scalar, DerivedV::RowsAtCompileTime,4> hV;
    hV.resize(V.rows(),4);
    hV.block(0,0,V.rows(),V.cols()) = V;
    hV.col(3).setConstant(1);
    return sort_triangles(hV,F,MV,P,FF,I);
  }else
  {
    return sort_triangles(V,F,MV,P,FF,I);
  }
}
void
dmz::WeaponPluginGravityBullet::_store_speed (
      const Handle ObjectHandle,
      const ObjectType &Type) {

   Float64 *ptr (_speedTable.lookup (Type.get_handle ()));

   if (!ptr) {

      ptr = new Float64 (_defaultSpeed);

      if (ptr && !_speedTable.store (Type.get_handle (), ptr)) {

         delete ptr; ptr = 0;
      }
   }

   if (ptr) {

      _objectTable.store (ObjectHandle, ptr);

      ObjectModule *objMod (get_object_module ());

      if (objMod) {

         Matrix ori;
         objMod->lookup_orientation (ObjectHandle, _defaultHandle, ori);
         Vector vel (0.0, 0.0, -(*ptr));
         ori.transform_vector (vel);
         objMod->store_velocity (ObjectHandle, _defaultHandle, vel);
      }
   }
}
Example #3
0
bool
SVGLineElement::GetGeometryBounds(
  Rect* aBounds, const StrokeOptions& aStrokeOptions, const Matrix& aTransform)
{
  float x1, y1, x2, y2;
  GetAnimatedLengthValues(&x1, &y1, &x2, &y2, nullptr);

  if (aStrokeOptions.mLineWidth <= 0) {
    *aBounds = Rect(aTransform * Point(x1, y1), Size());
    aBounds->ExpandToEnclose(aTransform * Point(x2, y2));
    return true;
  }

  if (aStrokeOptions.mLineCap == CapStyle::ROUND) {
    if (!aTransform.IsRectilinear()) {
      // TODO: handle this case.
      return false;
    }
    Rect bounds(Point(x1, y1), Size());
    bounds.ExpandToEnclose(Point(x2, y2));
    bounds.Inflate(aStrokeOptions.mLineWidth / 2.f);
    *aBounds = aTransform.TransformBounds(bounds);
    return true;
  }

  Float length = Float(NS_hypot(x2 - x1, y2 - y1));
  Float xDelta;
  Float yDelta;

  if (aStrokeOptions.mLineCap == CapStyle::BUTT) {
    if (length == 0.f) {
      xDelta = yDelta = 0.f;
    } else {
      Float ratio = aStrokeOptions.mLineWidth / 2.f / length;
      xDelta = ratio * (y2 - y1);
      yDelta = ratio * (x2 - x1);
    }
  } else {
    MOZ_ASSERT(aStrokeOptions.mLineCap == CapStyle::SQUARE);
    if (length == 0.f) {
      xDelta = yDelta = aStrokeOptions.mLineWidth / 2.f;
    } else {
      Float ratio = aStrokeOptions.mLineWidth / 2.f / length;
      xDelta = yDelta = ratio * (fabs(y2 - y1) + fabs(x2 - x1));
    }
  }

  Point points[4];

  points[0] = Point(x1 - xDelta, y1 - yDelta);
  points[1] = Point(x1 + xDelta, y1 + yDelta);
  points[2] = Point(x2 + xDelta, y2 + yDelta);
  points[3] = Point(x2 - xDelta, y2 - yDelta);

  *aBounds = Rect(aTransform * points[0], Size());
  for (uint32_t i = 1; i < 4; ++i) {
    aBounds->ExpandToEnclose(aTransform * points[i]);
  }
  return true;
}
//---------------------------------------------------------------------------
Matrix invert(const Matrix& A)
{
  Matrix y(A.numRows(), A.numRows(), false);

  // Make sure A is square!
  if (A.numRows() != A.numCols())
  {
    std::cerr << "ERROR!  A must be square matrix!\n";
    return y;
  }

  const int N = A.numRows();

  // Make a copy of A since it gets modified
  Matrix Acopy(A);

 // y.setSize(N,N);
  Matrix col(N,1);
  Matrix indx(N,1);
  double d;
  int i,j;

  ludcmp(Acopy,indx,d);
  for (j = 0; j < N; j++)
  {
    for (i = 0; i < N; i++) col(i,0) = 0.0;
    col(j,0) = 1.0;
    lubksb(Acopy,indx,col);
    for (i = 0; i < N; i++) y(i,j) = col(i,0);
  }

  std::cout.flush();
  // Return result
  return y;
}
Example #5
0
void  SegmentorOTSU::NdGrid (const VectorDouble&  x,
                             const VectorDouble&  y,
                             Matrix&              xm,
                             Matrix&              ym
                            )
{
  kkuint32  xLen = (kkuint32)x.size ();
  kkuint32  yLen = (kkuint32)y.size ();

  xm.ReSize (xLen, xLen);
  ym.ReSize (yLen, yLen);

  kkuint32  row, col;

  for  (row = 0;  row < xLen;  ++row)
  {
    for  (col = 0;  col < yLen;  ++col)
      xm[row][col] = x[row];
  }

  for  (row = 0;  row < xLen;  ++row)
  {
    for  (col = 0;  col < yLen;  ++col)
      ym[row][col] = y[col];
  }
}
  double BLSSS::log_model_prob(const Selector &g)const{
    // borrowed from MLVS.cpp
    double num = vpri_->logp(g);
    if(num==BOOM::negative_infinity() || g.nvars() == 0) {
      // If num == -infinity then it is in a zero support point in the
      // prior.  If g.nvars()==0 then all coefficients are zero
      // because of the point mass.  The only entries remaining in the
      // likelihood are sums of squares of y[i] that are independent
      // of g.  They need to be omitted here because they are omitted
      // in the non-empty case below.
      return num;
    }
    SpdMatrix ivar = g.select(pri_->siginv());
    num += .5*ivar.logdet();
    if(num == BOOM::negative_infinity()) return num;

    Vector mu = g.select(pri_->mu());
    Vector ivar_mu = ivar * mu;
    num -= .5*mu.dot(ivar_mu);

    bool ok=true;
    ivar += g.select(suf().xtx());
    Matrix L = ivar.chol(ok);
    if(!ok)  return BOOM::negative_infinity();
    double denom = sum(log(L.diag()));  // = .5 log |ivar|
    Vector S = g.select(suf().xty()) + ivar_mu;
    Lsolve_inplace(L,S);
    denom-= .5*S.normsq();  // S.normsq =  beta_tilde ^T V_tilde beta_tilde
    return num-denom;
  }
Example #7
0
/*
Saves matrix to file

@param filename filename for the txt output
@param mat matrix to be outputted
*/
void	saveMatrix(const std::string filename, const Matrix& mat){
	std::ofstream	fOut(filename);
	if (!fOut) {
		std::cout << "Error opening file: " << filename << std::endl;
		exit(EXIT_FAILURE);
	}

	if(!(fOut << mat.getDimM() << " " << mat.getDimN() << std::endl))
	{
		std::cout << "Error in writing matrix entries!" << std::endl;
		exit(EXIT_FAILURE);
	}

	for (int m = 0; m < mat.getDimM(); m++) {
		for (int n = 0; n < mat.getDimN(); n++) {
			if(!(fOut << mat(m, n) << std::endl))
			{
				std::cout << "Error in writing matrix entries!" << std::endl;
				exit(EXIT_FAILURE);
			}
		}
	}

	fOut.close();
}
Example #8
0
int main(int argc,char* argv[])
{
	Matrix mat(2,5);
	mat(1,0) = 2;
	mat(0,4) = 1;
	std::set<size_t> r;
	std::set<size_t> c;
	r.insert(0);
	// r.insert(1);
	c.insert(0);
	c.insert(4);
	Matrix m;
	m.blockFromMatrix(mat,r.begin(),r.end(),c.begin(),c.end());
	std::cout<<"blocking test one:"<<std::endl<<m<<std::endl;
	m.rowBlockFromMatrix(mat,r.begin(),r.end());
	std::cout<<"blocking test two:"<<std::endl<<m<<std::endl;
	m.columnBlockFromMatrix(mat,c.begin(),c.end());
	std::cout<<"blocking test three :"<<std::endl<<m<<std::endl;

	Vector vec(5);
	vec[4] = 2.0;
	Vector v;
	v.blockFromVector(vec,c);
	std::cout<<vec.block(c)<<std::endl;
	std::cout<<v<<std::endl; 
}
Example #9
0
//C = A^trA B^trB
int MatMul(Matrix &C, int trA, Matrix &A, int trB, Matrix &B) {

    CBLAS_TRANSPOSE_t TransA = ((trA == 0) ? CblasNoTrans : CblasTrans);
    CBLAS_TRANSPOSE_t TransB = ((trB == 0) ? CblasNoTrans : CblasTrans);

    return gsl_blas_dgemm( TransA, TransB, 1.0, A.Ma(), B.Ma(), 0.0, C.Ma());
}
Example #10
0
Matrix* Matrix::transpose(){
	Matrix *rezult = new Matrix(size);
	for (int i = 0; i < size; i++)
	for (int j = 0; j < size; j++)
		rezult->setElement(i, j, matrix[j][i]);
	return rezult;
}
Example #11
0
const Matrix& ActuatorCorot::getInitialStiff(void)
{
    // zero the matrix
    theMatrix->Zero();
    
    // local stiffness matrix
    static Matrix kl(3,3);
    
    // material stiffness portion
    kl.Zero();
    kl(0,0) = EA/L;
    
    // compute R'*kl*R
    static Matrix kg(3,3);
    kg.addMatrixTripleProduct(0.0, R, kl, 1.0);
    
    // copy stiffness into appropriate blocks in element stiffness
    int numDOF2 = numDOF/2;
    for (int i=0; i<numDIM; i++)  {
        for (int j=0; j<numDIM; j++)  {
            (*theMatrix)(i,j) = kg(i,j);
            (*theMatrix)(i,j+numDOF2) = -kg(i,j);
            (*theMatrix)(i+numDOF2,j) = -kg(i,j);
            (*theMatrix)(i+numDOF2,j+numDOF2) = kg(i,j);
        }
    }
    
    return *theMatrix;
}
Example #12
0
Matrix* Matrix::operator+(Matrix &matr){
	Matrix *rezult = new Matrix(size);
	for (int i = 0; i < size; i++)
	for (int j = 0; j < size; j++)
		rezult->setElement(i, j, matrix[i][j] + matr.getElement(i, j));
	return rezult;
}
Example #13
0
Matrix 
Assignment::RandomGenerate(size_t nrows, size_t ncols, int MAX, unsigned int _seed){
 
  //accept new seed for random generator
  if(_seed != SEED)
    srand(_seed);
  else 
    srand(this->seed);
  
  //update data members
  this->num_agents = nrows;
  this->num_tasks = ncols;

  //define a matrix
  Matrix matrix;
  matrix.resize(nrows);
  for(unsigned int i=0; i<nrows; i++)
    matrix[i].resize(ncols);

  //randomly generate
  for(unsigned int i=0; i<nrows; i++)
    for(unsigned int j=0; j<ncols; j++){
      int rdm=rand()%MAX-1; 
      if(rdm<0) rdm = 0;
      matrix[i][j].SetWeight(rdm);
  }      
  return matrix;
}
Example #14
0
void
DrawTargetSkia::Mask(const Pattern &aSource,
                     const Pattern &aMask,
                     const DrawOptions &aOptions)
{
    MarkChanged();
    AutoPaintSetup paint(mCanvas.get(), aOptions, aSource);

    SkPaint maskPaint;
    SetPaintPattern(maskPaint, aMask);

    SkLayerRasterizer *raster = new SkLayerRasterizer();
    raster->addLayer(maskPaint);
    SkSafeUnref(paint.mPaint.setRasterizer(raster));

    // Skia only uses the mask rasterizer when we are drawing a path/rect.
    // Take our destination bounds and convert them into user space to use
    // as the path to draw.
    SkPath path;
    path.addRect(SkRect::MakeWH(SkScalar(mSize.width), SkScalar(mSize.height)));

    Matrix temp = mTransform;
    temp.Invert();
    SkMatrix mat;
    GfxMatrixToSkiaMatrix(temp, mat);
    path.transform(mat);

    mCanvas->drawPath(path, paint.mPaint);
}
Example #15
0
void CRSMatrix::full2CRS(Matrix &A){
	int nz = 0;     //count non-zero elements
	int idx = 0;    //data index

	/* *** count number of non-zero entries */
	for(int i=0;i<A.numRows();i++)
    for(int j=0;j<A.numCols();j++)
        if(A(i,j)!=0)
            nz++;
	/* *** initialize private variables */
	_rows = A.numRows();
	_cols = A.numCols();
	_nonZeros = nz;
	delete[] _data;
	delete []_colindex;
	delete[]_rowptr;
	_data     = new double[nz];
	_colindex = new int[nz];
	_rowptr   = new int[_rows+1];

	/* *** fill _rowptr,_colindex,_data */
	for(int i=0;i<_rows;i++){
    _rowptr[i] = idx;
    for(int j=0; j<_cols;j++){
        if(A(i,j)!=0){
            _data[idx] =  A(i,j);
            _colindex[idx]= j;
            idx++;
        }
    }
	}
	_rowptr[_rows]= nz;
}
void LinearSolver_LU::solve_(Matrix* A, Matrix* B, LinearSolverRecord *record,
                             int incrImprov, bool preserveMatrix) 
{
    const int nbRows = B->getNbRows(),
              nbCols = B->getNbCols();
    Matrix *X = record->sol;
    int* p = new int[nbRows];
    double* v = new double[nbRows];
    
    Matrix *LU;
    if (preserveMatrix)
        LU = new Matrix(A);
    else
        LU = A;
        
    double* const*const lu = LU->getMatrix(R_W_ACCESS);
    double* const*const b = B->getMatrix(R_W_ACCESS);
    double* const*const x = X->getMatrix(R_W_ACCESS);

    record->status = factorize_(nbRows, lu, p, v);

    forwardSubstitution_(nbRows, nbCols, lu, b, p, x);
    backSubstitution_(nbRows, nbCols, lu, x, p, x);

    delete []p;
    delete []v;
    if (preserveMatrix)
        delete LU;
        
}
bool SRUKF::measurementUpdate(const Matrix& measurement, const Matrix& measurementNoise, const Matrix& predictedMeasurementSigmas, const Matrix& stateEstimateSigmas)
{

    int numberOfSigmaPoints = stateEstimateSigmas.getn();
    debug << "Predicted measurement sigmas:" << std::endl << predictedMeasurementSigmas;

    Matrix predictedMeasurement = CalculateMeanFromSigmas(predictedMeasurementSigmas);

    Matrix Mz(m_numStates,numberOfSigmaPoints, false);
    Matrix Mx(m_numStates,numberOfSigmaPoints, false);

    for(int i = 0; i < numberOfSigmaPoints; i++)
    {
        Mz.setCol(i, m_sqrtSigmaWeights[0][i] * (predictedMeasurementSigmas.getCol(i) - predictedMeasurement));
        Mx.setCol(i, m_sqrtSigmaWeights[0][i] * (stateEstimateSigmas.getCol(i) - m_mean));
    }

    Matrix Sz = horzcat(Mz,measurementNoise);
    Matrix Pxz = Mx*Mz.transp();
    Matrix K = Pxz * Invert22(Sz*Sz.transp());

    debug << "K:" << std::endl << K;
    m_mean  = m_mean + K * (measurement - predictedMeasurement);

    m_sqrtCovariance = HT(horzcat(Mx-K*Mz,K*measurementNoise));
    //m_covariance = HT(horzcat(sigmaPoints-m_mean*m_sigmaWeights - K*predictedObservationSigmas +
    //                          K*predictedObservation*m_sigmaWeights,K*measurementNoise));
    return true;
}
Example #18
0
void CDiamond::Draw(CDC* pDC,CRect& rcInvalid)
{
	if(m_nPtCount <= 0)
		return;

	Pen penDraw(m_crColor,(float)m_nWidth);
	penDraw.SetLineJoin(LineJoinRound);
	penDraw.SetDashStyle(m_nDashStyle);

	Graphics graph(pDC->m_hDC);
	graph.SetSmoothingMode(SmoothingModeAntiAlias);

	Matrix mat;
	mat.RotateAt(m_fAngle,m_ptCenter);
	graph.SetTransform(&mat);

	if(m_nPtCount <= 1)
	{
		return;
	}
	else 
	{
		if(m_bFinish && (m_bSelected || m_bEdit))
		{
			DrawHotShape(graph);
		}

		graph.ExcludeClip(&m_RgnErase);
		PointF* pPt = m_ptary.GetData();
		graph.DrawPolygon(&penDraw,pPt,4);
	}
}
Example #19
0
File: lu.hpp Project: GWTWFN/vsipl
  bool solve(const_Matrix<T, Block0> b, Matrix<T, Block1> x)
  {
    typedef typename Block_layout<Block0>::order_type order_type;
    typedef typename Block_layout<Block0>::complex_type complex_type;
    typedef Layout<2, order_type, Stride_unit_dense, complex_type> data_LP;
    typedef Strided<2, T, data_LP, Local_map> block_type;

    assert(b.size(0) == length_);
    assert(b.size(0) == x.size(0) && b.size(1) == x.size(1));

    Matrix<T, block_type> b_int(b.size(0), b.size(1));
    assign_local(b_int, b);

    if (tr == mat_conj || 
        (tr == mat_trans && Is_complex<T>::value) ||
        (tr == mat_herm && !Is_complex<T>::value))
      VSIP_IMPL_THROW(unimplemented(
        "LU solver (CVSIP backend) does not implement this transformation"));
    {
      Ext_data<block_type> b_ext(b_int.block());

      cvsip::View<2,T,true>
        cvsip_b_int(b_ext.data(),0,b_ext.stride(0),b_ext.size(0),
                    b_ext.stride(1),b_ext.size(1));

      cvsip_b_int.block().admit(true);
      traits::lu_solve(lu_, tr, cvsip_b_int.ptr());
      cvsip_b_int.block().release(true);
    }
    assign_local(x, b_int);
    return true;
  }
// Inliers:  Detected corners are in green, reprojected ones are in red
// Outliers: Detected corners are in yellow, reprojected ones are in blue
vector<Image> visualizeReprojection(const Image &im1, const Image &im2, Matrix  H, vector<Correspondance> & corr, const vector<bool> & ins) {
  // Initialize colors
  vector<float> red(3,0);
  vector<float> green(3,0);
  vector<float> blue(3,0);
  vector<float> yellow(3,0);
  red[0] = 1.0f;
  green[1]= 1.0f;
  blue[2] = 1.0f;
  yellow[0] = 1.0f;
  yellow[1] = 1.0f;

  vector<Point> detectedPts1In;
  vector<Point> projectedPts1In;
  vector<Point> detectedPts1Out;
  vector<Point> projectedPts1Out;

  vector<Point> detectedPts2In;
  vector<Point> projectedPts2In;
  vector<Point> detectedPts2Out;
  vector<Point> projectedPts2Out;

  for (int i = 0 ; i < (int) corr.size(); i++) {
    Point pt1 = corr[i].feature(0).point();
    Point pt2 = corr[i].feature(1).point();
    Matrix P1 = pt1.toHomogenousCoords();
    Matrix P2 = pt2.toHomogenousCoords();
    Matrix P2_proj = H.multiply(P1);
    Matrix P1_proj = H.inverse().multiply(P2);
    Point reproj1 = Point(P1_proj(0,0)/P1_proj(0,2), P1_proj(0,1)/P1_proj(0,2));
    Point reproj2 = Point(P2_proj(0,0)/P2_proj(0,2), P2_proj(0,1)/P2_proj(0,2));
    if (ins[i]) { // Inlier
      detectedPts1In.push_back(pt1);
      projectedPts1In.push_back(reproj1);
      detectedPts2In.push_back(pt2);
      projectedPts2In.push_back(reproj2);
    } else { // Outlier
      detectedPts1Out.push_back(pt1);
      projectedPts1Out.push_back(reproj1);
      detectedPts2Out.push_back(pt2);
      projectedPts2Out.push_back(reproj2);
    }
  }
  vector<Image> output;
  Image vim1(im1);
  Image vim2(im2);
  vim1 = visualizeCorners(im1, detectedPts1In,2, green);
  vim1 = visualizeCorners(vim1, projectedPts1In,1, red);
  vim1 = visualizeCorners(vim1, detectedPts1Out,2, yellow);
  vim1 = visualizeCorners(vim1, projectedPts1Out,1, blue);

  vim2 = visualizeCorners(im2, detectedPts2In,2, green);
  vim2 = visualizeCorners(vim2, projectedPts2In,1, red);
  vim2 = visualizeCorners(vim2, detectedPts2Out,2, yellow);
  vim2 = visualizeCorners(vim2, projectedPts2Out,1, blue);

  output.push_back(vim1);
  output.push_back(vim2);
  return output;
}
//---------------------------------------------------------------------------
Matrix solveLinearSystem(const Matrix& A, const Matrix& b)
{
  Matrix x;

  // Make sure A is square!
  if (A.numRows() != A.numCols())
  {
    std::cerr << "ERROR!  A must be square matrix!\n";
    return x;
  }

  // Make sure b is a column vector with the same dimensions
  // as A
  if (b.numRows() != A.numRows())
  {
    std::cerr << "ERROR!  b must be a column vector with the same dimensions as square matrix A!\n";
    return x;
  }

  // Make a copy of A since it gets modified
  Matrix Acopy(A);

  const int N = Acopy.numRows();
  Matrix indx(N,1);
  double d;
  ludcmp(Acopy,indx,d);
  x = b;  // x will contain solution
  lubksb(Acopy,indx,x);

  // Return solution column vector
  return x;
}
Example #22
0
ActionBase::ResultE Joint::intersectEnter(Action *action)
{
    // Use parent class for trivial reject
    if(Inherited::intersect(action) == Action::Skip)
        return Action::Skip;
    
    // Need to check children
    IntersectAction *ia = dynamic_cast<IntersectAction *>(action);
    Matrix           m  = this->getMatrix();

    m.mult(this->getJointTransformation());
    m.invert();
    
    Pnt3f pos;
    Vec3f dir;

    m.multFull(ia->getLine().getPosition (), pos);
    m.mult    (ia->getLine().getDirection(), dir);
    
    Real32 length = dir.length();

    if(length < TypeTraits<Real32>::getDefaultEps())
        SWARNING << "Joint::intersectEnter: Near-zero scale!" << std::endl;

    ia->setLine(Line(pos, dir), ia->getMaxDist());
    ia->scale  (length                          );
    
    return ActionBase::Continue; 
}
Example #23
0
int main(int argc, char **argv) {
	if(argc<=1) throw std::runtime_error(TOSTRING("Usage : " << argv[0] << " <file> <indices_subset_file> <outfile>"));
	file = argv[1];

	Matrix X;
	X.read(file);

	std::cout << X.height << "x" << X.width << "\n";

	int nbrows = 0;

	FILE* f = fopen(argv[2], "r");
	while(!feof(f)) {
		if(fgetc(f)=='\n') nbrows++;
	}
	fclose(f);

	Matrix Y(nbrows, X.width);
	std::cout << Y.height << "x" << Y.width << "\n";

	f = fopen(argv[2], "r");
	int i=0;
	while(!feof(f)) {
		int ind; fscanf(f, "%u", &ind);
		ind--;
		memcpy(&Y[X.width*(i++)],&X[X.width*ind],sizeof(float)*X.width);
		if(i==nbrows) break;
	}
	fclose(f);

	Y.write(argv[3]);
	std::cout << "ok\n";
}
Example #24
0
NewActionTypes::ResultE Joint::intersectActorEnter(
    ActorBase::FunctorArgumentType &funcArg)
{
    IntersectActor *pIA        = dynamic_cast<IntersectActor *>(
                                                           funcArg.getActor());
    Matrix          matrix     = this->getMatrix();
    Line            transLine;
    Pnt3f           pos;
    Vec3f           dir;

    matrix.mult(this->getJointTransformation());
    matrix.invert();

    matrix.multFull(pIA->getRay().getPosition (), pos);
    matrix.mult    (pIA->getRay().getDirection(), dir);

    transLine.setValue(pos, dir);

    pIA->beginEditState();
    {
        pIA->setRay        (transLine                           );
        pIA->setScaleFactor(pIA->getScaleFactor() / dir.length());
    }
    pIA->endEditState  ();

    pIA->setupChildrenPriorities();

    return NewActionTypes::Continue;
}
Example #25
0
// virtual
Matrix* NominalToCategorical::filterLabels(Matrix& labels)
{
	if(labels.cols() != 1)
		ThrowError("unexpected number of label dims");
	Matrix* pOut = new Matrix();
	pOut->setSize(0, m_labelVals);
	if(m_labelVals == 1)
	{
		for(size_t i = 0; i < labels.rows(); i++)
		{
			vector<double> row;
			row.push_back(labels[i][0]);
			pOut->copyRow(row);
		}
	}
	else
	{
		for(size_t i = 0; i < labels.rows(); i++)
		{
			vector<double> row;
			row.resize(m_labelVals, 0.0);
			row[(size_t)labels[i][0]] = 1.0;
			pOut->copyRow(row);
		}
	}
	return pOut;
}
Example #26
0
 double fwd_1(Vector &pi, Matrix &P, const Matrix &logQ, const Vector &logd,
              const Vector &one) {
   /*----------------------------------------------------------------------
    * Input: pi[0..S-1] is the conditionl distribution of h[t-1]|Y[t-1]
    *        one[0..S-1] is a vector of 1's
    *        logd[s] is logp(y[t] | model[s])
    *        logQ[0..S-1] is the log of the square transition
    *                     probability matrix (rows of Q sum to 1)
    *
    * Output:  pi[0..S-1]  is prob(h[t] | Y[t])
    *          P[0..S-1]^2 is prob(h[t-1], h[t] | Y[t])
    *
    * Return:  logp(y[t] | y[1]..y[t-1])
    * --------------------------------------------------------------------*/
   uint S = pi.size();
   P = logQ;
   pi = log(pi);
   for (uint r = 0; r < S; ++r) P.row(r) += logd;  // P(r,s) += logd[s]
   for (uint s = 0; s < S; ++s) P.col(s) += pi;    // P(r,s) += pi[r]
   double m = max(P);
   P -= m;
   P.exp();
   double nc = P.abs_norm();
   P /= nc;
   pi = one * P;
   return m + log(nc);
 }
Example #27
0
bool LinearRegression::FitLinearModel(Matrix & X, Vector & y){
    Matrix Xt;
    Xt.Transpose(X);

    Matrix XtX;
    XtX.Product(Xt, X);
    if (!this->chol.TryDecompose(XtX))
        return false;
    chol.Decompose(XtX);
    chol.Invert();
    this->XtXinv = chol.inv;

    Vector tmp = y;
    tmp.Product(Xt, y);
    this->B.Product(this->XtXinv, tmp); // beta = (XtX)^{-1} Xt Y

    this->predict.Product(X, this->B);
    this->residuals = y;
    this->residuals.Subtract(this->predict);

    this->sigma2 = 0.0;
    for (int i = 0; i < this->residuals.Length(); i++){
        sigma2 += (this->residuals[i]) * (this->residuals[i]);
    }
    sigma2 /= y.Length(); // MLE estimates of sigma2

    this->covB = this->XtXinv;
    this->covB.Multiply(sigma2);
    return true;
}; 
Example #28
0
//---------------------------------------------------------------------------
void TEstimation::calculateMarginalDensitiesOfRetained(){
   //ofstream fmfile;
   //fmfile.open("fmfile.txt");
	if(calcObsPValue>mySimData->numUsedSims) calcObsPValue=mySimData->numUsedSims;
	cout << "   - calculating the marginal density for " << calcObsPValue << " used sims to get the p-value of the observed data." << endl;
   fmFromRetained=new float[mySimData->numUsedSims];
   Matrix D = SigmaS+CHat*SigmaTheta*CHat.t();
   Matrix D_inv=D.i();
   ColumnVector theta_j, m_j, simStats;
   for(int s=1; s<=calcObsPValue;++s){
	   simStats=mySimData->statMatrix.column(s);
	   double f_M=0.0;
	   //loop over all dirac peaks
	   for(int d=1; d<=mySimData->numUsedSims;++d){
		   theta_j=mySimData->paramMatrix.submatrix(d,d,2,mySimData->numParams+1).as_column();
		   m_j=c_zero+CHat*theta_j;
		   Matrix temp=-0.5*(simStats-m_j).t()*D_inv*(simStats-m_j);
		   f_M+=exp(temp(1,1));
	   }
	   f_M=f_M/(mySimData->numUsedSims*sqrt((2*3.14159265358979323846*D).determinant()));
	   fmFromRetained[s-1]=f_M*((double)mySimData->numUsedSims/(double)mySimData->numReadSims);
	  // fmfile << fmFromRetained[s-1] << endl;
   }
   //fmfile.close();
}
bool
GyroRatePerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
    Predicate &predicate = predList->AddPredicate();
    predicate.name = "GYR";
    predicate.parameter.Clear();
    
    ParameterList &nameElement = predicate.parameter.AddList();
    nameElement.AddValue(std::string("n"));
    nameElement.AddValue(GetName());       

    Matrix invRot = mBody->GetRotation();
    invRot.InvertRotationMatrix();
    Vector3f rate = invRot * mBody->GetAngularVelocity();

    ParameterList &ratesElement = predicate.parameter.AddList();    
    ratesElement.AddValue(std::string("rt"));
    ratesElement.AddValue(gRadToDeg(rate.x()));
    ratesElement.AddValue(gRadToDeg(rate.y()));
    ratesElement.AddValue(gRadToDeg(rate.z()));

   // What should be done when yrotate is around 90? in that case, the parameters of the atan2 are 0!
//    const dReal* q = dBodyGetQuaternion(mBody->GetODEBody());
//    float xrotate = gArcTan2(2*(q[0]*q[1]+q[2]*q[3]), 1-2*(q[1]*q[1]+q[2]*q[2]));
//    float yrotate = gArcSin(2*(q[0]*q[2] - q[3]*q[1]));
//    float zrotate = gArcTan2(2*(q[0]*q[3] + q[1]*q[2]), 1-2*(q[2]*q[2]+q[3]*q[3]));
//    
//    ParameterList & anglesElement = predicate.parameter.AddList();    
//    anglesElement.AddValue(std::string("ang"));
//    anglesElement.AddValue(gRadToDeg(xrotate));
//    anglesElement.AddValue(gRadToDeg(yrotate));
//    anglesElement.AddValue(gRadToDeg(zrotate));
    
    return true;
}
    void FixedContactEKFFlexEstimatorIMU::setFlexibilityGuess(const Matrix & x)
    {
        bool bstate =ekf_.checkStateVector(x);
        bool b6= (x.rows()==6 && x.cols()==1);
        bool bhomogeneous = (x.rows()==4 && x.cols()==4);

        (void)b6;//avoid warning

        BOOST_ASSERT((bstate||b6||bhomogeneous) &&
                "ERROR: The flexibility state has incorrect size \
                    must be 18x1 vector, 6x1 vector or 4x4 matrix");

        Vector x0=x;

        if (bstate)
        {
            ekf_.setState(x0,k_);
        }
        else
        {
            if (bhomogeneous)
                x0=kine::homogeneousMatrixToVector6(x);

            Vector x_s = ekf_.stateVectorZero();

            x_s.segment(indexes::pos,3)=x0.head(3);

            x_s.segment(indexes::ori,3)=x0.tail(3);

            ekf_.setState(x_s,k_);

            ekf_.setQ(Q_);
        }
    }