void Encoder_runSM(){
pitchAngle = calculateAngle(zeroPitchAngle,SLAVE_VERTICAL_READ_ADDRESS,
SLAVE_VERTICAL_WRITE_ADDRESS);
yawAngle = calculateAngle(zeroYawAngle,SLAVE_HORIZONTAL_READ_ADDRESS,
SLAVE_HORIZONTAL_WRITE_ADDRESS);
}
double calculateGoalProbability(struct RoboAI *ai, int player) {
//guestimate in seconds
double TURN_TIME = 1.5;
struct vector playerDirection, playerToBall, playerGoalToBall, goalToPlayer, playerBallToGoal;
calculateInitialVectors(&playerDirection, &playerToBall, &playerGoalToBall, &goalToPlayer, ai, player);
playerBallToGoal.x = -playerGoalToBall.x;
playerBallToGoal.y = -playerGoalToBall.y;
// Breakdown:
// time to reach ball is composed of dist to ball, if heading to ball matches angle to ball, and the angle between ball to goal and self to ball
// it's the distance,what else would it be?
double playerDist = magnitude(&playerToBall);
// if heading to ball matches angle to ball(facing the ball)
double playerAngleToBall = calculateAngle(&playerToBall, &playerDirection) / (180.0 * TURN_TIME);
// if vector to ball and ball to goal are similiar(directions agree)
// if vector to ball and ball to goal disagree, we need to reposition behind the ball,
// this is expensive, so we should penalize more on this
double repositionAngle = calculateAngle(&playerToBall, &playerBallToGoal) / 180.0 * TURN_TIME * 5;
//smaller values are better
return playerDist + playerAngleToBall + repositionAngle;
}
/**
Update the average position from the cumulative average position. Basically
divide all relevant cumulative values by the number of entries in the list.
@param positionAverageList
The position average list
*/
static void updatePositionAverageFromCumulative(
PositionAverageList * positionAverageList) {
double divider = positionAverageList->entriesCount;
positionAverageList->positionAverage = positionAverageList->positionAverageCumulative;
/* smask: use from cumulative average */
/* utc: use from cumulative average */
/* sig: use from cumulative average */
/* fix: use from cumulative average */
if (divider > 1.0) {
positionAverageList->positionAverage.nmeaInfo.PDOP /= divider;
positionAverageList->positionAverage.nmeaInfo.HDOP /= divider;
positionAverageList->positionAverage.nmeaInfo.VDOP /= divider;
positionAverageList->positionAverage.nmeaInfo.lat /= divider;
positionAverageList->positionAverage.nmeaInfo.lon /= divider;
positionAverageList->positionAverage.nmeaInfo.elv /= divider;
positionAverageList->positionAverage.nmeaInfo.speed /= divider;
positionAverageList->positionAverage.nmeaInfo.track = calculateAngle(&positionAverageList->positionAverageCumulative.track);
positionAverageList->positionAverage.nmeaInfo.mtrack = calculateAngle(&positionAverageList->positionAverageCumulative.mtrack);
positionAverageList->positionAverage.nmeaInfo.magvar = calculateAngle(&positionAverageList->positionAverageCumulative.magvar);
}
/* satinfo: use from average */
}
//x and y in meters
void navigateToWaypoint (float new_x, float new_y)
{
//new_x *= 100;
//new_y *= 100;
float dx = new_x - x;
float dy = new_y - y;
float newAngle = calculateAngle(dx, dy);
float distanceToMove = calculateDistance(dx, dy);
float step = 20;
float moved = 0;
while (distanceToMove > 0)
{
dx = new_x - x;
dy = new_y - y;
newAngle = calculateAngle(dx, dy);
distanceToMove = calculateDistance(dx, dy);
turnNDegrees(newAngle);
moved = step < distanceToMove ? step : distanceToMove;
moveForward(moved);
//distanceToMove = distanceToMove - moved;
}
}
//! Draw any parts on the screen which are for our module
void AngleMeasure::draw(StelCore* core)
{
if (lineVisible.getInterstate() < 0.000001f && messageFader.getInterstate() < 0.000001f)
return;
const StelProjectorP prj = core->getProjection(StelCore::FrameEquinoxEqu);
StelPainter painter(prj);
painter.setFont(font);
if (lineVisible.getInterstate() > 0.000001f)
{
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_BLEND);
glEnable(GL_TEXTURE_2D);
Vec3d xy;
QString displayedText;
if (prj->project(perp1EndPoint,xy))
{
painter.setColor(textColor[0], textColor[1], textColor[2], lineVisible.getInterstate());
if (flagShowPA)
displayedText = QString("%1 (%2%3)").arg(calculateAngle(), messagePA, calculatePositionAngle(startPoint, endPoint));
else
displayedText = calculateAngle();
painter.drawText(xy[0], xy[1], displayedText, 0, 15, 15);
}
glDisable(GL_TEXTURE_2D);
// OpenGL ES 2.0 doesn't have GL_LINE_SMOOTH
// glEnable(GL_LINE_SMOOTH);
glEnable(GL_BLEND);
// main line is a great circle
painter.setColor(lineColor[0], lineColor[1], lineColor[2], lineVisible.getInterstate());
painter.drawGreatCircleArc(startPoint, endPoint, NULL);
// End lines
painter.drawGreatCircleArc(perp1StartPoint, perp1EndPoint, NULL);
painter.drawGreatCircleArc(perp2StartPoint, perp2EndPoint, NULL);
}
if (messageFader.getInterstate() > 0.000001f)
{
painter.setColor(textColor[0], textColor[1], textColor[2], messageFader.getInterstate());
int x = 83;
int y = 120;
int ls = painter.getFontMetrics().lineSpacing();
painter.drawText(x, y, messageEnabled);
y -= ls;
painter.drawText(x, y, messageLeftButton);
y -= ls;
painter.drawText(x, y, messageRightButton);
}
}
void OrthographicCamera::findDirectionForIsoView(float targetAngle, float epsilon, int maxIterations)
{
float start = targetAngle - 5;
float end = targetAngle + 5;
float mid = targetAngle;
int iterations = 0;
float aMid = 0;
while(fabs(targetAngle - aMid) > epsilon && iterations++ < maxIterations)
{
aMid = calculateAngle(mid);
if(targetAngle < aMid)
{
end = mid;
}
else
{
start = mid;
}
mid = start + (end - start) / 2;
}
calculateDirection(mid, position);
position.y = -position.y;
lookAt(0, 0, 0);
normalizeUp();
}
Gesture::Detected RotateDesktopGesture::isRecognizedImpl(GestureContext *gestureContext)
{
_gestureTouchPaths = gestureContext->getActiveTouchPaths();
if (sel->isInSelection(_gestureTouchPaths[0]->getFirstTouchPoint().getPickedObject()))
return gestureRejected("First finger is on an object");
if (sel->isInSelection(_gestureTouchPaths[1]->getFirstTouchPoint().getPickedObject()))
return gestureRejected("Second finger is on an object");
float angle = calculateAngle(gestureContext);
if (abs(angle) > MINIMUM_GESTURE_ANGLE)
{
Vec3 camPos, camDir;
int wallIndex = getWallCameraIsFacing();
assert(wallIndex != -1);
cam->lookAtWall(GLOBAL(Walls)[wallIndex], camPos, camDir);
// make camera face the "forward" wall, so rotation would be to the sides
if ((cam->getEye() - camPos).magnitude() > 0.5f)
cam->animateTo(camPos, camDir, Vec3(0, 1, 0), 10);
_animatingToForward = true;
_startDir = cam->getDir();
_startUp = cam->getUp();
_startPos = cam->getEye();
_recalculateInitials = false;
return gestureAccepted();
}
return Maybe;
}
void Methods::signAlign(Mat dst)
{
//find angle
//Image(Mat*image, int yu, int yl, int xl, int xr, Point2f temp);
Mat temp1,temp2;
temp1 = dst(Rect(0, 0, dst.cols / 2, dst.rows));
temp2 = dst(Rect(dst.cols / 2, 0, dst.cols / 2, dst.rows));
Image temp_image1(&temp1, 0, temp1.rows, 0, temp1.cols, Point2f(0, 0));
Image temp_image2(&temp2, 0, temp2.rows, 0, temp2.cols, Point2f(0, 0));
cout << "BEFORE\n";
cout << temp_image1.x_left << "-Upper down-" << temp_image1.y_lower<<" ";
cout << temp_image2.x_left << "-Upper down-" << temp_image2.y_lower;
CropImage(&temp_image1, 20);
CropImage(&temp_image2, 20);
cout << "\nAFTER\n";
cout << temp_image1.x_left << "-Upper down-" << temp_image1.y_lower << " ";
cout << temp_image2.x_left << "-Upper down-" << temp_image2.y_lower << "\n\n";
//float angle1=calculateAngle(Point2f(temp_image1.x_left, temp_image1.y_upper), Point2f(temp_image2.x_left, temp_image2.y_upper));
float angle2 = calculateAngle(Point2f(0, temp_image1.y_lower), Point2f(1, temp_image2.y_lower));
namedWindow("temp1", CV_WINDOW_NORMAL);
namedWindow("temp2", CV_WINDOW_NORMAL);
imshow("temp1", temp1);
imshow("temp2", temp2);
waitKey(0);
cout << angle2<<"\n\n";
double angle = angle2;
rotateByAngle(dst, angle);
}
void KisToolPan::continuePrimaryAction(KoPointerEvent *event)
{
Q_ASSERT(canvas());
Q_ASSERT(canvas()->canvasController());
QPointF actualPosition = convertDocumentToWidget(e->point);
adjustCursor();
if (!e->buttons())
return;
e->accept();
if(e->modifiers() & Qt::ShiftModifier) {
if(!isInCheckerArea(actualPosition)) {
qreal angle = calculateAngle(m_lastPosition, actualPosition);
kritaCanvasController()->rotateCanvas(angle);
}
}
else {
QPointF distance(m_lastPosition - actualPosition);
kritaCanvasController()->pan(distance.toPoint());
}
m_lastPosition = actualPosition;
}
void Server::drawBrick() const
{
ofSetColor(ofColor::red);
//Draw a cross in brick position
Vertex aux0 = brickPosition*4, aux1 = brickPosition*4;
aux0[X] = aux0[X] - 10;
aux1[X] = aux1[X] + 10;
Polygon::drawLine(aux0, aux1);
aux0 = brickPosition*4;
aux1 = brickPosition*4;
aux0[Y] = aux0[Y] - 10;
aux1[Y] = aux1[Y] + 10;
Polygon::drawLine(aux0, aux1);
//Draw a little cross in brick pen position
float finalAngle = calculateAngle(yAxis, brickAngle);
//Rotate penOffset that angle
Vertex currentPenOffset = penOffset.rotate(finalAngle*TO_RADIANS);
aux0 = brickPosition*4 + currentPenOffset*4;
aux1 = brickPosition*4 + currentPenOffset*4;
aux0[X] = aux0[X] - 5;
aux1[X] = aux1[X] + 5;
Polygon::drawLine(aux0, aux1);
aux0 = brickPosition*4 + currentPenOffset*4;
aux1 = brickPosition*4 + currentPenOffset*4;
aux0[Y] = aux0[Y] - 5;
aux1[Y] = aux1[Y] + 5;
Polygon::drawLine(aux0, aux1);
ofSetColor(ofColor::white);
}
double FreeRotationFilter::calculateAngle(int x1, int y1, int x2, int y2)
{
QPoint p1(x1, y1);
QPoint p2(x2, y2);
return calculateAngle(p1, p2);
}
void Server::moveForNextPoint( const Vertex& finalPosition, const Vertex& finalVector )
{
//Angle between final vector and yAxis, because
//penOffset is defined on yAxis
float finalAngle = calculateAngle(yAxis, finalVector);
//Rotate penOffset that angle
Vertex currentPenOffset = penOffset.rotate(finalAngle*TO_RADIANS);
//Substract the rotated penOffset to the final position to obtain the
//brick final position
Vertex auxPosition = finalPosition - currentPenOffset;
//If position and angle are the same then do nothing
if(auxPosition == brickPosition && finalVector == brickAngle){
return;
}
float distanceToAux = brickPosition.distance(auxPosition);
//Vector from brick current position to final brick position
Vertex currentToAux = auxPosition - brickPosition;
currentToAux.normalize();
//Calculate the angle between where the brick is looking and
//currentToAux vector
float rotationAngle = calculateAngle(brickAngle, currentToAux);
//To go to auxPosition the brick must rotate rotationAngle
sendMessage( rotationAngle*ROTATION_FACTOR, -rotationAngle*ROTATION_FACTOR, PEN_UP );
//Update brick angle
brickAngle = currentToAux;
//Go forward the distance to auxPosition
sendMessage( distanceToAux*MOVE_FACTOR, distanceToAux*MOVE_FACTOR, PEN_UP );
//Update brickposition
brickPosition = auxPosition;
//Calculate the angle between where the brick is looking now, after rotationAngle
//rotation, and vector auxToFinal
rotationAngle = calculateAngle(currentToAux, finalVector);
//Rotate the brick to look in finalVector direction
sendMessage( rotationAngle*ROTATION_FACTOR, -rotationAngle*ROTATION_FACTOR, PEN_UP );
//Update brick angle
brickAngle = finalVector;
}
void Encoder_runSM() {
if (accumulatorIndex < ACCUMULATOR_LENGTH) {
accumulateAngle(currentReadAddress, currentWriteAddress);
accumulatorIndex++;
}
else {
// calculate and switch encoder choice, resetting index
calculateAngle();
}
}
void ServoWave::move(){
int dir = 0;
//Serial.println(curAngle);
int newAngle=calculateAngle();
servos[0].write(newAngle);
int curAngle = servos[0].read();
float dAngle =1;// ampMax / steps;//distance of one unit to be changed;
// Serial.print(" steps = ");
// Serial.print(steps);
// Serial.print(" ampmax = ");
// Serial.print(ampMax);
// Serial.print(" dangle (ampmax/steps) = ");
// Serial.println(dAngle);
if ((curAngle + dAngle) > (ampMax+90) ) {
dir = -1;
//newAngle = curAngle - dAngle;
}
else if ((curAngle + dAngle) < (90-ampMax) ) {
// newAngle = curAngle + dAngle;
dir=1;
}
Serial.print(" dir:");
Serial.print(dir);
newAngle = curAngle+dAngle*dir;
Serial.print(" newAngle:");
Serial.print(newAngle);
for(int i=1;i<numServos;i++){
//HERE PUT IN A TIMING FUNCTION TO SEE IF IT WORKS
newAngle = curAngle;
curAngle = servos[i].read();
//wait why i'm confused why i did this
// Serial.println(
servos[i].write(newAngle);
Serial.print(" S:");
Serial.print(i);
Serial.print(" ANG:");
Serial.print(newAngle);
// Serial.print(servos[i].read());
}
Serial.println();
}
void CVehicle::calculateAngleList( const osg::ref_ptr<osg::Vec3Array>& junctionArray)
{
osg::Vec3 vecA,vecB;
for (int i = 0;i != m_index.size()-1;i++)
{
if(i == 0)
vecA.set(0.0,1.0,0.0);
else
vecA = (*junctionArray)[m_index[i]] - (*junctionArray)[m_index[i-1]];
vecA.normalize();
vecB = (*junctionArray)[m_index[i+1]] - (*junctionArray)[m_index[i]];
vecB.normalize();
m_angleList.push_back(calculateAngle(vecA,vecB));
}
}
bool RotateDesktopGesture::processGestureImpl(GestureContext *gestureContext)
{
float angle = calculateAngle(gestureContext);
QList<Path *> activePaths = gestureContext->getActiveTouchPaths();
if (_animatingToForward)
{
// user may realize accidental trigger of rotation while camera is orienting to face "forward" wall
if (gestureContext->getNumActiveTouchPoints() != 2)
{
_animatingToForward = false;
cam->killAnimation();
cam->animateTo(_startPos, _startDir, _startUp);
return false;
}
if(cam->isAnimating())
return true; // let camera animate to face the "forward" wall first
else
{
_originalDir = cam->getDir();
_originalUp = cam->getUp();
_compensate = angle;
_animatingToForward = false;
}
}
if (gestureContext->getNumActiveTouchPoints() != 2)
{
if (activePaths.count() == 1)
{
if (activePaths.contains(_gestureTouchPaths[0]) || activePaths.contains(_gestureTouchPaths[1]))
{
_recalculateInitials = true;
return true; // still remain as rotate gesture, user probably lifted and repositioning one finger
}
}
if (abs(angle) < SNAP_MINIMUM_ANGLE) // finished rotation gesture, check if a rotation is achieved
{
cam->animateTo(_startPos, _startDir, _startUp); // rotation amount too small, revert to original view
return false;
}
// Snap the camera to the nearest wall
int wallIndex = getWallCameraIsFacing();
assert(wallIndex != -1);
Vec3 camDir, camPos;
cam->lookAtWall(GLOBAL(Walls)[wallIndex], camPos, camDir);
cam->animateTo(camPos, camDir);
return false;
}
if (_recalculateInitials)
{
_recalculateInitials = false;
// if rotation after repositioning is canceled, revert camera to before lifting and repositioning
// not revert camera to before rotation gesture is recognized
_startUp = cam->getUp();
_startDir = cam->getDir();
_startPos = cam->getEye();
}
cam->revertAnimation();
pair<Vec3, Vec3> camPair = calculateCameraPosition();
cam->setEye(camPair.first);
// Rotate the camera; take out the amount used to trigger rotation and face "forward" wall to avoid too much initial rotation
float rotationAngle = angle - _compensate;
Quat rotation(rotationAngle, Vec3(0.0f, 1.0f, 0.0f));
Vec3 dir = _originalDir;
Vec3 up = _originalUp;
rotation.rotate(dir);
rotation.rotate(up);
dir.sety(camPair.second.y);
cam->animateTo(cam->getEye(), dir, up, 5, false);
return true;
}
void ServoWave::move(){
// for all the servos from 0 to numServos
// lookup a position i.e. servoPositions[i]
// and write that to the corresponding servo, i.e. servos[i].write(...)
//HERE IS WHERE I CAN USE THE WAVELENGTH -= GET THE POSITION OF THE
//int factor = 90;//just add this in order to get back to a servoWrite value of 0-180
int dir = 0;
//dont change this as this will be what inlusnces the next servo
//Serial.println(curAngle);
int newAngle=calculateAngle();
servos[0].write(newAngle);
int curAngle = servos[0].read();
// Serial.print(" curAng = ");
// Serial.print(curAngle);
// Serial.print("newan= ");
//Serial.println(newAngle);
float dAngle =100;// ampMax / steps;//distance of one unit to be changed;
// Serial.print(" steps = ");
// Serial.print(steps);
// Serial.print(" ampmax = ");
// Serial.print(ampMax);
// Serial.print(" dangle (ampmax/steps) = ");
// Serial.println(dAngle);
if ((curAngle + dAngle) > (ampMax+90) ) {
dir = -1;
//newAngle = curAngle - dAngle;
}
else if ((curAngle + dAngle) < (90-ampMax) ) {
// newAngle = curAngle + dAngle;
dir=1;
}
Serial.print(" dir:");
Serial.print(dir);
newAngle = curAngle+dAngle*dir;
Serial.print(" newAngle:");
Serial.print(newAngle);
for(int i=1;i<numServos;i++){
newAngle = curAngle;
curAngle = servos[i].read();
//wait why i'm confused why i did this
// Serial.println(
servos[i].write(newAngle);
Serial.print(" S:");
Serial.print(i);
Serial.print(" ANG:");
Serial.print(newAngle);//servos[i].read());
}
Serial.println();
/*
int lastAng = 0;
int currentAngle=servoPositions[i];// = currentPositions;see that i set servoPositions ot be 90
int angleChange = lastAng-currentAngle;
angleChange = calculateAngle();
currentAngle-=angleChange;
lastAng = currentAngle;
servos[i].write(servoPositions[currentAngle]);
//Serial.println(servoPositions[currentAngle]);*/
}
void approachTargetLocation(struct RoboAI* ai, int mode){
int targetProximity = 200;
double angleThreshold = 20;
// The idea behind this version of approaching the ball
// is simple: I turn and check the angle between our facing
// and the ball. If it's decreasing, we are turning the right way;
// if not, we start turning the opposite way.
// This is to avoid the problem with acos: its always positive.
struct vector selfToTarget, selfDirection, selfToBall, goalToBall, goalToSelf, selfToKickPos;
struct vector selfToGoal;
calculateInitialVectors(&selfDirection, &selfToBall, &goalToBall, &goalToSelf, ai, SELF);
switch(mode) {
case TO_BALL: // for chase mode, or after oriented to goal - go directly to ball
selfToTarget.x = selfToBall.x;
selfToTarget.y = selfToBall.y;
break;
case TO_KICK_POS: // we are still approaching the optimal kicking position
selfToGoal.x = -goalToSelf.x;
selfToGoal.y = -goalToSelf.y;
if (calculateAngle(&selfToBall, &selfToGoal) < 5) {
selfToTarget.x = selfToBall.x;
selfToTarget.y = selfToBall.y;
} else {
calculateKickPosition(&selfToBall, &goalToBall, &goalToSelf, &selfToKickPos, ai);
selfToTarget.x = selfToKickPos.x;
selfToTarget.y = selfToKickPos.y;
}
break;
case TO_DEFENSE: // assume defensive positions
getInterceptPos(ai, &selfToTarget);
break;
default: // this should never happen, but otherwise, just go for ball
selfToTarget.x = selfToBall.x;
selfToTarget.y = selfToBall.y;
break;
}
printf("[%d|%d] CURRENT TARGET: [%f, %f], DIST: [%f]\n", ai->st.state, mode, selfToTarget.x, selfToTarget.y, magnitude(&selfToTarget));
// Note how the quadrant problem still exists at this point.
// I will fix this by forcing the robot to reset the motion vector
// whenever our angles start increasing in distance.
//////////////////// ! RESET FLAGGED ANGLE ! ////////////////////
// First, we check if the oldAngle variable is still valid.
// If it is flagged for a renewal, we move forwards and reset it.
if (oldAngle == -999999){
drive_speed(35);
oldAngle = calculateAngle(&selfToTarget, &selfDirection);
return;
}
//////////////////// ! HANDLE TURNING ! ////////////////////
// If we, instead, have a valid old angle, we must check if after
// turning we are increasing the angle.
double currentAngle = calculateAngle(&selfToTarget, &selfDirection);
// printf("ANGLE: %f from %f, difference of %f\n", currentAngle, oldAngle, currentAngle - oldAngle);
if (currentAngle > oldAngle && currentAngle > 5){
adjustOverturn = 1;
// There are two possible circumstances that we are concerned with.
// a) We just jumped into a different quadrant from the motion vector.
// b) We are really just turning away from the target.
// Let me first address the quadrant problem first.
//////////////////// ! QUADRANT SOLUTION ! ////////////////////
if (attemptedQuadrantSolution < 5){
// The solution to this is rather simple:
// I continue turning in the direction I already am, and
// reset the oldAngle to update it in the new quadrant.
// IF I was indeed turning in the right direction but the
// angle was messed up because of the bajanxed quadrants,
// then the (currentAngle > oldAngle) boolean above would
// then return false and we will continue on.
printf("ATTEMPTED QUADRANT SOLUTION\n");
if (attemptedQuadrantSolution < 1)
pivotRobot(25);
else if (attemptedQuadrantSolution < 2){
oldAngle = -999999;
}
else if (attemptedQuadrantSolution < 3){
drive_speed(50);
} else {
// drive_speed(25);
all_stop();
}
// if (attemptedQuadrantSolution == 4)
attemptedQuadrantSolution++;
return;
} else {
//////////////////// ! CHANGE DIRECTION ! ////////////////////
// If I got into here, I have already attempted a quadrant
// problem solution, and thus am most likely turning in the
// wrong direction. I will then change the direction I have
// been turning in, and reset the attemptedQuadrantSolution
// flag in anticipation of further quadrant problems.
printf("CHANGED DIRECTION\n");
toggleTurn = !toggleTurn;
attemptedQuadrantSolution = 0;
pivotRobot(20);
oldAngle = currentAngle; // Store the old angle to keep checking.
return;
}
} else {
// If we reached this part of the code, that means we are actually
// turning towards the target location. This is great! We can
// continue turning towards the target until the angle is within
// an acceptable range.
if (currentAngle > angleThreshold){
//////////////////// ! CONTINUE TURNING ! ////////////////////
// We need to continue turning until an optimal angle is achieved.
attemptedQuadrantSolution = 0;
if (mode == TO_BALL) {
printf("SLOW YO ROLL\n");
pivotRobot(17);
} else {
pivotRobot(20);
}
oldAngle = currentAngle;
return;
} else {
//////////////////// ! APPROACH TARGET ! ////////////////////
// We are facing the target location within an acceptable range.
if (magnitude(&selfToTarget) > targetProximity){
// Let us start moving forwards if we are not within range.
if (mode == TO_BALL) {
if (adjustOverturn){
adjustOverturn = 0;
if (toggleTurn){
drive_custom(38, 18);
} else {
drive_custom(18, 38);
}
} else {
drive_speed(20);
}
} else {
if (adjustOverturn){
adjustOverturn = 0;
if (toggleTurn){
drive_custom(18 + driveRelative(magnitude(&selfToTarget)), driveRelative(magnitude(&selfToTarget)));
} else {
drive_custom(driveRelative(magnitude(&selfToTarget)), 18 + driveRelative(magnitude(&selfToTarget)));
}
} else {
drive_speed(driveRelative(magnitude(&selfToTarget)));
}
}
attemptedQuadrantSolution = 5;
oldAngle = currentAngle;
return;
} else {
if (mode == TO_DEFENSE){
all_stop();
return;
}
// We are in an acceptable range of the target!
// Let us then increase the state of the AI directive and
// handle the different cases for different game modes.
ai->st.state ++;
return;
}
}
}
}
void
PostscriptRenderer::renderLines (PostscriptCanvas& _canvas,Map& map)
{
// Draw drawableLines
_canvas.setlinejoin (1);
const std::set<DrawableLine*>& selectedLines = map.getSelectedLines ();
// Predraw
for (std::set<DrawableLine*>::const_iterator it = selectedLines.begin ();
it != selectedLines.end () ; ++it) {
const DrawableLine* dbl = *it;
dbl->prepare (map, _config.getSpacing ());
}
_canvas.setlinewidth (_config.getBorderWidth ());
_canvas.setrgbcolor(_config.getBorderColor ());
// Draw
for (std::set<DrawableLine*>::const_iterator it = selectedLines.begin ();
it != selectedLines.end () ; ++it) {
const DrawableLine* dbl = *it;
// const std::vector<Coordinate>& shiftedPoints = dbl->getShiftedPoints ();
doDrawCurvedLine(_canvas, dbl);
}
_canvas.setlinewidth (_config.getLineWidth ());
for (std::set<DrawableLine*>::const_iterator it = selectedLines.begin ();
it != selectedLines.end () ; ++it) {
const DrawableLine* dbl = *it;
_canvas.setrgbcolor(dbl->getColor ());
doDrawCurvedLine(_canvas, dbl);
}
for (std::set<DrawableLine*>::const_iterator it = selectedLines.begin ();
it != selectedLines.end () ; ++it) {
const DrawableLine* dbl = *it;
if (dbl->getWithPhysicalStops() == false) continue;
const std::vector<Coordinate>& shiftedPoints = dbl->getShiftedPoints ();
for (unsigned int i=1; i<shiftedPoints.size()-1; ++i)
{
Coordinate pt (shiftedPoints[i].x + 100.0, shiftedPoints[i].y);
double angle = calculateAngle (pt, shiftedPoints[i], shiftedPoints[i+1]);
if (dbl->isStopPoint (i))
{
doDrawTriangleArrow(_canvas, shiftedPoints[i], Angle::toDegrees(angle - M_PI_2));
doDrawSquareStop(_canvas, shiftedPoints[i], Angle::toDegrees(angle - M_PI_2));
}
}
}
// Postdraw
for (std::set<DrawableLine*>::const_iterator it = selectedLines.begin ();
it != selectedLines.end () ; ++it) {
const DrawableLine* dbl = *it;
const std::vector<Coordinate>& shiftedPoints = dbl->getShiftedPoints ();
// Draw Terminuses
if (shiftedPoints.size () >= 2) {
Coordinate pt (shiftedPoints[0].x + 100.0,
shiftedPoints[0].y);
double angle = calculateAngle (pt,
shiftedPoints[0],
shiftedPoints[1]);
doDrawSquareTerminus (_canvas, shiftedPoints[0],
Angle::toDegrees(angle - M_PI_2));
pt = Coordinate (shiftedPoints[shiftedPoints.size ()-2].x + 100.0,
shiftedPoints[shiftedPoints.size ()-2].y);
angle = calculateAngle (pt,
shiftedPoints[shiftedPoints.size ()-2],
shiftedPoints[shiftedPoints.size ()-1]);
doDrawSquareTerminus (_canvas, shiftedPoints[shiftedPoints.size ()-1],
Angle::toDegrees(angle - M_PI_2));
}
}
}
void WSpinny::paintEvent(QPaintEvent *e) {
Q_UNUSED(e); //ditch unused param warning
QPainter p(this);
p.setRenderHint(QPainter::SmoothPixmapTransform);
if (m_pBgImage) {
p.drawImage(0, 0, *m_pBgImage);
}
#ifdef __VINYLCONTROL__
// Overlay the signal quality drawing if vinyl is active
if (m_bVinylActive && m_bSignalActive) {
// draw the last good image
p.drawImage(this->rect(), m_qImage);
}
#endif
// To rotate the foreground image around the center of the image,
// we use the classic trick of translating the coordinate system such that
// the origin is at the center of the image. We then rotate the coordinate system,
// and draw the image at the corner.
p.translate(width() / 2, height() / 2);
bool bGhostPlayback = m_pSlipEnabled->get();
if (bGhostPlayback) {
p.save();
}
double playPosition = -1;
double slipPosition = -1;
m_pVisualPlayPos->getPlaySlipAt(0, &playPosition, &slipPosition);
if (playPosition != m_dAngleLastPlaypos) {
m_fAngle = calculateAngle(playPosition);
m_dAngleLastPlaypos = playPosition;
}
if (slipPosition != m_dGhostAngleLastPlaypos) {
m_fGhostAngle = calculateAngle(slipPosition);
m_dGhostAngleLastPlaypos = slipPosition;
}
if (m_pFgImage && !m_pFgImage->isNull()) {
// Now rotate the image and draw it on the screen.
p.rotate(m_fAngle);
p.drawImage(-(m_pFgImage->width() / 2),
-(m_pFgImage->height() / 2), *m_pFgImage);
}
if (bGhostPlayback && m_pGhostImage && !m_pGhostImage->isNull()) {
p.restore();
p.save();
p.rotate(m_fGhostAngle);
p.drawImage(-(m_pGhostImage->width() / 2),
-(m_pGhostImage->height() / 2), *m_pGhostImage);
//Rotate back to the playback position (not the ghost positon),
//and draw the beat marks from there.
p.restore();
}
}
/**
* Calculate the angle from point1 to point2 by using arc-tangent.
* @param point1 The first point.
* @param point2 The second point.
* @return The calculated angle.
*/
float math::calculateAngle(Point point1, Point point2)
{
return calculateAngle(point1.getX(), point1.getY(), point2.getX(), point2.getY());
}
void contentAccessModuleServer::PoiSearchStarted(const handleId_t& poiSearchHandle, const uint16_t& maxSize, const DBus_geoCoordinate3D::DBus_geoCoordinate3D_t& location, const std::vector< DBus_categoryRadius::DBus_categoryRadius_t >& poiCategories, const std::vector< DBus_attributeDetails::DBus_attributeDetails_t >& poiAttributes, const std::string& inputString, const int32_t& sortOption)
{
uint16_t index,sub_index;
categoryId_t category_index;
bool isCategoryFound, isAttributeFound;
DBus_attributeDetails attribDet;
DBus_attributeDetails::attributeDetails_t attribDetails;
DBus_geoCoordinate3D geoCoord;
DBus_geoCoordinate3D::geoCoordinate3D_t geoCoordinate;
DBus_categoryRadius catRad;
DBus_categoryRadius::categoryRadius_t catRadius;
//todo: limit the search to maxSize, manage sortOption
if (m_poiSearchHandle != INVALID_HANDLE)
// new search launched before end of the last one
sendDBusError("org.genivi.poiprovider.poiSearch.Error.HandleNotAvailable"); //to be discussed !
else
{
//preliminary reset of flags for categories
for (index=0;index<m_availableCategories;index++)
{
m_availableCategoryTable[index].isSearch = false;
//reset flags for all the attributes of the category
for (category_index=0;category_index<((m_availableCategoryTable[index]).attributeList).size();category_index++)
(m_availableCategoryTable[index]).attributeList.at(category_index).isSearched =false;
}
//now scan the flags to set and set the flag into the m_availableCategoryTable
for (index=0;index<poiCategories.size();index++)
{
catRad.setDBus(poiCategories.at(index));
catRadius = catRad.get();
sub_index = 0;
isCategoryFound = false;
do
{
if (catRadius.id == (m_availableCategoryTable[sub_index].alias_id))
{ //the content access server is using alias !
isCategoryFound = true;
m_availableCategoryTable[sub_index].isSearch = true;
m_availableCategoryTable[sub_index].radius = (catRadius.radius)*10; //get the radius (unit is 10 m)
m_availableCategoryTable[sub_index].angle = calculateAngle(m_availableCategoryTable[sub_index].radius);
}
else
++sub_index;
} while((isCategoryFound == false) && (sub_index<m_availableCategories));
}
//reset flags for all the attributes of the categories into the database
for (index=0;index<m_availableCategories;index++)
{
for (category_index=0;category_index<((m_availableCategoryTable[index]).attributeList).size();category_index++)
(m_availableCategoryTable[index]).attributeList.at(category_index).isSearched =false;
}
//now scan the flags to be set
for (index=0;index<poiAttributes.size();index++)
{
sub_index = 0;
isAttributeFound = false;
attribDet.setDBus(poiAttributes.at(index));
attribDetails = attribDet.get();
if ( isCategoryAvailable(attribDetails.categoryId,&category_index) == true)
{ //category found into the database!
if (m_availableCategoryTable[category_index].isSearch)
{ //category selected for the search
for (sub_index=0;sub_index<(m_availableCategoryTable[category_index].attributeList.size());sub_index++)
{ //check attribute by id
if ((m_availableCategoryTable[category_index].attributeList.at(sub_index)).id == attribDetails.attribute.id)
(m_availableCategoryTable[category_index].attributeList.at(sub_index)).isSearched =true;
}
}
}
}
m_poiSearchHandle = poiSearchHandle;
geoCoord.setDBus(location);
geoCoordinate = geoCoord.get();
m_centerLocation.latitude = geoCoordinate.latitude;
m_centerLocation.longitude = geoCoordinate.longitude;
m_centerLocation.altitude = geoCoordinate.altitude;
m_searchStatus = GENIVI_POISERVICE_SEARCHING;
//sortOption is not used
//maxSize is not used
m_totalSize = searchAroundALocation(m_centerLocation,&inputString);
m_searchStatus = GENIVI_POISERVICE_FINISHED;
}
}
void
PostscriptRenderer::doDrawCurvedLine (PostscriptCanvas& _canvas,const DrawableLine* dbl)
{
const std::vector<Coordinate>& shiftedPoints = dbl->getShiftedPoints ();
_canvas.newpath();
/*
_canvas.moveto(shiftedPoints[0].x+5, shiftedPoints[0].y+10);
_canvas.rotate (45.0);
_canvas.text (dbl->getShortName ());
_canvas.rotate (-45.0);
*/
_canvas.moveto(shiftedPoints[0].x, shiftedPoints[0].y);
for (unsigned int i=1; i<shiftedPoints.size (); ++i)
{
double x = shiftedPoints[i].x;
double y = shiftedPoints[i].y;
double radiusShift = 0.0;
if (_config.getEnableCurves () && (i < shiftedPoints.size () - 1))
{
// Take care of intern/extern turn to invert radius
const Coordinate& p_minus_1 = shiftedPoints[i-1];
const Coordinate& p_plus_1 = shiftedPoints[i+1];
double angle = calculateAngle (p_minus_1, shiftedPoints[i], p_plus_1);
if (angle < 0)
{
radiusShift = -dbl->getShift (i) * _config.getSpacing ();
}
else
{
radiusShift = +dbl->getShift (i) * _config.getSpacing ();
}
double radiusToUse = _config.getRadius () + radiusShift;
if (radiusToUse > _config.getRadius () + _config.getRadiusDelta ())
{
radiusToUse = _config.getRadius () - _config.getRadiusDelta ();
}
if (radiusToUse < _config.getRadius () - _config.getRadiusDelta ())
{
radiusToUse = _config.getRadius () - _config.getRadiusDelta ();
}
_canvas.arct(x, y, shiftedPoints[i+1].x,
shiftedPoints[i+1].y, radiusToUse);
}
else
{
_canvas.lineto(x, y);
}
}
_canvas.stroke();
}
void AngleMeasure::drawOne(StelCore *core, const StelCore::FrameType frameType, const StelCore::RefractionMode refractionMode, const Vec3f txtColor, const Vec3f lineColor)
{
const StelProjectorP prj = core->getProjection(frameType, refractionMode);
StelPainter painter(prj);
painter.setFont(font);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_BLEND);
if (lineVisible.getInterstate() > 0.000001f)
{
Vec3d xy;
QString displayedText;
if (frameType==StelCore::FrameEquinoxEqu)
{
if (prj->project(perp1EndPoint,xy))
{
painter.setColor(txtColor[0], txtColor[1], txtColor[2], lineVisible.getInterstate());
if (flagShowPA)
displayedText = QString("%1 (%2%3)").arg(calculateAngle(), messagePA, calculatePositionAngle(startPoint, endPoint));
else
displayedText = calculateAngle();
painter.drawText(xy[0], xy[1], displayedText, 0, 15, 15);
}
}
else
{
if (prj->project(perp1EndPointHor,xy))
{
painter.setColor(txtColor[0], txtColor[1], txtColor[2], lineVisible.getInterstate());
if (flagShowHorizontalPA)
displayedText = QString("%1 (%2%3)").arg(calculateAngle(true), messagePA, calculatePositionAngle(startPointHor, endPointHor));
else
displayedText = calculateAngle(true);
painter.drawText(xy[0], xy[1], displayedText, 0, 15, -5);
}
}
glDisable(GL_TEXTURE_2D);
// OpenGL ES 2.0 doesn't have GL_LINE_SMOOTH. But it looks much better.
#ifdef GL_LINE_SMOOTH
if (QOpenGLContext::currentContext()->format().renderableType()==QSurfaceFormat::OpenGL)
glEnable(GL_LINE_SMOOTH);
#endif
// main line is a great circle
painter.setColor(lineColor[0], lineColor[1], lineColor[2], lineVisible.getInterstate());
if (frameType==StelCore::FrameEquinoxEqu)
{
painter.drawGreatCircleArc(startPoint, endPoint, NULL);
// End lines
painter.drawGreatCircleArc(perp1StartPoint, perp1EndPoint, NULL);
painter.drawGreatCircleArc(perp2StartPoint, perp2EndPoint, NULL);
}
else
{
painter.drawGreatCircleArc(startPointHor, endPointHor, NULL);
// End lines
painter.drawGreatCircleArc(perp1StartPointHor, perp1EndPointHor, NULL);
painter.drawGreatCircleArc(perp2StartPointHor, perp2EndPointHor, NULL);
}
#ifdef GL_LINE_SMOOTH
if (QOpenGLContext::currentContext()->format().renderableType()==QSurfaceFormat::OpenGL)
glDisable(GL_LINE_SMOOTH);
#endif
}
if (messageFader.getInterstate() > 0.000001f)
{
painter.setColor(txtColor[0], txtColor[1], txtColor[2], messageFader.getInterstate());
int x = 83;
int y = 120;
int ls = painter.getFontMetrics().lineSpacing();
painter.drawText(x, y, messageEnabled);
y -= ls;
painter.drawText(x, y, messageLeftButton);
y -= ls;
painter.drawText(x, y, messageRightButton);
}
glDisable(GL_BLEND);
}
void CVehicle::createRandomPath(const osg::ref_ptr<osg::Vec3Array>& junctionArray,std::vector< std::vector<unsigned int> >& junctionAdjacency)
{
std::vector<unsigned int> temp1,temp2;
unsigned int length = 25; //定义路径长度
unsigned int begin=0,end=0,temp=0;
begin = rand() % junctionArray->size();
m_index.push_back(begin);
temp1 = junctionAdjacency[begin];
for (int i = 0;i != temp1.size();i++)
{
if (temp1[i])
{
temp2.push_back(i);
}
}
temp = rand() % temp2.size();
end = temp2[temp];
m_index.push_back(end);
temp1.clear();
temp2.clear();
osg::Vec3 vecA,vecB;
unsigned int mid;
double angle;
while (m_index.size() < length )
{
temp1 = junctionAdjacency[end];
mid = end;
vecA = (*junctionArray)[mid] - (*junctionArray)[begin];
vecA.normalize();
for (int i = 0;i != temp1.size();i++)
{
if (temp1[i])
{
temp2.push_back(i);
}
}
if( temp2.size() == 1)
break;
else
if (temp2.size() == 2)
{
end = temp2[0];
if( begin == end)
end = temp2[1];
m_index.push_back(end);
}
else
{
do
{
temp = rand() % temp2.size();
end = temp2[temp];
vecB = (*junctionArray)[end] - (*junctionArray)[mid];
vecB.normalize();
angle = calculateAngle( vecA,vecB );
srand(s_seed++);
}while( angle > (osg::PI / 3.0) || begin == end );
m_index.push_back(end);
}
temp1.clear();
temp2.clear();
begin = m_index[m_index.size()-2];
end = m_index[m_index.size()-1];
}
// m_index.push_back(5);
// m_index.push_back(15);
// m_index.push_back(25);
// m_index.push_back(35);
// m_index.push_back(45);
// m_index.push_back(55);
// m_index.push_back(65);
// m_index.push_back(75);
// m_index.push_back(85);
calculateAngleList(junctionArray);
}