float FVoronoiDiagramSite::GetDistanceFrom(TSharedPtr<IVoronoiDiagramPoint, ESPMode::ThreadSafe> Point)
{
float dx, dy;
dx = GetCoordinate().X - Point->GetCoordinate().X;
dy = GetCoordinate().Y - Point->GetCoordinate().Y;
return FMath::Sqrt(dx * dx + dy * dy);
}
void DialogRepositionSurface::OnApply()
{
LayerSurface* surf = (LayerSurface*)MainWindow::GetMainWindow()->GetActiveLayer("Surface");
LayerMRI* mri = (LayerMRI*)MainWindow::GetMainWindow()->GetActiveLayer("MRI");
QString msg;
if ( !surf )
msg = "No active surface found.";
else if ( !mri && ui->tabWidget->currentIndex() == 0)
msg = "No active volume found.";
if (!msg.isEmpty())
{
QMessageBox::warning(this, "Error", msg);
return;
}
if (ValidateAll())
{
ui->pushButtonApply->setDisabled(true);
if (ui->tabWidget->currentIndex() == 0)
{
if (ui->comboBoxTarget->currentIndex() == 0)
{
surf->RepositionSurface(mri, GetVertex(),
GetIntensity(),
GetNeighborSize(),
GetSigma(),
GetFlags());
}
else
{
double pos[3];
GetCoordinate(pos);
surf->RepositionSurface( mri, GetVertex(),
pos,
GetNeighborSize(),
GetSigma(),
GetFlags());
}
}
else if (ui->tabWidget->currentIndex() == 1)
{
double pos[3];
GetCoordinate(pos);
surf->RepositionVertex(GetVertex(), pos);
}
else
{
surf->RepositionSmoothSurface(GetVertex(), GetNeighborSize(), GetSmoothingSteps());
}
UpdateUI();
ui->pushButtonApply->setDisabled(false);
QTimer::singleShot(0, MainWindow::GetMainWindow(), SIGNAL(SlicePositionChanged()));
}
}
int reachLine(float x, float y, float speed){
typeCoordinate nowPos = GetCoordinate();
if(nowPos.x >= x)
return 1;
else
return 0;
}
int SleepUntilSafe2(int frontR, float length){
u8 flag = 0;
typeCoordinate nowp = GetCoordinate();
typeCoordinate front;
#ifdef VISIT_DEMO
/**************for visitor demo******************/
if(activeRb[2] == rbID){
if(bCastInfo[activeRb[1] - 1].isStop){
return flag;
}
}
/**************for visitor demo******************/
#endif
if((frontR >= 1) && (frontR < ROBOTS))
{
front.x = bCastInfo[frontR - 1].rpos.locationX;
front.y = bCastInfo[frontR - 1].rpos.locationY;
float dist = getDistance2(nowp.x, nowp.y, front.x, front.y);
if(dist < (length + 0.04)){
flag++;
//halSetLedStatus(LED_BLUE, LED_ON);
}
}
halt(flag*4);
return flag;
}
bool GoalInFront(float x, float y,int phase){
typeCoordinate nowp = GetCoordinate();
float dist = getDistance2(nowp.x, nowp.y, x,y);
if( dist < MIN_RANGE)
return true;
else
return false;
}
void CResizer::Move(int nIndex) const{
const CControlInfo &ci = m_vInfo[nIndex];
CRectInfo &ri = m_vRectInfo[nIndex];
m_nCachedSize = nIndex + 1; //Now m_vRectInfo contains nIndex + 1 valid items
ri.nID = ci.pInfo->nID;
RECT &rc = ri.rc;
rc.left = GetCoordinate(eLeft, ci.rcInitial, ci.pInfo->left, rc);
rc.top = GetCoordinate(eTop, ci.rcInitial, ci.pInfo->top, rc);
rc.right = GetCoordinate(eRight, ci.rcInitial, ci.pInfo->right, rc);
rc.bottom = GetCoordinate(eBottom, ci.rcInitial, ci.pInfo->bottom, rc);
HWND pCtl = GetDlgItem(ci.pInfo->nID);
LONG dwStyle = ::GetWindowLong(pCtl, GWL_STYLE);
ri.bVisible = (::IsWindowVisible(pCtl) != FALSE && (dwStyle&WS_CLIPSIBLINGS) == 0);
ri.bHide = false;
}
void runStraight(float x, float y, float speed, int flag) {
float speedL = speed;
float speedR = speed;
typeCoordinate start = GetCoordinate();
typeCoordinate nowp = GetCoordinate();
while (1) {
int side = whichSide(start.x,start.y, x, y, nowp.x, nowp.y);
float dist = getDistance2(nowp.x, nowp.y, x, y);
while(1){
if(sleepSafe(FRONT_SIGHT_LENGTH) == 0)
break;
}
while(dist <= 0.10){
rotateTo(x,y,ANGLESPEED,1);
SetLeftWheelGivenSpeed(speedL);
SetRightWheelGivenSpeed(speedR);
vTaskDelay(300);
if(GoalInFront(x,y) == true)
return;
}
if (side == -2 || dist < EPS) break;
if (side == -1) { // right side
speedL = speed;
speedR = speed+5;
} else if (side == 1) { // left side
speedL = speed+5;
speedR = speed;
} else {
speedL = speedR = speed;
}
SetLeftWheelGivenSpeed(speedL);
SetRightWheelGivenSpeed(speedR);
vTaskDelay(300);
nowp = GetCoordinate();
}
#ifdef DEBUG
halt(2);
#endif
}
////////////////////////////////////////////////////////////////////////////////
//check collision between robots
////////////////////////////////////////////////////////////////////////////////
bool ObjectInFront(float dir,typeCoordinate coordinate,float *interAngle,float length,int *leftOrRight) {
float myDir;
typeCoordinate myCoordinate;
float disRobot2Robot;
float lineDir;
float leftAngle,rightAngle,leftAngle1,rightAngle1;
myDir = ReadMagSensorAngle2North();
myCoordinate = GetCoordinate();
disRobot2Robot = sqrt((myCoordinate.x-coordinate.x)*(myCoordinate.x-coordinate.x)+(myCoordinate.y-coordinate.y)*(myCoordinate.y-coordinate.y));
lineDir = GetLineDirection(myCoordinate.x, myCoordinate.y, coordinate.x, coordinate.y);
*interAngle = r2rAngle(lineDir,myDir);
if (disRobot2Robot < length) {
leftAngle = myDir - FOV; //when -180 < myDir < -180-FOV below -180,eg: -230;
rightAngle = myDir + FOV;
if(leftAngle < -180){
leftAngle = -180;
rightAngle1 = 180;
leftAngle1 = 180 - (FOV - (180 + myDir));
}
if(rightAngle > 180){
rightAngle = 180;
leftAngle1 = -180;
rightAngle1 = -180 + (FOV - (180-myDir));
}
if ((lineDir > leftAngle) && (lineDir < rightAngle)) {
if((leftAngle < lineDir) && leftAngle <= myDir){
*leftOrRight = leftHand;
}
if((myDir <= lineDir) && lineDir < rightAngle){
*leftOrRight = rightHand;
}
return true;
}
if ((lineDir >= leftAngle1) && (lineDir <= rightAngle1)) {
if((leftAngle < lineDir) && leftAngle <= myDir){
*leftOrRight = leftHand;
}
if((myDir <= lineDir) && lineDir < rightAngle){
*leftOrRight = rightHand;
}
return true;
}
}
return false;
}
//如果前面有个机器人在距离自己13mm范围内，则暂停行驶 2'
int SleepUntilFrontSafe(u8 index, float length){
u8 maxIndex = index;
u8 flag = 0;
typeCoordinate nowp = GetCoordinate();
typeCoordinate front;
if(maxIndex == 0)
return 0;
u8 frontR = activeRb[maxIndex - 1];
front.x = bCastInfo[frontR - 1].rpos.locationX;
front.y = bCastInfo[frontR - 1].rpos.locationY;
float dist = getDistance2(nowp.x, nowp.y, front.x, front.y);
if(dist < (length)){
flag++;
halt(flag*2 + 3);
}
return flag;
}
void rotateTo(float x, float y, float speed,int flag) {
typeCoordinate start = GetCoordinate();
// float lineDir = GetLineDirection(start.x, start.y, x, y);
// float robotDir = CalibrateNorth2_Y();
float lineDir = GetLineDirectionX(start.x, start.y, x, y);
float robotDir = CalibrateNorth2X();
float turnangle = lineDir - robotDir;
if (turnangle < -180) turnangle += 360 ;
if (turnangle > 180) turnangle -= 360;
if(flag == ROTATE_LARGER_15)
{
if(FloatAbs(turnangle) <= 15)
return;
}
if(flag == ROTATE_LARGER_25) //used for runStraight
{
if(FloatAbs(turnangle) <= 25)
return;
}
ControlRobotRotate(turnangle, speed);
}
int whichSide(float x, float y){
typeCoordinate start = GetCoordinate();
// typeCoordinate start = getNowPos();
// float lineDir = GetLineDirection(start.x, start.y, x, y);
// float robotDir = CalibrateNorth2_Y();
float lineDir = GetLineDirectionX(start.x, start.y, x, y);
float robotDir = CalibrateNorth2X();
float turnangle = lineDir - robotDir;
if (turnangle < -180) turnangle += 360;
if (turnangle > 180) turnangle -= 360;
if(FloatAbs(turnangle) <= 25){
if(turnangle > 0)
return 1; //right side
if(turnangle < 0)
return -1; //left side
}
if(FloatAbs(turnangle) > 25){
ControlRobotRotate(turnangle, MAX_ROTATE_SPEED);
}
return 0; // on the line
}
void northRotate(float x, float y, float speed){
static typeCoordinate coordinate;
float lineDir;
float robotDir;
//1. calculate coordinate
coordinate = GetCoordinate();
//2. calculate direction of line
lineDir = GetLineDirection(coordinate.x, coordinate.y, x, y);
//3. turn dir to line
robotDir = ReadMagSensorAngle2North();
if ((lineDir-robotDir)<-180) {
ControlRobotRotate(lineDir-robotDir+360-20, 5);
}
else if ((lineDir-robotDir)>180) {
ControlRobotRotate(lineDir-robotDir-360+20, 5);
}
else {
ControlRobotRotate(lineDir-robotDir, 5);
}
}
void DialogRepositionSurface::OnApply( wxCommandEvent& event )
{
LayerSurface* surf = (LayerSurface*)MainWindow::GetMainWindowPointer()->GetActiveLayer( "Surface" );
LayerMRI* mri = (LayerMRI*)MainWindow::GetMainWindowPointer()->GetActiveLayer( "MRI" );
wxString msg;
if ( !surf )
msg = _("No active surface found.");
else if ( !mri )
msg = _("No active volume found." );
if ( !msg.IsEmpty() )
{
wxMessageDialog dlg( this, msg, _("Error"), wxOK );
dlg.ShowModal();
return;
}
if ( ValidateAll() )
{
if ( m_choiceTarget->GetCurrentSelection() == 0 )
{
surf->RepositionSurface( mri, GetVertex(),
GetIntensity(),
GetNeighborSize(),
GetSigma() );
}
else
{
double pos[3];
GetCoordinate( pos );
surf->RepositionSurface( mri, GetVertex(),
pos,
GetNeighborSize(),
GetSigma() );
}
UpdateUI();
}
}
void GraphicCamera::SetCenterOfFocus(const Vec3d& new_center) {
Vec3d dif = new_center - aim_;
GetCoordinate(pos_ + dif, new_center, up_);
}
int main(void)
{
int input = -1;
int smileyTypeInput = -1;
int smileyHealthInput = -1;
int xCoordinateInput = -1;
int yCoordinateInput = -1;
int turns = 0;
Smiley* board[BOARD_SIZE][BOARD_SIZE];
// Set up board
InitializeSmileyBoard(board);
srand(time(0));
PrintSmileyDemo();
// Main loop; continues until user exits program
while (input != 0)
{
PrintBoard(board);
// Prompt for initial user input
printf("Please choose from the following:\n");
printf("1: Add a smiley to the board\n");
printf("2: Do nothing this turn\n");
printf("0: Exit\n");
input = GetIntegerInput();
switch (input)
{
// Add smiley to the board
case 1:
// Smiley type
smileyTypeInput = GetSmileyType();
// If type is -1, then cancel operation
if (smileyTypeInput == -1)
{
break;
}
// Smiley health
smileyHealthInput = GetSmileyHealth();
// If health is -1, then cancel operation
if (smileyHealthInput == -1)
{
break;
}
// Coordinates
GetCoordinate(&xCoordinateInput, 'x');
GetCoordinate(&yCoordinateInput, 'y');
// If the given coordinates are empty and valid, then place the Smiley there
if ((IsValidCoordinatePair(xCoordinateInput, yCoordinateInput)) &&
(board[yCoordinateInput][xCoordinateInput] == NULL))
{
board[yCoordinateInput][xCoordinateInput] = CreateSmiley((SmileyType) smileyTypeInput,
(SmileyHealth) smileyHealthInput);
}
// Else display error message and cancel operation
else
{
printf("There is already a Smiley at (%d, %d).\n", xCoordinateInput, yCoordinateInput);
}
break;
case 2:
printf("You wait.\n");
break;
case 0:
continue;
default:
break;
}
IterateBoard(board);
turns++;
}
FreeSmileysOnBoard(board);
printf("Press ENTER to continue");
getchar();
exit(0);
}
void ControlRobotgo2Position(float x, float y, float speed,int flag) {
//float speedL = speed;
//float speedR = speed;
typeCoordinate start = GetCoordinate();
if(GoalInFront(x,y,0) == true)
return;
SetLeftWheelGivenSpeed(1);
SetRightWheelGivenSpeed(1);
vTaskDelay(500);
if(GoalInFront(x,y,0) == true)
return;
SetLeftWheelGivenSpeed(3);
SetRightWheelGivenSpeed(3);
vTaskDelay(500);
if(GoalInFront(x,y,0) == true)
return;
rotateToSafe(x,y);
SetLeftWheelGivenSpeed(5);
SetRightWheelGivenSpeed(5);
vTaskDelay(500);
if(GoalInFront(x,y,0) == true)
return;
rotateToSafe(x,y);
SetLeftWheelGivenSpeed(10);
SetRightWheelGivenSpeed(10);
vTaskDelay(300);
if(GoalInFront(x,y,0) == true)
return;
rotateToSafe(x,y);
SetLeftWheelGivenSpeed(15);
SetRightWheelGivenSpeed(15);
vTaskDelay(200);
if(GoalInFront(x,y,0) == true)
return;
rotateToSafe(x,y);
SetLeftWheelGivenSpeed(20);
SetRightWheelGivenSpeed(20);
rotateToSafe(x,y);
if(GoalInFront(x,y,0) == true)
return;
u8 i = 0;
while (1) {
for(i = 0;i < CHECK; i++){
rotateToSafe(x,y);
SetLeftWheelGivenSpeed(30);
SetRightWheelGivenSpeed(30);
vTaskDelay(300);
if(GoalInFront(x,y,0) == true)
return;
}
rotateTo(x,y,FASTSPEED,ROTATE_LARGER_15);
//go to target point
SetLeftWheelGivenSpeed(30);
SetRightWheelGivenSpeed(30);
vTaskDelay(300);
if(GoalInFront(x,y,0) == true)
return;
} /* end of while */
} /*end of ControlRobotgo2Position*/
////////////////////////////////////////////////////////////////////////////////
//angle: control Robot go from current position to given position
////////////////////////////////////////////////////////////////////////////////
void ControlRobot2Position(float x, float y, float speed){
static typeCoordinate coordinate;
float speedL, speedR;
//float calY;
float lineDir;
float robotDir;
speedL = speed;
speedR = speed;
//1. calculate coordinate
coordinate = GetCoordinate();
//2. y=kx+b, calculate line para k and b
float k, b;
k = (y-coordinate.y)/(x-coordinate.x);
b = y-k*x;
//3. calculate direction of line
lineDir = GetLineDirection(coordinate.x, coordinate.y, x, y);
//4. turn dir to line
robotDir = ReadMagSensorAngle2North();
float turnangle = lineDir - robotDir;
while (turnangle < 0) turnangle += 360;
while (turnangle >= 360) turnangle -= 360;
if (turnangle > 180) turnangle = 360 - turnangle;
ControlRobotRotate(turnangle, 5);
/*
if ((lineDir-robotDir)<-180) {
ControlRobotRotate(lineDir-robotDir+360-20, 5);
}
else if ((lineDir-robotDir)>180) {
ControlRobotRotate(lineDir-robotDir-360+20, 5);
}
else {
ControlRobotRotate(lineDir-robotDir, 5);
}
*/
SetLeftWheelGivenSpeed(1);
SetRightWheelGivenSpeed(1);
vTaskDelay(500);
SetLeftWheelGivenSpeed(3);
SetRightWheelGivenSpeed(3);
vTaskDelay(500);
SetLeftWheelGivenSpeed(5);
SetRightWheelGivenSpeed(5);
vTaskDelay(500);
SetLeftWheelGivenSpeed(10);
SetRightWheelGivenSpeed(10);
vTaskDelay(300);
SetLeftWheelGivenSpeed(15);
SetRightWheelGivenSpeed(15);
vTaskDelay(300);
SetLeftWheelGivenSpeed(20);
SetRightWheelGivenSpeed(20);
vTaskDelay(1000);
coordinate = GetCoordinate();
while (RobotInRange(coordinate.x,coordinate.y, x, y)==false){
/*
if (coordinate.y>(k*coordinate.x+b)) {
speedL = 25;
speedR = 20;
}
else {
speedL = 20;
speedR = 25;
}
*/
if (coordinate.y>(k*coordinate.x+b)+0.01) {
speedL = 23;
speedR = 20;
}
else if (coordinate.y+0.0<(k*coordinate.x+b)) {
speedL = 20;
speedR = 23;
}
else {
robotDir = ReadMagSensorAngle2North();
if ((lineDir-robotDir)>20){
speedL = 20;
speedR = 23;
}
else if ((lineDir-robotDir)<-20){
speedL = 23;
speedR = 20;
}
else{
speedL = 20;
speedR = 20;
}
}
SetLeftWheelGivenSpeed(speedL);
SetRightWheelGivenSpeed(speedR);
vTaskDelay(500);
coordinate = GetCoordinate();
}
SetLeftWheelGivenSpeed(0);
SetRightWheelGivenSpeed(0);
vTaskDelay(100000);
asm("NOP");
}
