// Performs an intersect operation on two interval lists to produce a
// result list. The this list and the otherList are inputs to the
// intersect operation. The result replaces the this list. The
// original interval list entries for the this list are deleted.
MdamIntervalList & MdamIntervalList::intersect
(const MdamIntervalList & otherList,
const ULng32 keyLen,
FixedSizeHeapManager & mdamIntervalHeap,
FixedSizeHeapManager & mdamRefListEntryHeap)
{
// Move entries from this into tempIntervalList.
#if defined ( NA_MDAM_EXECUTOR_DEBUG_ILTF )
MdamIntervalList tempIntervalList(1);
#else
MdamIntervalList tempIntervalList;
#endif /* NA_MDAM_EXECUTOR_DEBUG_ILTF */
giveAllIntervals(tempIntervalList);
MdamIntervalListMerger getNextPoint(&tempIntervalList,
&otherList,
keyLen);
MdamEndPoint endPoint1;
MdamEndPoint endPoint2;
getNextPoint(endPoint1);
while(endPoint1.exists())
{
if (getNextPoint.getActiveIntervals() == 2)
{
getNextPoint(endPoint2);
MdamInterval * tempIntervalPtr = new(mdamIntervalHeap)
MdamInterval(endPoint1, endPoint2);
append(tempIntervalPtr);
endPoint1 = endPoint2;
}
else
{
getNextPoint(endPoint1);
}; // end if
}; // end while
#if defined ( NA_MDAM_EXECUTOR_DEBUG_ILTF )
// Log entry for intervals resulting from the intersect operation.
logEvent(7,countIntervals(),otherList.intervalListId_);
#endif /* NA_MDAM_EXECUTOR_DEBUG_ILTF */
// Delete the original this list entries because the destructor
// can't.
tempIntervalList.deleteAllIntervals(mdamIntervalHeap,
mdamRefListEntryHeap);
return *this;
}
void envire::Bresenham::getLine(std::vector< envire::GridBase::Position >& positions
)
{
GridBase::Position p;
while(getNextPoint(p))
positions.push_back(p);
}
bool envire::Bresenham::getNextPoint(envire::GridBase::Position& p)
{
int x,y;
bool ret = getNextPoint(x, y);
p.x = x;
p.y = y;
return ret;
}
// Performs an union operation on two interval lists to produce a
// result list. The source lists have no MdamRefLists associated
// with them. The this list and the otherList are inputs to the
// union operation. The result replaces the this list. The
// original interval list entries for this list are deleted.
MdamIntervalList & MdamIntervalList::unionSameDisjunct
(const MdamIntervalList & otherList,
const ULng32 keyLen,
FixedSizeHeapManager & mdamIntervalHeap,
FixedSizeHeapManager & mdamRefListEntryHeap)
{
// Move entries from this into tempIntervalList.
MdamIntervalList tempIntervalList;
giveAllIntervals(tempIntervalList);
MdamIntervalListMerger getNextPoint(&tempIntervalList,
&otherList,
keyLen);
MdamEndPoint endPoint1;
MdamEndPoint endPoint2;
getNextPoint(endPoint1);
// If endPoint1 exists, it is a BEGIN endpoint.
while(endPoint1.exists())
{
while(getNextPoint.getActiveIntervals() >= 1)
{
getNextPoint(endPoint2);
}; // end while
// endPoint2 is an END endpoint.
MdamInterval * tempIntervalPtr = new(mdamIntervalHeap)
MdamInterval(endPoint1, endPoint2);
append(tempIntervalPtr);
getNextPoint(endPoint1);
}; // end while
// Delete the original this list entries because the destructor
// can't.
tempIntervalList.deleteAllIntervals(mdamIntervalHeap,
mdamRefListEntryHeap);
return *this;
}
GPPtr<GPParameter> GPGoldenDivideOpt::vFindBest(int n, IGPOptimizor::OPTFUNC computer, PFLOAT* target) const
{
GPASSERT(n>0);
GPPtr<GPParameter> result = new GPParameter(n);
PFLOAT* result_pars = result->attach();
for (int i=0; i<n; ++i)
{
PFLOAT fin = 1.0;
PFLOAT sta = 0.0;
PFLOAT t_fin = getNextPoint(sta,fin);
PFLOAT t_sta = getNextPoint(sta,t_fin);
PFLOAT fit_sta = runOnePass(i, t_sta, result, computer);
PFLOAT fit_fin = runOnePass(i, t_fin, result, computer);
while(fin-sta>mInter)
{
if (fit_sta > fit_fin)
{
fin = t_fin;
t_fin = t_sta;
t_sta = getNextPoint(sta, t_fin);
fit_fin = fit_sta;
fit_sta = runOnePass(i, t_sta, result, computer);
}
else
{
sta = t_sta;
t_sta = t_fin;
t_fin = getNextPoint(sta, fin);
fit_sta = fit_fin;
fit_fin = runOnePass(i, t_fin, result, computer);
}
}
result_pars[i] = getNextPoint(t_sta, t_fin);
}
return result;
}
inline void BoardStatus::checkChessStatus(const QPoint & chess, eDirection direct)
{
unsigned long bitMap = 0;
int leftWidth = 0;
int rightWidth = 0;
int numOfSpace = 2;
eChessColor color = haveChess(chess);
QPoint nextChess = chess;
eChessColor nextColor = color;
bool doubleFlag = true;
if (color == noChess || this->board[chess.x()][chess.y()].boardFlags[direct] == true)
{
return;
}
while (numOfSpace || (nextColor != noChess))
{
if (nextColor == color)
{
bitMap = bitMap << 1;
bitMap = bitMap | 1;
setBoardFlag(nextChess, direct);
numOfSpace = 2;
}
else if (nextColor == noChess)
{
bitMap = bitMap << 1;
numOfSpace--;
}
else
{
break;
}
rightWidth++;
getNextPoint(nextChess, direct, true);
nextColor = haveChess(nextChess);
}
if (numOfSpace == 0)
{
rightWidth--;
bitMap = bitMap >> 1;
doubleFlag = false;
}
/*! @brief Do the state behaviour */
void EvaluateWalkParametersState::doState()
{
#if DEBUG_BEHAVIOUR_VERBOSITY > 3
debug << "EvaluateWalkParametersState::doState() target: " << m_current_target_state << endl;
#endif
if (m_parent->stateChanged())
start();
else
tick();
lookAtGoals();
if (pointReached())
m_current_target_state = getNextPoint();
vector<float> speed = BehaviourPotentials::goToFieldState(m_field_objects->self, m_current_target_state, 0, 20, 5000);
m_jobs->addMotionJob(new WalkJob(speed[0], speed[1], speed[2]));
}
bool envire::Bresenham::getNextPoint(Eigen::Vector2i& p)
{
return getNextPoint(p.x(), p.y());
}
ccPlane* ccGenericPointCloud::fitPlane(double* rms /*= 0*/)
{
//number of points
unsigned count = size();
if (count<3)
return 0;
CCLib::Neighbourhood Yk(this);
//we determine plane normal by computing the smallest eigen value of M = 1/n * S[(p-µ)*(p-µ)']
CCLib::SquareMatrixd eig = Yk.computeCovarianceMatrix().computeJacobianEigenValuesAndVectors();
//invalid matrix?
if (!eig.isValid())
{
//ccConsole::Warning("[ccPointCloud::fitPlane] Failed to compute plane/normal for cloud '%s'",getName());
return 0;
}
eig.sortEigenValuesAndVectors();
//plane equation
PointCoordinateType theLSQPlane[4];
//the smallest eigen vector corresponds to the "least square best fitting plane" normal
double vec[3];
eig.getEigenValueAndVector(2,vec);
//PointCoordinateType sign = (vec[2] < 0.0 ? -1.0 : 1.0); //plane normal (always with a positive 'Z' by default)
for (unsigned i=0;i<3;++i)
theLSQPlane[i]=/*sign*/(PointCoordinateType)vec[i];
CCVector3 N(theLSQPlane);
//we also get centroid
const CCVector3* G = Yk.getGravityCenter();
assert(G);
//eventually we just have to compute 'constant' coefficient a3
//we use the fact that the plane pass through the centroid --> GM.N = 0 (scalar prod)
//i.e. a0*G[0]+a1*G[1]+a2*G[2]=a3
theLSQPlane[3] = G->dot(N);
//least-square fitting RMS
if (rms)
{
placeIteratorAtBegining();
*rms = 0.0;
for (unsigned k=0;k<count;++k)
{
double d = (double)CCLib::DistanceComputationTools::computePoint2PlaneDistance(getNextPoint(),theLSQPlane);
*rms += d*d;
}
*rms = sqrt(*rms)/(double)count;
}
//we has a plane primitive to the cloud
eig.getEigenValueAndVector(0,vec); //main direction
CCVector3 X(vec[0],vec[1],vec[2]); //plane normal
//eig.getEigenValueAndVector(1,vec); //intermediate direction
//CCVector3 Y(vec[0],vec[1],vec[2]); //plane normal
CCVector3 Y = N * X;
//we eventually check for plane extents
PointCoordinateType minX=0.0,maxX=0.0,minY=0.0,maxY=0.0;
placeIteratorAtBegining();
for (unsigned k=0;k<count;++k)
{
//projetion into local 2D plane ref.
CCVector3 P = *getNextPoint() - *G;
PointCoordinateType x2D = P.dot(X);
PointCoordinateType y2D = P.dot(Y);
if (k!=0)
{
if (minX<x2D)
minX=x2D;
else if (maxX>x2D)
maxX=x2D;
if (minY<y2D)
minY=y2D;
else if (maxY>y2D)
maxY=y2D;
}
else
{
minX=maxX=x2D;
minY=maxY=y2D;
}
}
//we recenter plane (as it is not always the case!)
float dX = maxX-minX;
float dY = maxY-minY;
CCVector3 Gt = *G + X * (minX+dX*0.5);
Gt += Y * (minY+dY*0.5);
ccGLMatrix glMat(X,Y,N,Gt);
return new ccPlane(dX,dY,&glMat);
}
// Performs a union operation on two interval lists to produce a
// result list. The list's intervals may have different
// MdamRefList*s associated with them. Any reference lists
// associated with otherList are ignored and disjunctNum is used
// instead. The this list and the otherList are inputs to the
// intersect operation. The result replaces the this list. The
// original interval list entries for the this list are deleted.
MdamIntervalList & MdamIntervalList::unionSeparateDisjuncts
(const MdamIntervalList & otherList,
const Int32 disjunctNum,
const ULng32 keyLen,
FixedSizeHeapManager & mdamIntervalHeap,
FixedSizeHeapManager & mdamRefListEntryHeap)
{
// Move entries from this into tempIntervalList.
#if defined ( NA_MDAM_EXECUTOR_DEBUG_ILTF )
MdamIntervalList tempIntervalList(2);
#else
MdamIntervalList tempIntervalList;
#endif /* NA_MDAM_EXECUTOR_DEBUG_ILTF */
giveAllIntervals(tempIntervalList);
MdamIntervalListMerger getNextPoint(&tempIntervalList,
&otherList,
keyLen);
MdamEndPoint currentEndPoint;
MdamEndPoint previousEndPoint;
// Track the active interval for each interval list. Zero means
// there is no active interval. This mechanism permits access
// to the corresponding reference list.
MdamInterval * intervalPtr0 = 0;
MdamInterval * intervalPtr1 = 0;
while(getNextPoint(currentEndPoint))
{
if (getNextPoint.getPreviousActiveIntervals() != 0
&& currentEndPoint.givesNonEmptyInterval(&previousEndPoint,keyLen))
{
// Create a new interval. The constructor will reverse the
// inclusion of end endpoint, if appropriate, and build
// the reference list.
MdamInterval * tempIntervalPtr = new(mdamIntervalHeap)
MdamInterval(previousEndPoint,
currentEndPoint,
intervalPtr0,
intervalPtr1,
disjunctNum,
mdamRefListEntryHeap);
// Append the new interval onto this list.
append(tempIntervalPtr);
}; // end if
// Adjust active reference lists.
currentEndPoint.adjustIntervalPtr(intervalPtr0, 0);
currentEndPoint.adjustIntervalPtr(intervalPtr1, 1);
// Save the current endpoint.
previousEndPoint = currentEndPoint;
if (currentEndPoint.end()){previousEndPoint.reverseInclusion();};
}; // end while
#if defined ( NA_MDAM_EXECUTOR_DEBUG_ILTF )
// Log entry for intervals resulting from the union operation.
logEvent(8,countIntervals(),otherList.intervalListId_);
#endif /* NA_MDAM_EXECUTOR_DEBUG_ILTF */
// Delete the original this list entries because the destructor
// can't.
tempIntervalList.deleteAllIntervals(mdamIntervalHeap,
mdamRefListEntryHeap);
return *this;
}
/* Start of main progam */
int main(int argc, char *argv[])
{
char szOut[255], szImg1[255], szImg2[255], *maskFile;
int x1, x2, y1, y2;
int goodPoints=0, attemptedPoints=0;
FILE *fp_output;
meta_parameters *masterMeta, *slaveMeta;
int logflag=FALSE, ampFlag=TRUE, currArg=1;
while (currArg < (argc-NUM_ARGS)) {
char *key = argv[currArg++];
if (strmatches(key,"-log","--log",NULL)) {
CHECK_ARG(1);
strcpy(logFile,GET_ARG(1));
fLog = FOPEN(logFile, "a");
logflag = TRUE;
}
else if (strmatches(key,"-mask","--mask",NULL)) {
CHECK_ARG(1);
maskFile = GET_ARG(1);
}
else {
printf("\n**Invalid option: %s\n", argv[currArg-1]);
usage(argv[0]);
}
}
if ((argc-currArg) < NUM_ARGS) {
printf("Insufficient arguments.\n");
usage(argv[0]);
}
// Fetch required arguments
strcpy(szImg1,argv[argc - 3]);
strcpy(szImg2,argv[argc - 2]);
strcpy(szOut, argv[argc - 1]);
/* Read metadata */
masterMeta = meta_read(szImg1);
slaveMeta = meta_read(szImg2);
if (masterMeta->general->line_count != slaveMeta->general->line_count ||
masterMeta->general->sample_count != slaveMeta->general->sample_count)
asfPrintWarning("Input images have different dimension!\n");
else if (masterMeta->general->data_type != slaveMeta->general->data_type)
asfPrintError("Input image have different data type!\n");
else if (masterMeta->general->data_type > 5 &&
masterMeta->general->data_type != COMPLEX_REAL32)
asfPrintError("Cannot compare raw images for offsets!\n");
lines = masterMeta->general->line_count;
samples = masterMeta->general->sample_count;
if (masterMeta->general->data_type == COMPLEX_REAL32) {
srcSize = 32;
ampFlag = FALSE;
cZero = Czero();
}
/* Create output file */
fp_output=FOPEN(szOut, "w");
/* Loop over grid, performing forward and backward correlations */
while (getNextPoint(&x1,&y1,&x2,&y2))
{
float dx=0.0, dy=0.0, snr=0.0, dxFW, dyFW, snrFW, dxBW, dyBW, snrBW;
attemptedPoints++;
// Check bounds and mask
if (!(outOfBounds(x1, y1, srcSize, maskFile)))
{
/* ...check forward correlation... */
if (ampFlag) {
if (!(findPeak(x1,y1,szImg1,x2,y2,szImg2,&dxFW,&dyFW,&snrFW))) {
attemptedPoints--;
continue; /* next point if chip in complete background fill */
}
}
else {
getPeak(x1,y1,szImg1,x2,y2,szImg2,&dxFW,&dyFW,&snrFW);
}
if ((!ampFlag && snrFW>minSNR) || (ampFlag)) {
/* ...check backward correlation... */
if (ampFlag) {
if (!(findPeak(x2,y2,szImg2,x1,y1,szImg1,&dxBW,&dyBW,&snrBW))) {
attemptedPoints--;
continue; /* next point if chip in complete background fill */
printf("dxFW: %.2f, dyFW: %.2f\n", dxFW, dyFW);
}
}
else {
getPeak(x2,y2,szImg2,x1,y1,szImg1,&dxBW,&dyBW,&snrBW);
}
dxBW*=-1.0;dyBW*=-1.0;
if (((!ampFlag && snrFW>minSNR) || (ampFlag)) &&
(fabs(dxFW-dxBW) < maxDisp) &&
(fabs(dyFW-dyBW) < maxDisp))
{
dx = (dxFW+dxBW)/2;
dy = (dyFW+dyBW)/2;
snr = snrFW*snrBW;
if (dx < maxDxDy && dy < maxDxDy) goodPoints++;
}
}
fprintf(fp_output,"%6d %6d %8.5f %8.5f %4.2f\n",
x1, y1, x2+dx, y2+dy, snr);
fflush(fp_output);
}
}
if (goodPoints < attemptedPoints)
printf("\n WARNING: %i out of %i points moved by "
"more than one pixel!\n\n",
(attemptedPoints-goodPoints), attemptedPoints);
else
printf("\n There is no difference between the images\n\n");
FCLOSE(fp_output);
FREE(masterMeta);
FREE(slaveMeta);
return(0);
}
