inline UMatrix2D<T>& UMatrix2D<T>::operator = (UMatrix2D<T>& M)
{
#ifdef _SAFE_ACCESS_
CheckLocker cl1(GetLocker());
CheckLocker cl2(M.GetLocker());
#endif //_SAFE_ACCESS_
nX = M.GetX();
nY = M.GetY();
if(mt == MXT_MEM) {
if(Ptr != NULL)
#ifdef __ICC
_mm_free(Ptr);
#else
free(Ptr);
#endif //__ICC
#ifdef __ICC
Ptr = (T*)(_mm_malloc(sizeof(T)*nX*nY,_ALIGN));
#else
Ptr = (T*)(malloc(sizeof(T)*nX*nY));
#endif
memcpy(Ptr,M.GetMatrixPtr(),sizeof(T)*nX*nY);
} else {
Ptr = M.GetMatrixPtr();
}
ms = M.GetMatrixState();
return *this;
}
void PathPlanner::fill_g_f(cell cell_from){
int i,j;
double currentGval;
bool in_open_list = false;
_close_list.insert(cell_from);
for(i=cell_from.y_Coordinate-1; i <=cell_from.y_Coordinate+1; i++)
{
for(j=cell_from.x_Coordinate-1; j <= cell_from.x_Coordinate+1; j++)
{
if((i>=0)&&(j>=0)&&(i<_grid.size())&&(j<_grid[0].size()))
{
if (i == _goal.y_Coordinate && j == _goal.x_Coordinate )
{
cell cl2(j, i);
_grid[i][j].parent.x_Coordinate = cell_from.x_Coordinate;
_grid[i][j].parent.y_Coordinate = cell_from.y_Coordinate;
_grid[i][j].g_val = 0;
_grid[i][j].f_val = 0;
_open_list.insert(cl2);
}
else {
bool is_close = check_in_set(_close_list,i,j);
if((_grid[i][j].cell_color == 0)&&(((j != cell_from.x_Coordinate)||(i != cell_from.y_Coordinate)))&&(!is_close))
{
cell cl(j, i);
currentGval = g_cost(cell_from, cl);
in_open_list = check_in_set(_open_list,i,j);
if(in_open_list){
if (_grid[i][j].g_val > currentGval){
_grid[i][j].g_val = currentGval;
_grid[i][j].f_val = _grid[i][j].g_val + _grid[i][j].h_val;
_grid[i][j].parent.x_Coordinate = cell_from.x_Coordinate;
_grid[i][j].parent.y_Coordinate = cell_from.y_Coordinate;
}
}
else{
_grid[i][j].g_val = currentGval;
_grid[i][j].f_val = _grid[i][j].g_val + _grid[i][j].h_val;
_grid[i][j].parent.x_Coordinate = cell_from.x_Coordinate;
_grid[i][j].parent.y_Coordinate = cell_from.y_Coordinate;
_open_list.insert(cl);
}
}
}
}
}
}
}
int main()
{
Client cl;
TestA ta;
Client2 cl2(&ta);
TestB tb;
TestAA taa;
cl2.SetMult(&ta);
std::cout << "Client use TestA " << cl2.Work(1,2) << std::endl;
cl2.SetMult(&taa);
std::cout << "Client use TestB " << cl2.Work(1,2) << std::endl;
return 0;
}
/*! \brief Test cell structure
*
* \tparam CellS
*
*/
template<unsigned int dim, typename T, typename CellS> void Test_cell_s(SpaceBox<dim,T> & box)
{
//! [Declare a cell list]
//Space where is living the Cell list
//SpaceBox<dim,T> box({0.0f,0.0f,0.0f},{1.0f,1.0f,1.0f});
// Subdivisions
size_t div[dim] = {16,16,16};
// Origin
Point<dim,T> org({0.0,0.0,0.0});
// id Cell list
CellS cl2(box,div);
//! [Declare a cell list]
// grid info
grid_sm<dim,void> g_info(div);
// Test force reallocation in case of Cell list fast
for (size_t i = 0 ; i < CELL_REALLOC * 3 ; i++)
{
cl2.add(org,i);
}
// Check the elements
BOOST_REQUIRE_EQUAL(cl2.getNelements(cl2.getCell(org)),CELL_REALLOC * 3ul);
for (size_t i = 0 ; i < CELL_REALLOC * 3 ; i++)
{
BOOST_REQUIRE_EQUAL(cl2.get(cl2.getCell(org),i),i);
}
//! [Usage of cell list]
// id Cell list
CellS cl1(box,div);
// Create a grid iterator
grid_key_dx_iterator<dim> g_it(g_info);
// Iterate through each element
// Add 1 element for each cell
// Usefull definition of points
Point<dim,T> end = box.getP2() - box.getP1();
Point<dim,T> middle = end / div / 2.0;
Point<dim,T> spacing = end / div;
Point<dim,T> offset[dim] = {middle,middle,middle};
// Create offset shift vectors
for (size_t i = 0 ; i < dim ; i++)
{
offset[i].get(i) += (1.0 / div[i]) / 8.0;
}
openfpm::vector<Point<dim,T>> pos;
size_t id = 0;
while (g_it.isNext())
{
// Add 2 particles on each cell
Point<dim,T> key = Point<dim,T>(g_it.get().toPoint());
key = pmul(key,spacing) + offset[0] + box.getP1();
pos.add(key);
cl1.add(key,id);
++id;
key = Point<dim,T>(g_it.get().toPoint());
key = pmul(key,spacing) + offset[1] + box.getP1();
pos.add(key);
cl1.add(key,id);
++id;
++g_it;
}
//! [Usage of cell list]
// check the cell are correctly filled
// reset iterator
g_it.reset();
while (g_it.isNext())
{
// Check that there are 2 particles on each cell
Point<dim,T> key = Point<dim,T>(g_it.get().toPoint());
key = pmul(key,spacing) + offset[2] + box.getP1();
size_t cell = cl1.getCell(key);
size_t n_ele = cl1.getNelements(cell);
BOOST_REQUIRE_EQUAL(n_ele,2ul);
BOOST_REQUIRE_EQUAL((long int)(cl1.get(cell,1) - cl1.get(cell,0)),1);
++g_it;
}
// reset itarator
g_it.reset();
//! [remove one particle from each cell]
while (g_it.isNext())
{
// remove 1 particle on each cell
Point<dim,T> key = Point<dim,T>(g_it.get().toPoint());
key = pmul(key,spacing) + offset[0] + box.getP1();
auto cell = cl1.getCell(key);
// Remove the first particle in the cell
cl1.remove(cell,0);
++g_it;
}
//! [remove one particle from each cell]
// Check we have 1 object per cell
g_it.reset();
while (g_it.isNext())
{
// remove 1 particle on each cell
Point<dim,T> key = Point<dim,T>(g_it.get().toPoint());
key = pmul(key,spacing) + offset[0] + box.getP1();
auto cell = cl1.getCell(key);
size_t n_ele = cl1.getNelements(cell);
BOOST_REQUIRE_EQUAL(n_ele,1ul);
++g_it;
}
// Check we have 1 object per cell
// Create a grid iterator
grid_key_dx<dim> p1(1,1,1);
grid_key_dx<dim> p2(div[0]-2,div[1]-2,div[2]-2);
grid_key_dx_iterator_sub<dim> g_it_s(g_info,p1,p2);
id = 0;
while (g_it_s.isNext())
{
// remove 1 particle on each cell
//! [Usage of the neighborhood iterator]
Point<dim,T> key = Point<dim,T>(g_it_s.get().toPoint());
key = pmul(key,spacing) + offset[0] + box.getP1();
auto NN = cl1.template getNNIterator<NO_CHECK>(cl1.getCell(key));
size_t total = 0;
while(NN.isNext())
{
// total
total++;
++NN;
}
//! [Usage of the neighborhood iterator]
BOOST_REQUIRE_EQUAL(total,(size_t)openfpm::math::pow(3,dim));
id = cl1.get(cl1.getCell(key),0);
auto NNSym = cl1.template getNNIteratorSym<NO_CHECK>(cl1.getCell(key),id,pos);
total = 0;
while(NNSym.isNext())
{
// total
total++;
++NNSym;
}
BOOST_REQUIRE_EQUAL(total,(size_t)openfpm::math::pow(3,dim) / 2 + 1);
++g_it_s;
}
}
void TestBandMatrixArith_D1()
{
std::vector<tmv::BandMatrixView<T> > b;
std::vector<tmv::BandMatrixView<std::complex<T> > > cb;
MakeBandList(b,cb);
const int N = b[0].rowsize();
tmv::Matrix<T> a1(N,N);
for (int i=0; i<N; ++i) for (int j=0; j<N; ++j) a1(i,j) = T(3+i-5*j);
tmv::Matrix<std::complex<T> > ca1(N,N);
for (int i=0; i<N; ++i) for (int j=0; j<N; ++j)
ca1(i,j) = std::complex<T>(3+i-5*j,4-8*i-j);
tmv::UpperTriMatrix<T,tmv::NonUnitDiag|tmv::RowMajor> u1(a1);
tmv::UpperTriMatrix<std::complex<T>,tmv::NonUnitDiag|tmv::RowMajor> cu1(ca1);
tmv::UpperTriMatrixView<T> u1v = u1.view();
tmv::UpperTriMatrixView<std::complex<T> > cu1v = cu1.view();
tmv::UpperTriMatrix<T,tmv::NonUnitDiag> u1x = u1v;
tmv::UpperTriMatrix<std::complex<T>,tmv::NonUnitDiag> cu1x = cu1v;
#if (XTEST & 2)
tmv::UpperTriMatrix<T,tmv::UnitDiag|tmv::RowMajor> u2(a1);
tmv::UpperTriMatrix<T,tmv::NonUnitDiag|tmv::ColMajor> u3(a1);
tmv::UpperTriMatrix<T,tmv::UnitDiag|tmv::ColMajor> u4(a1);
tmv::LowerTriMatrix<T,tmv::NonUnitDiag|tmv::RowMajor> l1(a1);
tmv::LowerTriMatrix<T,tmv::UnitDiag|tmv::RowMajor> l2(a1);
tmv::LowerTriMatrix<T,tmv::NonUnitDiag|tmv::ColMajor> l3(a1);
tmv::LowerTriMatrix<T,tmv::UnitDiag|tmv::ColMajor> l4(a1);
tmv::UpperTriMatrix<std::complex<T>,tmv::UnitDiag|tmv::RowMajor> cu2(ca1);
tmv::UpperTriMatrix<std::complex<T>,tmv::NonUnitDiag|tmv::ColMajor> cu3(ca1);
tmv::UpperTriMatrix<std::complex<T>,tmv::UnitDiag|tmv::ColMajor> cu4(ca1);
tmv::LowerTriMatrix<std::complex<T>,tmv::NonUnitDiag|tmv::RowMajor> cl1(ca1);
tmv::LowerTriMatrix<std::complex<T>,tmv::UnitDiag|tmv::RowMajor> cl2(ca1);
tmv::LowerTriMatrix<std::complex<T>,tmv::NonUnitDiag|tmv::ColMajor> cl3(ca1);
tmv::LowerTriMatrix<std::complex<T>,tmv::UnitDiag|tmv::ColMajor> cl4(ca1);
tmv::UpperTriMatrixView<T> u2v = u2.view();
tmv::UpperTriMatrixView<T> u3v = u3.view();
tmv::UpperTriMatrixView<T> u4v = u4.view();
tmv::LowerTriMatrixView<T> l1v = l1.view();
tmv::LowerTriMatrixView<T> l2v = l2.view();
tmv::LowerTriMatrixView<T> l3v = l3.view();
tmv::LowerTriMatrixView<T> l4v = l4.view();
tmv::UpperTriMatrixView<std::complex<T> > cu2v = cu2.view();
tmv::UpperTriMatrixView<std::complex<T> > cu3v = cu3.view();
tmv::UpperTriMatrixView<std::complex<T> > cu4v = cu4.view();
tmv::LowerTriMatrixView<std::complex<T> > cl1v = cl1.view();
tmv::LowerTriMatrixView<std::complex<T> > cl2v = cl2.view();
tmv::LowerTriMatrixView<std::complex<T> > cl3v = cl3.view();
tmv::LowerTriMatrixView<std::complex<T> > cl4v = cl4.view();
#endif
for(size_t i=START;i<b.size();i++) {
if (showstartdone) {
std::cerr<<"Start loop "<<i<<std::endl;
std::cerr<<"bi = "<<b[i]<<std::endl;
}
tmv::BandMatrixView<T> bi = b[i];
tmv::BandMatrixView<std::complex<T> > cbi = cb[i];
TestMatrixArith4(bi,cbi,u1v,cu1v,"Band/UpperTri");
TestMatrixArith5(bi,cbi,u1v,cu1v,"Band/UpperTri");
TestMatrixArith6x(bi,cbi,u1v,cu1v,"Band/UpperTri");
#if (XTEST & 2)
TestMatrixArith4(bi,cbi,l1v,cl1v,"Band/LowerTri");
TestMatrixArith5(bi,cbi,l1v,cl1v,"Band/LowerTri");
TestMatrixArith6x(bi,cbi,l1v,cl1v,"Band/LowerTri");
TestMatrixArith4(bi,cbi,u2v,cu2v,"Band/UpperTri");
TestMatrixArith5(bi,cbi,u2v,cu2v,"Band/UpperTri");
TestMatrixArith6x(bi,cbi,u2v,cu2v,"Band/UpperTri");
TestMatrixArith4(bi,cbi,l2v,cl2v,"Band/LowerTri");
TestMatrixArith5(bi,cbi,l2v,cl2v,"Band/LowerTri");
TestMatrixArith6x(bi,cbi,l2v,cl2v,"Band/LowerTri");
TestMatrixArith4(bi,cbi,u3v,cu3v,"Band/UpperTri");
TestMatrixArith5(bi,cbi,u3v,cu3v,"Band/UpperTri");
TestMatrixArith6x(bi,cbi,u3v,cu3v,"Band/UpperTri");
TestMatrixArith4(bi,cbi,l3v,cl3v,"Band/LowerTri");
TestMatrixArith5(bi,cbi,l3v,cl3v,"Band/LowerTri");
TestMatrixArith6x(bi,cbi,l3v,cl3v,"Band/LowerTri");
TestMatrixArith4(bi,cbi,u4v,cu4v,"Band/UpperTri");
TestMatrixArith5(bi,cbi,u4v,cu4v,"Band/UpperTri");
TestMatrixArith6x(bi,cbi,u4v,cu4v,"Band/UpperTri");
TestMatrixArith4(bi,cbi,l4v,cl4v,"Band/LowerTri");
TestMatrixArith5(bi,cbi,l4v,cl4v,"Band/LowerTri");
TestMatrixArith6x(bi,cbi,l4v,cl4v,"Band/LowerTri");
#endif
}
}
