object k_fixed_radius_search_array(ANNkd_tree& kdtree, object qarray, double sqRad, int k, double eps)
{
BOOST_ASSERT(k <= kdtree.nPoints());
int N = len(qarray);
if( N == 0 )
return boost::python::make_tuple(numeric::array(boost::python::list()).astype("i4"),numeric::array(boost::python::list()),numeric::array(boost::python::list()));
BOOST_ASSERT(len(qarray[0])==kdtree.theDim());
ANNpointManaged annq(kdtree.theDim());
npy_intp dimsball[] = { N};
PyObject *pykball = PyArray_SimpleNew(1,dimsball, PyArray_INT);
BOOST_ASSERT(!!pykball);
int* pkball = (int*)PyArray_DATA(pykball);
if( k <= 0 ) {
for(int i = 0; i < N; ++i) {
object q = qarray[i];
for (int c = 0; c < kdtree.theDim(); ++c)
annq.pt[c] = extract<ANNcoord>(q[c]);
pkball[i] = kdtree.annkFRSearch(annq.pt, sqRad, k, NULL, NULL, eps);
}
return boost::python::make_tuple(numeric::array(boost::python::list()).astype("i4"),numeric::array(boost::python::list()),static_cast<numeric::array>(handle<>(pykball)));
}
npy_intp dims[] = { N,k};
PyObject *pydists = PyArray_SimpleNew(2,dims, sizeof(ANNdist)==8 ? PyArray_DOUBLE : PyArray_FLOAT);
if( !pydists )
Py_DECREF(pykball);
BOOST_ASSERT(!!pydists);
PyObject *pyidx = PyArray_SimpleNew(2,dims, PyArray_INT);
if( !pyidx ) {
Py_DECREF(pykball);
Py_DECREF(pydists);
}
BOOST_ASSERT(!!pyidx);
ANNdist* pdists = (ANNdist*)PyArray_DATA(pydists);
ANNidx* pidx = (ANNidx*)PyArray_DATA(pyidx);
std::vector<ANNdist> dists(k);
std::vector<ANNidx> nn_idx(k);
for(int i = 0; i < N; ++i) {
object q = qarray[i];
for (int c = 0; c < kdtree.theDim(); ++c)
annq.pt[c] = extract<ANNcoord>(q[c]);
pkball[i] = kdtree.annkFRSearch(annq.pt, sqRad, k, &nn_idx[0], &dists[0], eps);
std::copy(nn_idx.begin(),nn_idx.end(),pidx);
pidx += k;
std::copy(dists.begin(),dists.end(),pdists);
pdists += k;
}
return boost::python::make_tuple(static_cast<numeric::array>(handle<>(pyidx)), static_cast<numeric::array>(handle<>(pydists)),static_cast<numeric::array>(handle<>(pykball)));
}
//*****************************************************
// Function: computeMI_ANN
//*****************************************************
double computeMI_ANN( ANNpointArray dataXY,
unsigned int dimX, unsigned int dimY,
unsigned int k, unsigned int N, double eps )
{
ANNpointArray dataX, dataY;
double* distsXY;
double MI_est;
ANNkd_tree* kdTreeX;
ANNkd_tree* kdTreeY;
unsigned int dimXY = dimX + dimY;
// Allocate memory
dataX = annAllocPts(N,dimX);
dataY = annAllocPts(N,dimY);
distsXY = new double[N];
// Normalize data and populate the marginals dataX, dataY
normalizeANN_XY( dataXY, dimXY, dataX, dimX, dataY, dimY, N);
// Get distance to knn for each point
kdTreeX = new ANNkd_tree( dataX, N, dimX );
kdTreeY = new ANNkd_tree( dataY, N, dimY );
distANN_XY( dataXY, dataXY, distsXY, dimXY, dimXY, N, N, k, eps );
// Compute mutual information
double marginal_contrib = 0.0;
for( unsigned int i = 0; i < N; i++ ) {
// get the number of points within a specified radius
int no_pts_X = kdTreeX->annkFRSearch( dataX[ i ], distsXY[ i ], 0, NULL, NULL, eps);
int no_pts_Y = kdTreeY->annkFRSearch( dataY[ i ], distsXY[ i ], 0, NULL, NULL, eps);
// digamma evaluations
marginal_contrib += gsl_sf_psi_int( no_pts_X+1 ) + gsl_sf_psi_int( no_pts_Y+1 );
}
MI_est = gsl_sf_psi_int( k ) + gsl_sf_psi_int( N ) - marginal_contrib / (double)N;
// Deallocate memory
delete kdTreeX;
delete kdTreeY;
delete [] distsXY;
annDeallocPts( dataX );
annDeallocPts( dataY );
return MI_est;
}
object k_fixed_radius_search(ANNkd_tree& kdtree, object q, double sqRad, int k, double eps)
{
BOOST_ASSERT(k <= kdtree.nPoints() && kdtree.theDim() == len(q));
ANNpointManaged annq(kdtree.theDim());
for (int c = 0; c < kdtree.theDim(); ++c)
annq.pt[c] = extract<ANNcoord>(q[c]);
if( k <= 0 ) {
int kball = kdtree.annkFRSearch(annq.pt, sqRad, k, NULL, NULL, eps);
return boost::python::make_tuple(numeric::array(boost::python::list()).astype("i4"),numeric::array(boost::python::list()),kball);
}
std::vector<ANNdist> dists(k);
std::vector<ANNidx> nn_idx(k);
int kball = kdtree.annkFRSearch(annq.pt, sqRad, k, &nn_idx[0], &dists[0], eps);
if( kball <= 0 )
return boost::python::make_tuple(numeric::array(boost::python::list()).astype("i4"),numeric::array(boost::python::list()),kball);
npy_intp dims[] = { min(k,kball)};
PyObject *pydists = PyArray_SimpleNew(1,dims, sizeof(ANNdist)==8 ? PyArray_DOUBLE : PyArray_FLOAT);
BOOST_ASSERT(!!pydists);
PyObject *pyidx = PyArray_SimpleNew(1,dims, PyArray_INT);
if( !pyidx )
Py_DECREF(pydists);
BOOST_ASSERT(!!pyidx);
ANNdist* pdists = (ANNdist*)PyArray_DATA(pydists);
ANNidx* pidx = (ANNidx*)PyArray_DATA(pyidx);
int addindex=0;
for (int i = 0; i < k; ++i) {
if (nn_idx[i] != ANN_NULL_IDX) {
pdists[addindex] = dists[i];
pidx[addindex] = nn_idx[i];
addindex++;
}
}
BOOST_ASSERT(kball > k || addindex==kball);
return boost::python::make_tuple(static_cast<numeric::array>(handle<>(pyidx)), static_cast<numeric::array>(handle<>(pydists)),kball);
}
void Mesh::computeVerticesNormals(const int &begin, const int &size)
{
int _size = (int)m_vertices.size();
if (begin > _size || begin + size > _size)
{
return;
}
// 构建kdtree
ANNpointArray verticesData = annAllocPts(_size, 3);
for (int i = 0; i < _size; i++)
{
Vec3d v = m_vertices[i].m_xyz;
verticesData[i][0] = v[0];
verticesData[i][1] = v[1];
verticesData[i][2] = v[2];
}
ANNkd_tree* kdtree = new ANNkd_tree(verticesData, _size, 3);
for (int i = begin; i < begin + size; i++)
{
Vec3d v = m_vertices[i].m_xyz;
ANNidx idxs[k];
ANNdist dists[k];
/* kdtree->annkSearch(verticesData[i], k, idxs, dists);*/
int cnt = kdtree->annkFRSearch(verticesData[i],
DISTANCE_RANGE, k, idxs, dists); // 其中idxs[0] = i;
if (cnt >= 3) // 最近邻小于3个，不能计算法向量
{
cnt = cnt > k ? k : cnt;
Mat barycenter;
m_vertices[i].m_normal = computeNormal(
verticesData, idxs, cnt, barycenter); // 计算法向量
memcpy(m_vertices[i].m_neighbors, idxs, cnt * sizeof(int));
m_vertices[i].m_neighbors[0] = cnt - 1;
inner(i);
//
// Vec3d tmp = v - Vec3d(barycenter);
// m_vertices[i].m_residual = abs(m_vertices[i].m_normal.ddot(tmp));
}
}
annDeallocPts(verticesData);
}
void CommonMesh::VertexEquivalenceClasses(std::vector< std::vector<int> >& outClasses, float distThresh) const
{
/** Set up KD tree for nearest-neighbor search **/
const int dim = 3; // we're in 3 space
const int k = 100000; // we want *all* points within the radius
const float r2 = distThresh*distThresh;
int nPts = (int)NumVertices(); // actual number of data points
ANNpointArray dataPts; // data points
ANNpoint queryPt; // query point
ANNidxArray nnIdx; // near neighbor indices
ANNdistArray dists; // near neighbor distances
ANNkd_tree* kdTree; // search structure
queryPt = annAllocPt(dim); // allocate query point
dataPts = annAllocPts(nPts, dim); // allocate data points
nnIdx = new ANNidx[k]; // allocate near neigh indices
dists = new ANNdist[k]; // allocate near neighbor dists
// Fill in data points array with my vertices
const Vec3List& verts = Vertices();
for (int i = 0; i < nPts; i++)
{
dataPts[i][0] = verts[i][0];
dataPts[i][1] = verts[i][1];
dataPts[i][2] = verts[i][2];
}
kdTree = new ANNkd_tree( // build search structure
dataPts, // the data points
nPts, // number of points
dim); // dimension of space
// Build 'equivalence classes' of vertices by doing nearest neighbor searches
// from each vertex
vector<bool> seen(nPts, false);
for (int i = 0; i < nPts; i++)
{
// If we haven't seen this vertex already, make a new equivalence class
if (!seen[i])
{
outClasses.push_back(vector<int>());
// Copy vertex location into query point
queryPt[0] = verts[i][0];
queryPt[1] = verts[i][1];
queryPt[2] = verts[i][2];
int numPoints = kdTree->annkFRSearch(
queryPt, // query point
r2, // squared radius
k, // number of near neighbors to return
nnIdx, // nearest neighbor array (modified)
dists, // dist to near neighbors (modified)
0.0); // error bound
if (numPoints == 0)
FatalError(string("CommonMesh::VertexEquivalenceClasses - ANN returned 0 points for query!"))
// Add all the retrieved points to the equivalence class, and mark them
// all as seen
for (int j = 0; j < numPoints; j++)
{
outClasses.back().push_back(nnIdx[j]);
seen[nnIdx[j]] = true;
}
}
}
Foam::labelListList Foam::parcelCloud::findParticlesIn()
{
if(useAllParticles_)
{
particlesIn_.resize(particlesInSubDomain_.size(),labelList(nPInParcel_,-1));
}else
{
particlesIn_.resize(particlesInSubDomain_.size()/nPInParcel_,labelList(nPInParcel_,-1));
}
// Number of particles in a parcel
int k = nPInParcel_;
// Dimensions, exact OR approximate
int dim =3; double eps = 0;
// Number of points
int nPts;
nPts = particlesInSubDomain_.size();
Pout << tab << "Number of particles in a parcel = " << nPInParcel_ << endl;
// Domain Min/Max
const fvMesh& mesh = refCast<const fvMesh>(obr_);
const pointField& pp = mesh.points();
// Min, max x-coordinates
scalar minX = Foam::min(pp & vector(1,0,0));
scalar maxX = Foam::max(pp & vector(1,0,0));
// Min, max y-coordinates
scalar minY = Foam::min(pp & vector(0,1,0));
scalar maxY = Foam::max(pp & vector(0,1,0));
// Min, max z-coordinates
scalar minZ = Foam::min(pp & vector(0,0,1));
scalar maxZ = Foam::max(pp & vector(0,0,1));
// Squared radius
const scalar sqRad = pow( (maxX - minX) * (maxX - minX)
+(maxY - minY) * (maxY - minY)
+(maxZ - minZ) * (maxZ - minZ), 0.5);
Pout << tab << "Squared radius = " << sqRad << endl;
// Data points
ANNpointArray dataPts;
// Query points
ANNpoint queryPt;
ANNidxArray nnIdx; // near neighbour indices
ANNdistArray dists; // near neighbour distances
ANNkd_tree* kdTree; // search structure
Pout << tab << "Created kdTree variables " << endl;
// Allocate
queryPt = annAllocPt(dim);
dataPts = annAllocPts(nPts, dim);
nnIdx = new ANNidx[k];
dists = new ANNdist[k];
Pout << tab << "Allocated kdTree variables " << endl;
labelList particleCreateParcelList(particlesInSubDomain_.size());
for(int ii =0; ii < particlesInSubDomain_.size(); ii++)
{
label particleGlobalID = particlesInSubDomain_[ii];
dataPts[ii][0] = sm_.position(particleGlobalID).x();
dataPts[ii][1] = sm_.position(particleGlobalID).y();
dataPts[ii][2] = sm_.position(particleGlobalID).z();
particleCreateParcelList[ii] = particleGlobalID;
}
Pout << tab << "Creating kdTree..." << endl;
kdTree = new ANNkd_tree(dataPts, nPts, dim);
Pout << tab << "Entering particle loops to create parcel" << endl;
label parcelI = 0;
forAll(particlesInSubDomain_,ii)
{
label particleGlobalID = particlesInSubDomain_[ii];
if ( particleCreateParcelList[ii] > -1 )
{
queryPt[0] = sm_.position(particleGlobalID).x();
queryPt[1] = sm_.position(particleGlobalID).y();
queryPt[2] = sm_.position(particleGlobalID).z();
kdTree->annkFRSearch(
queryPt, // query point
sqRad, // squared radius
k, // number of the near neighbours to return
nnIdx, // nearest neighbor array
dists, // dist to near neighbours
eps );
int partSum = 0;
int i = 0;
while( i < k && partSum < nPInParcel_ )
{
if ( particleCreateParcelList[nnIdx[i]] != -1 )
{
//Info << " parcelI " << parcelI << " partSum " << partSum << " Neighbour part "
// << nnIdx[i] << " particlesInSubDomain " << particlesInSubDomain_[nnIdx[i]]
// << " particlesIn " << particlesIn_[parcelI][partSum] << endl;
if (!useAllParticles_) particleCreateParcelList[nnIdx[i]] = -1 ;
particlesIn_[parcelI][partSum] = particlesInSubDomain_[nnIdx[i]];
partSum++;
if( partSum == nPInParcel_ ) parcelI++;
};
i++;
};
}
}
void createParcels
(
const fvMesh& mesh,
cfdemCloud& sm,
const int& parcelSize_,
int**& parcelCloud_,
double**& parcelPositions_,
double**& parcelVelocities_,
int*& parcelNparts_,
double** & parcelKinStress_,
scalar& aveSubQparcel2_,
vector& meanParcelVel_,
const bool verbose_
)
{
if ( parcelSize_ * parcelSize_ * parcelSize_ > sm.numberOfParticles() )
{
FatalError << " Number of particles in a parcel > number of particles" << abort(FatalError);
}
if ( parcelSize_ < 1 )
{
FatalError << " Number of particles < 0 in a parcel " << abort(FatalError);
}
// Number of particles in a parcel
int k = parcelSize_ * parcelSize_ * parcelSize_;
// Dimensions, exact OR approximate
int dim =3; double eps = 0;
// Number of points
int nPts;
nPts = sm.numberOfParticles();
// Data points
ANNpointArray dataPts;
// Query points
ANNpoint queryPt;
ANNidxArray nnIdx; // near neighbour indices
ANNdistArray dists; // near neighbour distances
ANNkd_tree* kdTree; // search structure
// Allocate
queryPt = annAllocPt(dim);
dataPts = annAllocPts(nPts, dim);
nnIdx = new ANNidx[k];
dists = new ANNdist[k];
for(int index = 0; index < sm.numberOfParticles(); index++)
{
dataPts[index][0] = sm.position(index).x();
dataPts[index][1] = sm.position(index).y();
dataPts[index][2] = sm.position(index).z();
}
kdTree = new ANNkd_tree(dataPts, nPts, dim);
// Initialize sub-parcel agitation
aveSubQparcel2_ = 0.;
// Initialize parcel velocity
meanParcelVel_ = vector(0,0,0);
for(int index = 0; index < sm.numberOfParticles(); index++)
{
// Particle neighbouring search distance
scalar sqRad = parcelSize_ * sm.radius(index);
queryPt[0] = sm.position(index).x();
queryPt[1] = sm.position(index).y();
queryPt[2] = sm.position(index).z();
kdTree->annkFRSearch(
queryPt, // query point
sqRad, // squared radius
k, // number of the near neighbours to return
nnIdx, // nearest neighbor array
dists, // dist to near neighbours
eps );
int nParts = 0;
scalar dist = 0;
// Initialize parcel velocities & positions & kinetic stresses
for(int j=0;j<3;j++)
{
parcelVelocities_[index][j] = 0.;
parcelPositions_[index][j] = 0.;
parcelKinStress_[index][j] = 0.;
parcelKinStress_[index][2*j] = 0.;
}
for (int i = 0; i < k; i++)
{
parcelCloud_[index][i] = nnIdx[i];
dist = mag( sm.position(nnIdx[i]) - sm.position(index) ) - sqRad;
if ( dist < SMALL )
{
for(int j=0;j<3;j++)
{
// Parcel velocity
parcelVelocities_[index][j] += sm.velocity(nnIdx[i])[j];
// Parcel center of mass
parcelPositions_[index][j] += sm.position(nnIdx[i])[j];
}
nParts++;
}
}
for(int j=0;j<3;j++) parcelPositions_[index][j] /= nParts;
parcelNparts_[index] = nParts;
// Parcel kinetic stresses
for(int i = 0; i < parcelNparts_[index]; i++)
{
int particleID = parcelCloud_[index][i];
// U'xU'x
parcelKinStress_[index][0] += ( sm.velocity(particleID)[0] - parcelVelocities_[index][0] )
*( sm.velocity(particleID)[0] - parcelVelocities_[index][0] );
// U'yU'y
parcelKinStress_[index][1] += ( sm.velocity(particleID)[1] - parcelVelocities_[index][1] )
*( sm.velocity(particleID)[1] - parcelVelocities_[index][1] );
// U'zU'z
parcelKinStress_[index][2] += ( sm.velocity(particleID)[2] - parcelVelocities_[index][2] )
*( sm.velocity(particleID)[2] - parcelVelocities_[index][2] );
// U'xU'y
parcelKinStress_[index][3] += ( sm.velocity(particleID)[0] - parcelVelocities_[index][0] )
*( sm.velocity(particleID)[1] - parcelVelocities_[index][1] );
// U'xU'z
parcelKinStress_[index][4] += ( sm.velocity(particleID)[0] - parcelVelocities_[index][0] )
*( sm.velocity(particleID)[2] - parcelVelocities_[index][2] );
// U'yU'z
parcelKinStress_[index][5] += ( sm.velocity(particleID)[1] - parcelVelocities_[index][1] )
*( sm.velocity(particleID)[2] - parcelVelocities_[index][2] );
}
// Mean parcel velocity
for(int j=0;j<3;j++) meanParcelVel_[j] += parcelVelocities_[index][j];
// Domain-averaged parcel agitation
aveSubQparcel2_ += 1./2. * ( parcelKinStress_[index][0] + parcelKinStress_[index][1] + parcelKinStress_[index][2] );
}
for(int j=0;j<3;j++) meanParcelVel_[j] /= sm.numberOfParticles();
if ( verbose_ )
{
int index = 0;
Info << " Parcel particle list ";
for (int i = 0; i < parcelNparts_[index]; i++)
{
Info << parcelCloud_[index][i] << " " ;
}
Info << endl;
Info << " Parcel center " << parcelPositions_[index][0] << "," << parcelPositions_[index][1] << "," << parcelPositions_[index][2] << endl;
Info << " Parcel velocity " << parcelVelocities_[index][0] << "," << parcelVelocities_[index][1] << "," << parcelVelocities_[index][2] << endl;
for (int i = 0; i < parcelNparts_[index]; i++)
{
Info << " Particle " << parcelCloud_[index][i] << endl;
Info << " Particle center " << sm.position(parcelCloud_[index][i]) << endl;
Info << " Particle velocity " << sm.velocity(parcelCloud_[index][i]) << endl;
}
}
}
