//------------------------------------------------------------------------------
void Solver::HashTable::Query (const Vector2f& position, float range)
{
// reset previous result
mResult.clear();
Vector2f begf(position);
begf.X -= range;
begf.Y -= range;
Vector2f endf(position);
endf.X += range;
endf.Y += range;
Vector2ui beg = computeCoordinate(begf, mDomain);
Vector2ui end = computeCoordinate(endf, mDomain);
for (unsigned int i = beg.X; i <= end.X; i++)
{
for (unsigned int j = beg.Y; j <= end.Y; j++)
{
unsigned int hash = computeHash(i, j, mDomain.Dimensions.X);
mResult.insert
(
mResult.end(),
mBuckets[hash].begin(),
mBuckets[hash].end()
);
}
}
}
//-----------------------------------------------------------------------------
void ParticleHashGrid::RangeQuery (const std::set<Wm5::Vector3f> result,
const Wm5::Vector3f& query, float range) const
{
Wm5::Vector3f start;
start.X() = query.X() - range;
start.Y() = query.Y() - range;
start.Z() = query.Z() - range;
Wm5::Vector3f end;
end.X() = query.X() + range;
end.Y() = query.Y() + range;
end.Z() = query.Z() + range;
Wm5::Tuple<3, int> sc = computeCoordinate(start);
Wm5::Tuple<3, int> se = computeCoordinate(end);
for (int k = sc[2]; k < ec[2]; k++)
{
for (int j = sc[1]; j < ec[1]; j++)
{
for (int i = sc[0]; i < ec[0]; i++)
{
}
}
}
}
//------------------------------------------------------------------------------
inline unsigned int computeHash
(
const Vector2f& position,
const Domain& domain
)
{
return computeHash
(
computeCoordinate(position, domain),
domain.Dimensions.X
);
}
//-----------------------------------------------------------------------------
void ParticleHashGrid::AddParticlePosition (const Wm5::Vector3f& position)
{
Wm5::Tuple<3, int> coordinate = computeCoordinate(position);
unsigned int index = computeIndex(coordinate);
mHashGrid[index].insert(position);
}
void StereoReconstructor::RestructCalibrationPointsFromImage(std::string leftFilename, std::string rightFilename, int x, int y, int squareLength) {
//计算角点
bool f1, f2;
CameraCalibration::findCorners(leftFilename, f1, x, y);
CameraCalibration::findCorners(rightFilename, f2, x, y);
if (!f1 || !f2) {
std::cout << "角点没找到" << std::endl;
return;
}
//载入2个虚拟摄像机
VirtualCamera vc1;
VirtualCamera vc2;
//载入内参
vc1.loadCameraMatrix("reconstruct\\LeftMatrix.txt");
vc1.loadDistortion("reconstruct\\LeftDistortion.txt");
vc1.rotationMatrix = cv::Mat::eye(3, 3, CV_32FC1);
vc1.translationVector = cv::Mat::zeros(3, 1, CV_32FC1);
vc2.loadCameraMatrix("reconstruct\\RightMatrix.txt");
vc2.loadDistortion("reconstruct\\RightDistortion.txt");
vc2.loadRotationMatrix("reconstruct\\R.txt");
vc2.loadTranslationVector("reconstruct\\T.txt");
//载入角点
vc1.loadKeyPoints(leftFilename + "_imgCorners.txt");
vc2.loadKeyPoints(rightFilename + "_imgCorners.txt");
//重建3d点
std::vector<RestructPoint> ret;
ret = triangulation(vc1, vc2, true); //进行畸变矫正,false表示不进行
//保存结果
//生成点云文件
plyFileGenerate(ret,
leftFilename + ".ply", //ply文件
"_calPointsDis.txt", //3d点+相交距离
"_calPoints.txt"); //3d点
//评估长度误差
resultEvaluate(ret,
"_calLineLength.txt", //157条线段的误差calLineLength.txt
"_calLineLengthDist.txt", //线段的长度的概率分布
x, y, squareLength);
//计算棋盘格坐标系
computeCoordinate(
"_calPoints.txt", //读取棋盘格坐标
"_coordinate.txt", //计算坐标系，保存到Coordinate.txt
x, y); //水平方向和垂直方向的角点个数
//转换到棋盘格坐标系
translateCoordinate(
"_coordinate.txt", //棋盘格坐标系
"_calPoints.txt", //要转换的3D点
"_calPoints_ObjSpace.txt"); //在棋盘格坐标系下的坐标calPoints_ObjSpace.txt
//生成物体坐标系下的ply文件
std::vector<cv::Point3f> pointcloud = Utilities::load3dPoints("_calPoints_ObjSpace.txt");
Utilities::savePly(pointcloud, "ObjSpace" + leftFilename + ".ply");
//生成棋盘格物体空间坐标，并载入
Utilities::generateObjCorners("_chessbordCorner.txt", x, y, squareLength);
std::vector<cv::Point3f> ideaPts = Utilities::load3dPoints("_chessbordCorner.txt");
std::vector<cv::Point3f> realPts = Utilities::load3dPoints("_calPoints_ObjSpace.txt");
//计算到角点的绝对距离，并保存
std::ofstream absDis("_absCornerDistance.txt");
for (int i = 0; i < ideaPts.size(); i++) {
cv::Point3f idea = ideaPts[i];
cv::Point3f real = realPts[i];
float dis = cv::norm((idea - real));
absDis << dis << std::endl;
}
absDis.close();
}
void wet::duplicateDrop1(void)
{
int kdup ;
int xkdup, ykdup, zkdup;
//double *nGlobal;
//nGlobal = new double[N];
generatePhiGlobal();
generateNGlobal();
generateGlobalMask();
MPI_Bcast(pGlobal,N,MPI_DOUBLE,ROOT,MPI_COMM_WORLD);
MPI_Bcast(nGlobal,N,MPI_DOUBLE,ROOT,MPI_COMM_WORLD);
MPI_Bcast(maskGlobal,N,MPI_DOUBLE,ROOT,MPI_COMM_WORLD);
/*for (k=0; k<N;k++){
cout << " - Process " << rank <<" k " << k << " pG[k] " << pGlobal[k] << endl;
}*/
initialise(); //pGlobal, nGlobal, maskGlobal not initialised
/*for (k=0; k < N/2 ;k++){
cout << " - - Process " << rank <<" k " << k << " pG[k] " << pGlobal[k] << endl;
}*/
cout << "Proces " << rank <<": re-initialised" << endl;
cout << "Proces " << rank << ": new LX = " << LX << endl;
//saveFiles(); //take OUT
if (rank < size/2){ //first half of the box, drop just copied over
for (k=k1 ; k< k2; k++) {
kdup = k-k1 + rank * (k2-k1);
xkdup =int(kdup/(float) (LZ*LY));
ykdup =int((kdup-xkdup*LZ*LY)/(float) LZ);
zkdup =kdup-xkdup*LZ*LY-ykdup*LZ;
p[k] = pGlobal[kdup];
n[k] = nGlobal[kdup];
mask[k] = maskGlobal[kdup];
//computeCoordinate(k);
//cout << "-- Process " << rank <<" k= "<< k << " xk yk zk " << xk << " " << yk << " " << zk << " " << "p[k] = " << p[k] << "; kdup= "<< kdup << " xk yk zk " << xkdup << " " << ykdup << " " << zkdup <<" " << "pG[kdup] = " << pGlobal[kdup] << endl;
//cout << "--- Process " << rank <<": k "<< k << ", kdup in pG "<< kdup <<endl;
}
}
if (rank >= size/2){
for (int kk=k1 ; kk< k2; kk++) {
kdup = kk;
computeCoordinate(kk);
xk = LX-1 - xk;
invComputeCoordinate(); // sets k, here value on Global arrays; invCompCooordinate not for parallel setup, so convenient
xkdup =int(kdup/(float) (LZ*LY));
ykdup =int((kdup-xkdup*LZ*LY)/(float) LZ);
zkdup =kdup-xkdup*LZ*LY-ykdup*LZ;
xkdup =xkdup+rank*LX/size-1%LX;
p[kdup] = pGlobal[k];
n[kdup] = nGlobal[k];
mask[kdup] = maskGlobal[k];
//computeCoordinate(k);
//cout << "<< Process " << rank <<" k= "<< kdup << " xk yk zk " << xkdup << " " << ykdup << " " << zkdup <<" " << "p[k] = " << p[kdup] <<"; kdup= "<< k << " xk yk zk " << xk << " " << yk << " " << zk << " " << "pG[kdup] = " << pGlobal[k] << endl;
}
}
if(equilfirst == true && (t - equilTime)<(1)){
if (afterequilflag == true && strcmp(geometry,"TESTSYSTEM2eq") == 0) {
//cout << "*** process " << rank << ": after equil, TESTSYSTEM2eq " << endl;
for (k = k1; k < k2; k++){
computeCoordinate(k);
uxs[k] = initUX*( -1.0* fabs(zk - (LZ-Dh)/2.0)/(LZ -Dh)*2 + 0.5);
//cout << "*** process " << rank << ": uxs at k=" << k << " is " << uxs[k] << endl;
if (zk < Dh) {
uxs[k] = 0.0;
}
uys[k] = 0.0;
uzs[k] = 0.0;
}
}
if (afterequilflag == true && strcmp(geometry,"TESTSYSTEM4eq") == 0) {
for (k = k1; k < k2; k++){
if (p[k] >= 0.0L) {
uxs[k] = initUX;
uys[k] = initUY;
uzs[k] = initUZ;
}
else{
uxs[k] = 0.0;
uys[k] = 0.0;
uzs[k] = 0.0;
}
}
}
if (afterequilflag == true && strcmp(geometry,"TESTSYSTEM3eq") == 0) {
for (k = k1; k < k2; k++){
computeCoordinate(k);
uxs[k] = initUX*( -1.0* fabs(zk - (LZ-Dh)/2.0)/(LZ -Dh)*2 + 0.5);
if (zk < Dh) {
uxs[k] = 0.0;
}
uys[k] = 0.0;
uzs[k] = 0.0;
}
}
if (afterequilflag == true && strcmp(geometry,"TESTSYSTEM5eq") == 0) {
for (k = k1; k < k2; k++){
if (p[k] < 0.0L) { //squeeze drop to form ellipse, does only make sense with duplicationtype=2
computeCoordinate(k);
/*
uxs[k] = 0.0L;
uys[k] = 0.0L;
uzs[k] = 0.0L;
*/
if( (xk-dropletCenterX)*(xk-dropletCenterX) + (zk-dropletCenterZ)*(zk-dropletCenterZ) - (dropletR+5)*(dropletR+5) <= 0 || (xk-(LX-dropletCenterX))*(xk-(LX-dropletCenterX)) + (zk-dropletCenterZ)*(zk-dropletCenterZ) - (dropletR+5)*(dropletR+5) <= 0 ){
if ( (zk-dropletCenterZ) > dropletR/sqrt(2) && ( fabs(xk-dropletCenterX) <= dropletR/sqrt(2) || fabs(xk-(LX-dropletCenterX)) <= dropletR/sqrt(2)) ) {
//uxs[k] = 0.0L;
//uys[k] = 0.0L;
uzs[k] = initUZ;
}
if ( -(zk-dropletCenterZ) > dropletR/sqrt(2) && ( fabs(xk-dropletCenterX) <= dropletR/sqrt(2) || fabs(xk-(LX-dropletCenterX)) <= dropletR/sqrt(2))) {
//uxs[k] = 0.0L;
//uys[k] = 0.0L;
uzs[k] = -initUZ;
}
if ( fabs(zk-dropletCenterZ) < dropletR/sqrt(2) ) {
if ( (xk < dropletCenterX) || (LX/2.0 < xk && xk < (LX-dropletCenterX)) ) {
uxs[k] = initUX;
//uys[k] = 0.0L;
//uzs[k] = 0.0L;
}
if ( (dropletCenterX < xk && xk < LX/2.0) || (xk > (LX-dropletCenterX) ) ) {
uxs[k] = -initUX;
//uys[k] = 0.0L;
//uzs[k] = 0.0L;
}
}
}
}
}
}
}
// have to set equil f and g again after assiging n[k] and p[k], otherwise it's zero.
for(k = k1; k < k2; k++){
if (mask[k] == 28) {n[k] = 1.0; p[k] = 0.0;}
//nn = n[k]; pp = p[k]; ux = uxs[k]; uy = uys[k]; uz = uzs[k];
equilibrium();
ff0[k] = fe0;
ff1[k] = fe1; ff2[k] = fe2; ff3[k] = fe3; ff4[k] = fe4; ff5[k] = fe5; ff6[k] = fe6;
ffa[k] = fea; ffb[k] = feb; ffc[k] = fec; ffd[k] = fed; ffe[k] = fee; fff[k] = fef;
ffg[k] = feg; ffh[k] = feh; ffi[k] = fei; ffj[k] = fej; ffk[k] = fek; ffl[k] = fel;
gg0[k] = ge0;
gg1[k] = ge1; gg2[k] = ge2; gg3[k] = ge3; gg4[k] = ge4; gg5[k] = ge5; gg6[k] = ge6;
gga[k] = gea; ggb[k] = geb; ggc[k] = gec; ggd[k] = ged; gge[k] = gee; ggf[k] = gef;
ggg[k] = geg; ggh[k] = geh; ggi[k] = gei; ggj[k] = gej; ggk[k] = gek; ggl[k] = gel;
}
/*
delete []maskGlobal;
delete []pGlobal;
delete []nGlobal;
*/
pGlobal = new double[N];
maskGlobal = new int[N];
generatePhiGlobal();
generateGlobalMask();
//saveFiles(); // just for testing - REMOVE! and instead put back in in LGAlg
if(rank==ROOT)
{
int i, j, h;
char filename[25];
sprintf(filename, "./densitydata/dd_dupl%ld.m", t); //Create a name for file that contain data
//sprintf(filename, "./dd%ld.m", t);
ofstream file(filename);
file.precision(4);
for( h = 0 ; h < LZ ; h++)
{
file << "ddup" << t << "(:,:," << h+1 << ")=[" << endl;
for( i = 0 ; i < LX ; i++)
{
for( j = 0 ; j < LY ; j++)
{
k = h + j*LZ + i*LY*LZ;
if( maskGlobal[k]==28)
file << -2.0 << " ";
else
file << pGlobal[k] << " ";
}
file << endl;
}
file <<"];" << endl;
}
file.close();
}
}
