// decompress sphere into a cube
int sph2cb(float *sphere, Vec3i volsize, int nrays, int ri,
int nnz0, int *ptrs, int *cord, float *cube)
{
int status=0;
int r2, i, j, ix, iy, iz, nnz;
int nx = (int)volsize[0];
int ny = (int)volsize[1];
// int nz = (int)volsize[2];
r2 = ri*ri;
nnz = 0;
ptrs(1) = 1;
// no need to initialize
// for (i = 0; i<nx*ny*nz; i++) cube[i]=0.0;
nnz = 0;
for (j = 1; j <= nrays; j++) {
iz = cord(1,j);
iy = cord(2,j);
ix = cord(3,j);
for (i = ptrs(j); i<=ptrs(j+1)-1; i++, iz++) {
nnz++;
cube(iz,iy,ix) = sphere(nnz);
}
}
if (nnz != nnz0) status = -1;
return status;
}
int cb2sph(float *cube, Vec3i volsize, int ri, Vec3i origin,
int nnz0, int *ptrs, int *cord, float *sphere)
{
int xs, ys, zs, xx, yy, zz, rs, r2;
int ix, iy, iz, jnz, nnz, nrays;
int ftm = 0, status = 0;
int xcent = (int)origin[0];
int ycent = (int)origin[1];
int zcent = (int)origin[2];
int nx = (int)volsize[0];
int ny = (int)volsize[1];
int nz = (int)volsize[2];
r2 = ri*ri;
nnz = 0;
nrays = 0;
ptrs(1) = 1;
for (ix = 1; ix <= nx; ix++) {
xs = ix-xcent;
xx = xs*xs;
for ( iy = 1; iy <= ny; iy++ ) {
ys = iy-ycent;
yy = ys*ys;
jnz = 0;
ftm = 1;
// not the most efficient implementation
for (iz = 1; iz <= nz; iz++) {
zs = iz-zcent;
zz = zs*zs;
rs = xx + yy + zz;
if (rs <= r2) {
jnz++;
nnz++;
sphere(nnz) = cube(iz, iy, ix);
// record the coordinates of the first nonzero ===
if (ftm) {
nrays++;
cord(1,nrays) = iz;
cord(2,nrays) = iy;
cord(3,nrays) = ix;
ftm = 0;
}
}
} // end for (iz..)
if (jnz > 0) {
ptrs(nrays+1) = ptrs(nrays) + jnz;
} // endif (jnz)
} // end for iy
} // end for ix
if (nnz != nnz0) status = -1;
return status;
}
SPoint CBobsMap::TransRelativePointToAbusolutePoint(const SPoint& src)
{
Coordinate cord(m_spA,m_spB);
SPoint target =cord.GetCoordinate(src.x,src.y);
return target;
}
void StereoReconstructor::translateCoordinate(std::string cordsys, std::string points, std::string newPoints) {
std::ifstream cord(cordsys.c_str());
if (!cord) {
std::cout << cordsys << " file open failed!" << std::endl;
return;
}
std::vector<cv::Point3f> coord;
cv::Point3f p;
while (cord >> p.x >> p.y >> p.z) {
coord.push_back(p);
}
std::ifstream point(points.c_str());
if (!point) {
std::cout << points << " file open failed!" << std::endl;
return;
}
std::vector<cv::Point3f> pointsv;
while (point >> p.x >> p.y >> p.z) {
pointsv.push_back(p);
}
std::ofstream newpoint(newPoints);
if (!newpoint) {
std::cout << "new_points.txt open failed" << std::endl;
return;
}
for (size_t i = 0; i != pointsv.size(); i++) {
cv::Point3f p = coordinateChange(pointsv[i], coord);
newpoint << p.x << '\t' << p.y << '\t' << p.z << std::endl;
}
newpoint.close();
}
void
sql_parser_create(struct Parse *parser, sql *db)
{
memset(parser, 0, sizeof(struct Parse));
parser->db = db;
rlist_create(&parser->record_list);
region_create(&parser->region, &cord()->slabc);
}
void npc::move()
{
bool first_run = true;
if(first_run)
{elapsedTime = clock.restart();}
tempSpeed = elapsedTime.asSeconds()*10000;
/*cout <<tempSpeed<<endl;*/
sf::Vector2f cord(sprite.getPosition());
if(tempSpeed>50)
{
not_working = true;
find_dir();
cout<<dir<<endl;
/*cout<<cord.x<<" , "<<cord.y<<endl;*/
sf::Vector2f arrowcords=sprite.getPosition();
//cout<<arrowcords.x<<" , "<<arrowcords.y<<endl;
switch (dir)
{
case 0:
sprite.move(-40,-40);
break ;
case 1:
sprite.move(0,-40);
break ;
case 2:
sprite.move(40,-40);
break ;
case 3:
sprite.move(-40,0);
break ;
case 4:
sprite.move(40,0);
break ;
case 5:
sprite.move(-40,40);
break ;
case 6:
sprite.move(0,40);
break ;
case 7:
sprite.move(40,40);
break ;
default:
sprite.move(0,0);
break ;
elapsedTime = clock.restart();
//cout<<"reset clock\n";
}
elapsedTime = clock.restart();
//cout<<"reset clock\n";
}
}
vector<SPoint> CBobsMap::FixToBestPath(const vector<WayPoint> &waypoints)
{
vector<WayPoint> vecPath=waypoints;
Coordinate cord(m_spA,m_spB);
//push the last point
SPoint endPoint=cord.GetRelativePoint(m_spB.x,m_spB.y);
vecPath.push_back(WayPoint(m_spB,endPoint));
//fix waypoint
SPoint preAbs,fBRel,fCRel,fAbs;
preAbs=m_spA;
vector<SPoint>bestPath;
//push start point
bestPath.push_back(m_spA);
int i=0;
int index=0;
int vecPathSize = vecPath.size();
for(int q=0;q<vecPathSize;q++)
{
fBRel=vecPath[q].relativeXY;
fAbs =vecPath[q].absoluteXY;
for(i=q+1;i<vecPath.size();i++)
{
fCRel=vecPath[i].relativeXY;
if(abs(fBRel.y)>abs(fCRel.y)||Equal(fBRel.y,fCRel.y))
{
bool log = BarrierIntersection(preAbs,vecPath[i].absoluteXY);
if(!log)
{
fBRel = fCRel;
fAbs =vecPath[i].absoluteXY;
q=i;
}
}
}
bestPath.push_back(fAbs);
preAbs=fAbs;
}
return bestPath;
}
bool CBobsMap::GetOneValidPath(vector<int> &vecBits,int chromolen)
{
int testCount=0;
int iChromoLength=chromolen;
int iCutCount=iChromoLength+1;
Coordinate cord(m_spA,m_spB);
SPoint spPre=m_spA;
int randomY = 0;
//divied space
SVector2D line(m_spA.x-m_spB.x,m_spA.y-m_spB.y);
int diffX = (int) Vec2DLength(line)/iCutCount;
for(int i=1;i<=iChromoLength;i++)
{
randomY = RandInt(-150,150);
SPoint spRelative(i*diffX,randomY);
SPoint spPathAbsolute=cord.GetCoordinate(spRelative.x,spRelative.y);
while(true)
{
if(!BarrierIntersection(spPre,spPathAbsolute)&&IsValidPoint(spPathAbsolute))
{
break;
}else
{
testCount++;
if(testCount>100)
{
return false;
}
}
randomY = RandInt(-150,150);
spRelative=SPoint(i*diffX,randomY);
spPathAbsolute=cord.GetCoordinate(spRelative.x,spRelative.y);
}
//save waypoint
vecBits.push_back(randomY);
spPre=spPathAbsolute;
}
return true;
}
void CBobsMap::RenderOriginRoute(const int cxClient,
const int cyClient,
HDC surface)
{
//grab a red pen
m_OldPen = (HPEN)SelectObject(surface,m_RedPen);
Coordinate cord(m_spA,m_spB);
vector<vector<WayPoint>>::const_iterator veciter = m_TestRoute.begin();
//draw orginin path in red pen
if(true)
{
while( veciter!=m_TestRoute.end())
{
SPoint spPre = m_spA;
vector<WayPoint>::const_iterator iter = veciter->begin();
while(iter != veciter->end())
{
SPoint spRelative =iter->relativeXY;
spRelative.y=0;
SPoint spRoot = cord.GetCoordinate(spRelative.x,spRelative.y);
SPoint spPathAbsolute = iter->absoluteXY;
DrawLine(surface,spPre,spPathAbsolute);
spPre =spPathAbsolute;
iter++;
}
DrawLine(surface,spPre,m_spB);
veciter++;
}
}
SelectObject(surface, m_OldPen);
}
struct ipc_value *
ipc_value_new()
{
if (! mempool_is_initialized(&ipc_value_pool)) {
/*
* We don't need to bother with
* destruction since the entire slab cache
* is freed when the thread ends.
*/
mempool_create(&ipc_value_pool, &cord()->slabc,
sizeof(struct ipc_value));
}
struct ipc_value *value = (struct ipc_value *)
mempool_alloc(&ipc_value_pool);
if (value == NULL) {
diag_set(OutOfMemory, sizeof(struct ipc_value),
"ipc_msg_pool", "struct ipc_value");
return NULL;
}
value->base.destroy = ipc_value_delete;
return value;
}
// backproject from 2D to 3D for a single image
int bckpj3(Vec3i volsize, int nrays, int nnz, float *dm,
Vec3i origin, int ri, int *ptrs, int *cord,
float *x, float *y)
{
int i, j, iqx,iqy, xc, yc, zc;
float xb, yb, dx, dy, dx1m, dy1m, dxdy;
int status = 0;
int xcent = origin[0];
int ycent = origin[1];
int zcent = origin[2];
int nx = volsize[0];
int ny = volsize[1];
// Phi: adding the shift parameters that get passed in as the last two entries of dm
float sx, sy;
sx = dm(7);
sy = dm(8);
if ( nx > 2*ri) {
for (i = 1; i <= nrays; i++) {
zc = cord(1,i) - zcent;
yc = cord(2,i) - ycent;
xc = cord(3,i) - xcent;
xb = zc*dm(1)+yc*dm(2)+xc*dm(3) + xcent + sx;
yb = zc*dm(4)+yc*dm(5)+xc*dm(6) + ycent + sy;
for (j = ptrs(i); j <ptrs(i+1); j++) {
iqx = ifix(xb);
iqy = ifix(yb);
dx = xb - iqx;
dy = yb - iqy;
dx1m = 1.0 - dx;
dy1m = 1.0 - dy;
dxdy = dx*dy;
/*
c y(j) = y(j) + dx1m*dy1m*x(iqx , iqy)
c & + dx1m*dy *x(iqx , iqy+1)
c & + dx *dy1m*x(iqx+1, iqy)
c & + dx *dy *x(iqx+1, iqy+1)
c
c --- faster version of the above commented out
c code (derived by summing the following table
c of coefficients along the colunms) ---
c
c 1 dx dy dxdy
c ------ -------- -------- -------
c x(i,j) -x(i,j) -x(i,j) x(i,j)
c x(i,j+1) -x(i,j+1)
c x(i+1,j) -x(i+1,j)
c x(i+1,j+1)
c
*/
// Phi: add index checking, now that shifts are being used
if ( iqx <= nx && iqy <= ny && iqx >= 1 && iqy >= 1 ) {
y(j) += x(iqx,iqy);
if ( iqx + 1 <= nx && iqx + 1 >= 1 ) {
y(j) += dx*(-x(iqx,iqy)+x(iqx+1,iqy));
}
if ( iqy + 1 <= ny && iqy + 1 >= 1 ) {
y(j) += dy*(-x(iqx,iqy)+x(iqx,iqy+1));
}
if ( iqx + 1 <= nx && iqy + 1 <= ny && iqx + 1 >= 1 && iqy + 1 >= 1 ) {
y(j) += dxdy*( x(iqx,iqy) - x(iqx,iqy+1) -x(iqx+1,iqy) + x(iqx+1,iqy+1) );
}
}
// y(j) += x(iqx,iqy)
// + dx*(-x(iqx,iqy)+x(iqx+1,iqy))
// + dy*(-x(iqx,iqy)+x(iqx,iqy+1))
// + dxdy*( x(iqx,iqy) - x(iqx,iqy+1)
// -x(iqx+1,iqy) + x(iqx+1,iqy+1) );
xb += dm(1);
yb += dm(4);
} // end for j
} // end for i
}
else {
fprintf(stderr, "bckpj3: nx must be greater than 2*ri\n");
}
return status;
}
// project from 3D to 2D (single image)
int fwdpj3(Vec3i volsize, int nrays, int nnz, float *dm,
Vec3i origin, int ri, int *ptrs, int *cord,
float *x, float *y)
{
/*
purpose: y <--- proj(x)
input : volsize the size (nx,ny,nz) of the volume
nrays number of rays within the compact spherical
representation
nnz number of voxels within the sphere
dm an array of size 9 storing transformation
associated with the projection direction
origin coordinates of the center of the volume
ri radius of the sphere
ptrs the beginning address of each ray
cord the coordinates of the first point in each ray
x 3d input volume
y 2d output image
*/
int iqx, iqy, i, j, xc, yc, zc;
float ct, dipx, dipy, dipx1m, dipy1m, xb, yb, dm1, dm4;
int status = 0;
// Phi: adding the shift parameters that get passed in as the last two entries of dm
float sx, sy;
sx = dm(7);
sy = dm(8);
int xcent = origin[0];
int ycent = origin[1];
int zcent = origin[2];
int nx = volsize[0];
int ny = volsize[1];
dm1 = dm(1);
dm4 = dm(4);
if ( nx > 2*ri ) {
for (i = 1; i <= nrays; i++) {
zc = cord(1,i)-zcent;
yc = cord(2,i)-ycent;
xc = cord(3,i)-xcent;
xb = zc* dm(1) +yc* dm(2) +xc* dm(3) + xcent + sx;
yb = zc* dm(4) +yc* dm(5) +xc* dm(6) + ycent + sy;
for (j = ptrs(i); j< ptrs(i+1); j++) {
iqx = ifix(xb);
iqy = ifix(yb);
ct = x(j);
// dipx = xb - (float)(iqx);
// dipy = (yb - (float)(iqy)) * ct;
dipx = xb - iqx;
dipy = (yb - iqy) * ct;
dipy1m = ct - dipy;
dipx1m = 1.0 - dipx;
if (iqx <= nx && iqy <= ny && iqx >= 1 && iqy >= 1)
// y(iqx ,iqy) = y(iqx ,iqy) + dipx1m*dipy1m;
y(iqx ,iqy) += dipx1m*dipy1m;
if (iqx + 1 <= nx && iqy <= ny && iqx >= 0 && iqy >= 1)
// y(iqx+1,iqy) = y(iqx+1,iqy) + dipx*dipy1m;
y(iqx+1,iqy) += dipx*dipy1m;
if (iqx + 1 <= nx && iqy + 1 <= ny && iqx >= 0 && iqy >= 0)
// y(iqx+1,iqy+1) = y(iqx+1,iqy+1) + dipx*dipy;
y(iqx+1,iqy+1) += dipx*dipy;
if (iqx <= nx && iqy + 1 <= ny && iqx >= 1 && iqy >= 0)
// y(iqx ,iqy+1) = y(iqx ,iqy+1) + dipx1m*dipy;
y(iqx ,iqy+1) += dipx1m*dipy;
xb += dm1;
yb += dm4;
}
}
}
else {
fprintf(stderr, " nx must be greater than 2*ri\n");
exit(1);
}
return status;
}
int main(void)
{
std::bounds<1> cord(std::numeric_limits<ptrdiff_t>::max() + 1);
}
