int BiBfs(int disccnt, int begin, int end) {
if (begin == end)
return 0;
std::queue<int> q;
memset(C, 0, sizeof(C));
q.push(begin);
C[begin] = 1;
q.push(end);
C[end] = -1;
while (!q.empty()) {
int here = q.front();
q.pop();
int top[4] = { -1, -1, -1, -1 };
for (int i = disccnt-1; i >= 0; --i) {
top[Get(here, i)] = i;
}
for (int i = 0; i < 4; ++i) {
if (top[i] != -1) {
for (int j = 0; j < 4; ++j) {
if (i != j && (top[j] == -1 || top[j] > top[i])) {
int there = Set(here, top[i], j);
if (C[there] == 0) {
C[there] = Incr(C[here]);
q.push(there);
} else if (GetSign(C[there]) != GetSign(C[here])) {
return abs(C[there]) + abs(C[here]) -1;
}
}
}
}
}
}
return -1;
}
mitk::pa::Vessel::Pointer mitk::pa::Vessel::Bifurcate(std::mt19937* rng)
{
VesselProperties::Pointer vesselParams = VesselProperties::New(m_VesselProperties);
double thetaChange = m_RangeDistribution(*rng) * GetSign(rng);
double phiChange = m_RangeDistribution(*rng) * GetSign(rng);
vesselParams->GetDirectionVector()->Rotate(thetaChange, phiChange);
m_VesselProperties->GetDirectionVector()->Rotate(-thetaChange, -phiChange);
double newRadius = m_RadiusRangeDistribution(*rng)*m_VesselProperties->GetRadiusInVoxel();
vesselParams->SetRadiusInVoxel(newRadius);
m_VesselProperties->SetRadiusInVoxel(
sqrt(m_VesselProperties->GetRadiusInVoxel()*m_VesselProperties->GetRadiusInVoxel() - newRadius*newRadius));
m_WalkedDistance = 0;
return Vessel::New(vesselParams);
}
/****************************************
* ManualLift:
* Input = The motor for the lift (Victors only(?))
* Data from Joystick 2
* Output = Scissor lift movement
* false = Error of some kind (probably passed from ScissorPreset)
* true = Everything is just dandy.
* TURN_TRANSFORM- Distance * TURN_TRANSFORMS == amount of motor turns
* Victor turns at rate of [-1.0 to 1.0]
*
* TODO:
* - Test code
* - Test MAXIMUM_TURN, calibrate numbers
* - Use limit switches for max or min, not dead reckoning.
*/
bool ManualLift(GenericHID *liftStick)
{
bool result;
isDoingPreset = lift->isPresetSelected (liftStick);
// If presets were not overriden, continue moving.
if (isDoingPreset)
{
int valueReturned = lift->moveToPreset();
if (1 == valueReturned) {
isDoingPreset = false;
} else if (-1 == valueReturned){
result = false; // Error
}
}
else
{
// Pushing joystick up returns a negative Y-value, oddly.
float userInput = -liftStick->GetY();
SmartDashboard::Log(userInput, "absoluteInput: ");
userInput = (fabs(userInput) > 1.0) ? GetSign(userInput) : userInput;
// Override any presets.
isDoingPreset = false;
// Positive = counter-clockwise spin (when facing the motor)
// Counter-clockwise = rise.
// Luckily, a positive value corresponds with a rise in height(?).
if (userInput > 0.05)
result = lift->retract(fabs(userInput));
else if (userInput < -0.05)
result = lift->extend(fabs(userInput));
else
result = lift->stop();
SmartDashboard::Log(userInput, "userInput: ");
}
return result;
}
TRISTATE CTextParser::GetDigits(unsigned long long int* pulli, int* piSign, int* piNumDigits, BOOL bSkipWhiteSpace, BOOL bTestSign)
{
unsigned long long int iNum;
int iSign;
int iTemp;
TRISTATE tReturn;
BOOL bFirstDigit;
int i;
//This still needs to be failed on the case where the number is larger than MAX_ULONG.
PushPosition();
if (bSkipWhiteSpace)
{
SkipWhiteSpace();
}
*pulli = 0;
i = 0;
if (!mbOutsideText)
{
iNum = 0;
if (bTestSign)
GetSign(&iSign);
else
iSign = 1;
bFirstDigit = TRUE;
for (;;)
{
if (!mbOutsideText)
{
tReturn = GetDigit(&iTemp);
if (tReturn == TRITRUE)
{
i++;
iNum *= 10;
iNum += iTemp;
}
else if ((tReturn == TRIFALSE) || (tReturn == TRIERROR))
{
break;
}
bFirstDigit = FALSE;
}
else
{
break;
}
}
if (bFirstDigit)
{
//might already have got a sign... so reset the parser.
PopPosition();
return TRIFALSE;
}
else
{
*piSign = iSign;
*pulli = iNum;
if (piNumDigits)
{
*piNumDigits = i;
}
PassPosition();
return TRITRUE;
}
}
else
{
PopPosition();
SetErrorEndOfFile();
return TRIERROR;
}
}
/////////////////////////////////////////////////////////
// processImage
//
/////////////////////////////////////////////////////////
void pix_refraction :: processRGBAImage(imageStruct &image)
{
nWidth = image.xsize;
nHeight = image.ysize;
if (!init) {
init = 1;
}
pSource = (U32*)image.data;
myImage.xsize = image.xsize;
myImage.ysize = image.ysize;
myImage.setCsizeByFormat(image.format);
myImage.reallocate();
pOutput = (U32*)myImage.data;
const int nHalfWidth=(nWidth/2);
const int nHalfHeight=(nHeight/2);
const int nNumPixels = nWidth*nHeight;
float GatedRefraction=m_Refraction;
if ((m_DoAllowMagnification==0.0f)&&(m_Refraction<1.0f)) {
GatedRefraction=1.0f;
}
const int nRefraction=static_cast<int>(GatedRefraction*256.0f);
int nCellWidth=static_cast<int>(m_CellWidth);
if (nCellWidth<=0) {
nCellWidth=1;
}
int nCellHeight=static_cast<int>(m_CellHeight);
if (nCellHeight<=0) {
nCellHeight=1;
}
const int nHalfCellWidth=(nCellWidth/2);
const int nHalfCellHeight=(nCellHeight/2);
//const int nYStartOffset=(nHalfCellHeight*(256))/nRefraction;
//const int nXStartOffset=(nHalfCellWidth*(256))/nRefraction;
U32* pCurrentOutput=pOutput;
const U32* pOutputEnd=(pOutput+nNumPixels);
int nY=-nHalfHeight+nHalfCellHeight;
while (pCurrentOutput!=pOutputEnd) {
//const U32* pOutputLineStart=pCurrentOutput;
const U32* pOutputLineEnd=pCurrentOutput+nWidth;
const int nYCentre=(((nY+(GetSign(nY)*nHalfCellHeight))/nCellHeight)*nCellHeight)+nHalfCellHeight;
const int nYDist=(nY-nYCentre);
int nSourceY=((nYDist*nRefraction)>>8)+nYCentre+nHalfHeight;
nSourceY=clampFunc(nSourceY,0,(nHeight-1));
const U32* pSourceLineStart=(pSource+(nSourceY*nWidth));
int nX=-nHalfWidth+nHalfCellWidth;
while (pCurrentOutput!=pOutputLineEnd) {
const int nXCentre=(((nX+(GetSign(nX)*nHalfCellWidth))/nCellWidth)*nCellWidth)+nHalfCellWidth;
const int nXDist=(nX-nXCentre);
int nSourceX=((nXDist*nRefraction)>>8)+nXCentre+nHalfWidth;
nSourceX=clampFunc(nSourceX,0,(nWidth-1));
const U32* pCurrentSource=pSourceLineStart+nSourceX;
*pCurrentOutput=*pCurrentSource;
pCurrentOutput+=1;
nX+=1;
}
nY+=1;
}
image.data = myImage.data;
}
bool IsPositive(float number)
{
return GetSign(number) == cPositive;
}
bool IsNegative(float number)
{
return GetSign(number) == cNegative;
}