bool Character::update(float elapsed)
{
float distance = speed * elapsed;
//Distanz zum nächsten Feld berechnen
CL_Point nextField = getNextField(moveDirection);
CL_Pointf nextFieldCoordinates = getCoordinates(nextField);
float distanceToField = abs(position.x - nextFieldCoordinates.x) + abs(position.y - nextFieldCoordinates.y);
//Ist die zurückzulegende Distanz größer als die Distanz zum nächsten Feldmittelpunkt
//und ist das Feld frei, so muss eine eventuelle Richtungsänderung geprüft werden
while (distance > distanceToField && world->getLevel()->isFreeField(nextField))
{
//Zum Feldmittelpunkt bewegen
move(distanceToField);
distance -= distanceToField;
currentField = getIndices(nextFieldCoordinates);
//wird für erbende Klassen aufgerufen, um Aktionen auf dem Feldmittelpunkt ausführen zu können
onFieldCenter();
//Ist eine Richtungsänderung möglich, dann Richtung ändern
if(world->getLevel()->isFreeField(getNextField(newDirection)))
moveDirection = newDirection;
//Distanz zum nächsten Feld berechnen
nextField = getNextField(moveDirection);
nextFieldCoordinates = getCoordinates(nextField);
distanceToField = abs(position.x - nextFieldCoordinates.x) + abs(position.y - nextFieldCoordinates.y);
}
//Spielfigur weiterbewegen bzw. Richtung ändern
if(world->getLevel()->isFreeField(nextField))
move(distance);
else if(world->getLevel()->isFreeField(getNextField(newDirection)))
moveDirection = newDirection;
animationPos += elapsed * animationSpeed;
while (animationPos >= animationLength)
animationPos -= animationLength;
body.set_frame(getAnimationPos());
return true;
}
//Return Current CCamera Direction
vec3 CArcBallCamera::getDirection()
{
vec3 dir = m_origin - getCoordinates();
dir.normalize();
return dir;
}
void MainWindow::probeCalibration()
{
std::cout<<"Probe Calibration"<<std::endl;
if (!displayWidget->getImageStack().empty())
{
ProbeCalibrationWidget* probeCalibration = new ProbeCalibrationWidget();
if (displayWidget->isImageStackLoaded)
probeCalibration->setImageStack(displayWidget->getImageStack());
else
probeCalibration->setImage(displayWidget->getImageViewer()->GetInput());
displayWidget->setProbeFlag(true);
// get left mouse pressed with high priority
Connections->Connect(displayWidget->getQVTKWidget()->GetRenderWindow()->GetInteractor(),
vtkCommand::LeftButtonPressEvent,
probeCalibration,
SLOT(getCoordinates()));
probeCalibration->setMainWindow(this);
probeCalibration->show();
}
else
{
QErrorMessage errorMessage;
errorMessage.showMessage(
"No images loaded, </ br> please load an images before calibrate the probe");
errorMessage.exec();
}
}
// Probes the 7 points on a delta can be used for off board calibration
bool DeltaCalibrationStrategy::probe_delta_points(Gcode *gcode)
{
// get probe points
float t1x, t1y, t2x, t2y, t3x, t3y;
std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
// gather probe points
float pp[][2] {{t1x, t1y}, {t2x, t2y}, {t3x, t3y}, {0, 0}, {-t1x, -t1y}, {-t2x, -t2y}, {-t3x, -t3y}};
float max_delta= 0;
float last_z= NAN;
for(auto& i : pp) {
int s;
if(!zprobe->doProbeAt(s, i[0], i[1])) return false;
float z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("X:%1.4f Y:%1.4f Z:%1.4f (%d) A:%1.4f B:%1.4f C:%1.4f\n",
i[0], i[1], z, s,
THEKERNEL->robot->actuators[0]->get_current_position()+z,
THEKERNEL->robot->actuators[1]->get_current_position()+z,
THEKERNEL->robot->actuators[2]->get_current_position()+z);
if(isnan(last_z)) {
last_z= z;
}else{
max_delta= std::max(max_delta, fabsf(z-last_z));
}
}
gcode->stream->printf("max delta: %f\n", max_delta);
return true;
}
void Touch::drawMiddle(Mat &mat)
{
Point up(getCoordinates().x, getCoordinates().y+2);
Point down(getCoordinates().x, getCoordinates().y-2);
Point left(getCoordinates().x-2, getCoordinates().y);
Point right(getCoordinates().x+2, getCoordinates().y);
line(mat, up, down, Scalar(0,0,255,0), 1);
line(mat, left, right, Scalar(0,0,255,0), 1);
}
int main()
{
openFileOutput(getFileName());
getNumOfVehicles(file);
printCoordinates(getCoordinates(file,3,4),3,4);
//getCoordinates(file,3,4);
return 0;
}
void PrimitiveBox::draw(ImageBase &im) {
Coordinate upleft = getCoordinates().front();
Coordinate downright = getCoordinates().back();
Coordinate upright = Coordinate(downright.getX(), upleft.getY());
Coordinate downleft = Coordinate(upleft.getX(), downright.getY());
vector<Coordinate> newCoords;
newCoords.push_back(upleft);
newCoords.push_back(upright);
newCoords.push_back(downright);
newCoords.push_back(downleft);
PrimitiveBase::setCoordinates(newCoords);
PrimitivePolygon::draw(im);
}
vec3 CArcBallCamera::getStrafe()
{
vec3 dir;
dir = getCoordinates();
dir.normalize();
dir.cross(getUp());
return dir;
}
void Vertex::render(float alpha, Appearance& appearance)
{
glPointSize(5.0);
glColor4f(0.0, 0.0, 0.0, 1.0f);
glBegin(GL_POINTS);
glVertex3fv(getCoordinates());
glEnd();
}
void SBarInfo::getCoordinates(FScanner &sc, bool fullScreenOffsets, SBarInfoCoordinate &x, SBarInfoCoordinate &y)
{
int tmpX = *x;
int tmpY = *y;
getCoordinates(sc, fullScreenOffsets, tmpX, tmpY);
x = tmpX;
y = tmpY;
}
//[10]
bool GameField::didGainCherry()
{
auto snakeHead = *(snake.getSnakeBody().rbegin());
auto snakeHeadCoord = snakeHead.getCoordinates();
auto cherryCoord = cherry.getCoordinates();
return (snakeHeadCoord == cherryCoord);
}
void GeomData::calculate_CDL() {
int i,dom, row;
const int* indices = NULL;
// elements' coordinates
const double* coords[4] = {NULL,NULL,NULL,NULL};
// vector created over elements' edges
int num_edges = 3*(dim-1);
double* vectors[6];
for (i=0; i<num_edges; i++) {
vectors[i] = new double[dim];
}
// indices to automate edge vectors creation
int idx[6][2] = {{0,1},{1,2},{2,0},{0,3},{1,3},{2,3}};
// Auxiliary variable
int pos = dim+1;
int k = 0;
for (dom=0; dom<_ndom; dom++) {
int nelements = numDomElem[dom];
// loop over domains' elements
for (row=0; row<nelements; row++) {
getElement(dom,row,indices);
// get vertices coordinates
for (i=0; i<pos; i++) {
getCoordinates(indices[i+pos],coords[i]);
}
// create edge vectors
for (i=0; i<num_edges; i++) {
const double* vtx1 = coords[ idx[i][0] ];
const double* vtx2 = coords[ idx[i][1] ];
for (int j=0; j<dim; j++) {
(vectors[i])[j] = vtx1[j] - vtx2[j];
}
}
// vectors lengths summation
for (i=0; i<num_edges; i++) {
elem_CDL[k] = sqrt( inner_product(vectors[i],vectors[i],dim) );
}
// take average lenght
elem_CDL[k] /= pos;
k++;
}
}
for (i=0; i<num_edges; i++) {
delete[] vectors[i];
vectors[i] = 0;
}
}
geos::geom::MultiPoint* GeoJSONReader::getMultiPoint(Handle<Value> coords) {
if (coords->IsArray()) {
return geometryFactory->createMultiPoint(*getCoordinates(coords));
}
return geometryFactory->createMultiPoint();
}
geos::geom::LineString* GeoJSONReader::getLineString(Handle<Value> coords) {
if (coords->IsArray()) {
return geometryFactory->createLineString(getCoordinates(coords));
}
return geometryFactory->createLineString();
}
// Probes the 7 points on a delta can be used for off board calibration
bool DeltaCalibrationStrategy::probe_delta_points(Gcode *gcode)
{
float bedht= findBed();
if(isnan(bedht)) return false;
gcode->stream->printf("initial Bed ht is %f mm\n", bedht);
// check probe ht
float mm;
if(!zprobe->doProbeAt(mm, 0, 0)) return false;
float dz = zprobe->getProbeHeight() - mm;
gcode->stream->printf("center probe: %1.4f\n", dz);
// get probe points
float t1x, t1y, t2x, t2y, t3x, t3y;
std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
// gather probe points
float pp[][2] {{t1x, t1y}, {t2x, t2y}, {t3x, t3y}, {0, 0}, {-t1x, -t1y}, {-t2x, -t2y}, {-t3x, -t3y}};
float max_delta= 0;
float last_z= NAN;
float start_z;
std::tie(std::ignore, std::ignore, start_z)= THEROBOT->get_axis_position();
for(auto& i : pp) {
float mm;
if(!zprobe->doProbeAt(mm, i[0], i[1])) return false;
float z = mm;
if(gcode->subcode == 0) {
gcode->stream->printf("X:%1.4f Y:%1.4f Z:%1.4f A:%1.4f B:%1.4f C:%1.4f\n",
i[0], i[1], z,
THEROBOT->actuators[0]->get_current_position()+z,
THEROBOT->actuators[1]->get_current_position()+z,
THEROBOT->actuators[2]->get_current_position()+z);
}else if(gcode->subcode == 1) {
// format that can be pasted here http://escher3d.com/pages/wizards/wizarddelta.php
gcode->stream->printf("X%1.4f Y%1.4f Z%1.4f\n", i[0], i[1], start_z - z);
}
if(isnan(last_z)) {
last_z= z;
}else{
max_delta= std::max(max_delta, fabsf(z-last_z));
}
}
gcode->stream->printf("max delta: %f\n", max_delta);
return true;
}
int calculateHeadingHome(PathData home, float* position, float heading){
float waypointPosition[3];
getCoordinates(position[0], position[1], (float*)&waypointPosition);
waypointPosition[2] = position[2];
float targetCoordinates[3];
getCoordinates(home.longitude, home.latitude, (float*)&targetCoordinates);
targetCoordinates[2] = home.altitude;
float waypointDirection[3];
float norm = sqrt(pow(targetCoordinates[0] - waypointPosition[0],2) + pow(targetCoordinates[1] - waypointPosition[1],2) + pow(targetCoordinates[2] - waypointPosition[2],2));
waypointDirection[0] = (targetCoordinates[0] - waypointPosition[0])/norm;
waypointDirection[1] = (targetCoordinates[1] - waypointPosition[1])/norm;
waypointDirection[2] = (targetCoordinates[2] - waypointPosition[2])/norm;
heading = deg2rad(90 - heading);//90 - heading = magnetic heading to cartesian heading
float courseAngle = atan2(waypointDirection[1], waypointDirection[0]);
//Don't use follow straight path in emergency situations (the gains will give you a heading that will converge over time, but not instantaneously)
return (int)(90 - rad2deg(courseAngle));
}
/**
* The class constructor. This sets up the UI, adds validators to the
* line editors and connects the signal/slot for transmitting the coordinates.
*/
RotationPointDialog::RotationPointDialog(QWidget *parent) :
QDialog(parent)
{
this->ui.setupUi(this);
this->ui.xLineEdit->setValidator(new QDoubleValidator(this));
this->ui.yLineEdit->setValidator(new QDoubleValidator(this));
this->ui.zLineEdit->setValidator(new QDoubleValidator(this));
// Gather the coordinates
QObject::connect(this->ui.buttonBox,
SIGNAL(accepted()),
this,
SLOT(getCoordinates()));
}
void pathManagerRuntime(void) {
#if DEBUG
// char str[16];
// sprintf(&str,"%f",pmData.time);
// UART1_SendString(&str);
#endif
requestGPSInfo();
copyGPSData();
// Update status LED
if (led_bright == 0) going_up = true;
else if (led_bright == 255) going_up = false;
if (going_up) led_bright++;
else led_bright--;
setLEDBrightness(led_bright);
if (returnHome){
interchip_send_buffer.pm_data.targetWaypoint = -1;
} else {
interchip_send_buffer.pm_data.targetWaypoint = path[currentIndex]->id;
}
//Check for new uplink command data
checkAMData();
float position[3];
float heading;
//Get the position of the plane (in meters)
getCoordinates(gps_data.longitude,gps_data.latitude,(float*)&position);
position[2] = gps_data.altitude;
heading = (float)gps_data.heading;
if (returnHome || (pathCount - currentIndex < 1 && pathCount >= 0)){
interchip_send_buffer.pm_data.sp_Heading = lastKnownHeadingHome;
} else if (followPath && pathCount - currentIndex >= 1 && interchip_send_buffer.pm_data.positionFix > 0) {
currentIndex = followWaypoints(path[currentIndex], (float*)position, heading, (int*)&interchip_send_buffer.pm_data.sp_Heading);
}
if (interchip_send_buffer.pm_data.positionFix >= 1){
lastKnownHeadingHome = calculateHeadingHome(home, (float*)position, heading);
}
if (getTimeUs() - interchip_last_send_time >= INTERCHIP_SEND_INTERVAL_US){
interchip_last_send_time = getTimeUs();
interchip_send_buffer.pm_data.interchip_error_count = getInterchipErrorCount();
sendInterchipData();
}
}
/*public*/
LinearRing *
EdgeRing::getRingInternal()
{
if (ring!=NULL) return ring;
getCoordinates();
try {
ring=factory->createLinearRing(*ringPts);
} catch (const std::exception& e) {
// FIXME: print also ringPts
std::cerr << "EdgeRing::getRingInternal: "
<< e.what()
<< endl;
}
return ring;
}
//Bu fonksiyon char matris alıyor ve içerdeki hücre adresini yerinde onun gerçek değeri atıyor.
//Parametreler :
//part : hücrenin değeri, ayni zamanda hücrenin değeri getirdikten sonra bu matris içinde saklanılacak
//Returned Value (Error Code):
//Başarlı oldugunu ya da başarsız olduğunu ve sebebi
// 0 : başarsız, çünkü getirelecek hücrenin değeri hala bir formuladır
// -1 : bu hücre tablonun dışında ve değer olarak $ verildi
// 1 : başarıyla tamamlandı
int bringCellValue(char *part){
int i,j;
getCoordinates(part,&i,&j);
if(*(*(cellsTypes+i)+j) == 'f'){
return 0; // Not calculated yet
}
if( (i>=Height) || (j>=Width) )
{
strcpy(part,"$");
return -1;//Removed Cell
}
itoa(*(*(evaluated+i)+j), part, 10); // 10 - decimal;
return 1; //success
}
bool DriverRadarPositioner::isSelected(QPoint p)
{
QPoint coord = getCoordinates();
if (inPits)
{
if ((abs(p.x() - coord.x())) <= 13 && (abs(coord.y() - p.y()) <= 10))
return true;
if ((p.y() - coord.y() >= 0) && (p.y() - coord.y() <= 16) &&
(coord.x() - p.x() >= 12) && (coord.x() - p.x() <= 48))
return true;
}
else if ((abs(p.x() - coord.x())) <= 13 && ((coord.y() - p.y() <= 13) && (p.y() - coord.y() <= 26)))
return true;
return false;
}
/*public*/
LinearRing *
EdgeRing::getRingInternal()
{
if (ring!=NULL) return ring;
getCoordinates();
try {
ring=factory->createLinearRing(*ringPts);
} catch (const geos::util::IllegalArgumentException& e) {
#if GEOS_DEBUG
// FIXME: print also ringPts
std::cerr << "EdgeRing::getRingInternal: "
<< e.what()
<< endl;
#endif
::geos::ignore_unused_variable_warning(e);
}
return ring;
}
/*
* Converts this EdgeString into a new LineString.
*/
LineString*
EdgeString::toLineString()
{
return factory->createLineString(getCoordinates());
}
bool KNIKinematics::get_position_fk(moveit_msgs::GetPositionFK::Request &req,
moveit_msgs::GetPositionFK::Response &res)
{
std::vector<double> jointAngles, coordinates;
if (req.fk_link_names.size() != 1 || req.fk_link_names[0] != "katana_gripper_tool_frame")
{
ROS_ERROR("The KNI kinematics solver can only solve requests for katana_gripper_tool_frame!");
return false;
}
// ignoring req.robot_state.multi_dof_joint_state
std::vector<int> lookup = makeJointsLookup(req.robot_state.joint_state.name);
if (lookup.size() == 0)
return false;
for (size_t i = 0; i < joint_names_.size(); i++)
{
jointAngles.push_back(req.robot_state.joint_state.position[lookup[i]]);
}
coordinates = getCoordinates(jointAngles);
geometry_msgs::PoseStamped pose_in, pose_out;
pose_in.header.frame_id = "katana_base_frame";
pose_in.header.stamp = ros::Time(0);
pose_in.pose.position.x = coordinates[0];
pose_in.pose.position.y = coordinates[1];
pose_in.pose.position.z = coordinates[2];
pose_in.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(coordinates[3], coordinates[4], coordinates[5]);
// The frame_id in the header message is the frame in which
// the forward kinematics poses will be returned
try
{
bool success = tf_listener_.waitForTransform(req.header.frame_id, pose_in.header.frame_id, pose_in.header.stamp,
ros::Duration(1.0));
if (!success)
{
ROS_ERROR("Could not get transform");
return false;
}
tf_listener_.transformPose(req.header.frame_id, pose_in, pose_out);
}
catch (const tf::TransformException &ex)
{
ROS_ERROR("%s", ex.what());
return false;
}
res.pose_stamped.push_back(pose_out);
res.fk_link_names = req.fk_link_names;
res.error_code.val = res.error_code.SUCCESS;
return true;
}
/*public*/
LineString*
EdgeRing::getLineString()
{
getCoordinates();
return factory->createLineString(*ringPts);
}
void Game::play(){
getCoordinates();
}
LinearRing* WKTReader::readLinearRingText(StringTokenizer *tokenizer) {
CoordinateSequence *coords = getCoordinates(tokenizer);
LinearRing *ret;
ret = geometryFactory->createLinearRing(coords);
return ret;
}
/*****
* Name: _XmHTMLAddAreaToMap
* Return Type: void
* Description: adds the given area specification to the given imagemap
* In:
* map: XmHTMLImageMap
* object: raw area data
* Returns:
* nothing
*****/
void
_XmHTMLAddAreaToMap(XmHTMLWidget html, XmHTMLImageMap *map,
XmHTMLObject *object)
{
static mapArea *area;
mapArea *tmp;
String chPtr;
/* sanity */
if(map == NULL || object->attributes == NULL)
return;
area = (mapArea*)malloc(sizeof(mapArea));
(void)memset(area, 0, sizeof(mapArea));
area->url = _XmHTMLTagGetValue(object->attributes, "href");
area->alt = _XmHTMLTagGetValue(object->attributes, "alt");
area->nohref = _XmHTMLTagCheck(object->attributes, "nohref");
chPtr = _XmHTMLTagGetValue(object->attributes, "shape");
/* get specified coordinates */
area->coords = getCoordinates(object->attributes, &area->ncoords);
/*
* No shape given, try to figure it out using the number of specified
* coordinates
*/
if(chPtr == NULL)
{
switch(area->ncoords)
{
case 0:
/* no coords given => default area */
area->shape = MAP_DEFAULT;
break;
case 3:
/* 3 coords => circle */
area->shape = MAP_CIRCLE;
break;
case 4:
/* 4 coords => assume rectangle */
area->shape = MAP_RECT;
break;
default:
/* assume poly */
area->shape = MAP_POLY;
}
}
else
{
switch(tolower(chPtr[0]))
{
case 'c':
area->shape = MAP_CIRCLE;
break;
case 'r':
area->shape = MAP_RECT;
break;
case 'p':
area->shape = MAP_POLY;
break;
default:
area->shape = MAP_DEFAULT;
}
free(chPtr);
}
/* check if all coordinates specs are valid for the given shape */
switch(area->shape)
{
case MAP_RECT:
/* too bad if coords are bad */
if(area->ncoords != 4)
{
_XmHTMLWarning(__WFUNC__(html, "_XmHTMLAddAreaToImagemap"),
XMHTML_MSG_81, area->ncoords, object->line);
/* too many coordinates, drop the excessive ones */
if(area->ncoords > 4)
area->ncoords = 4;
else
{
/*****
* Less than required, do a complex rescan of the
* attributes (any sequence of non-digits is considered
* a separator).
*****/
free(area->coords);
area->coords = getComplexCoordinates(object->attributes,
&area->ncoords);
/* too many coordinates, drop the excessive ones */
if(area->ncoords > 4)
area->ncoords = 4;
else
{
String chPtr = _XmHTMLTagGetValue(object->attributes,
"coords");
_XmHTMLWarning(__WFUNC__(html,
"_XmHTMLAddAreaToImagemap"), XMHTML_MSG_82, chPtr);
free(chPtr);
deleteArea(area);
return;
}
}
}
break;
case MAP_CIRCLE:
/* too bad if coords are bad */
if(area->ncoords != 3)
{
_XmHTMLWarning(__WFUNC__(html,
"_XmHTMLAddAreaToImagemap"), XMHTML_MSG_83,
area->ncoords, object->line);
deleteArea(area);
return;
}
break;
case MAP_POLY:
if(!area->coords)
{
_XmHTMLWarning(__WFUNC__(html, "_XmHTMLAddAreaToImagemap"),
XMHTML_MSG_84, area->ncoords, object->line);
deleteArea(area);
return;
}
if(area->ncoords % 2)
{
_XmHTMLWarning(__WFUNC__(html, "_XmHTMLAddAreaToImagemap"),
XMHTML_MSG_85, area->ncoords, object->line);
area->ncoords--;
}
area->region = createPoly(area->ncoords, area->coords);
break;
default:
break;
}
/* gets automagically added to the list of anchors for this widget */
if(!area->nohref)
area->anchor = _XmHTMLNewAnchor(html, object);
/* add this area to the list of areas for this imagemap */
if(map->areas == NULL)
{
map->nareas = 1;
map->areas = area;
return;
}
for(tmp = map->areas; tmp != NULL && tmp->next != NULL ; tmp = tmp->next);
map->nareas++;
tmp->next = area;
_XmHTMLDebug(10, ("map.c: _XmHTMLAddAreaToMap, stored href %s, map now "
"contains %i areas.\n", area->url, map->nareas));
}
char followWaypoints(PathData* current, float* position, float heading, int* setpoint) {
static char recompute = 1;
static float radius = 5; // get from first waypoint?
static Vector current_position, target_position, next_position;
PathData* target = current;
PathData* next;
if (target->next) {
next = target->next;
} else {
next = &home;
}
getCoordinates(target->longitude, target->latitude, (float*)&target_position);
getCoordinates(next->longitude, next->latitude, (float*)&next_position);
static Vector current_heading, target_heading;
get_direction(&target_position, &next_position, &target_heading);
static Circle close, far;
static Line tangent, plane;
static DubinsPath progress = DUBINS_PATH_C1;
if (recompute) {
// printf("Recomputing path...\n");
current_position = (Vector) {
.x = position[0],
.y = position[1],
};
float angle = deg2rad(90 - heading);
current_heading = (Vector) {
.x = cos(angle),
.y = sin(angle),
};
float d = sqrt(pow(target_position.x - current_position.x, 2) + pow(target_position.y - current_position.y, 2));
if (d < 4*target->radius) {
return next->index;
}
plane = (Line) {
.initial = current_position,
.direction = current_heading,
};
if (belongs_to_half_plane(&plane, &next_position)) {
close.center = (Vector) {
.x = current_position.x + current->radius*current_heading.y,
.y = current_position.y - current->radius*current_heading.x,
};
far.center = (Vector) {
.x = target_position.x - target->radius*target_heading.y,
.y = target_position.y + target->radius*target_heading.x,
};
} else {
close.center = (Vector) {
.x = current_position.x - current->radius*current_heading.y,
.y = current_position.y + current->radius*current_heading.x,
};
far.center = (Vector) {
.x = target_position.x + target->radius*target_heading.y,
.y = target_position.y - target->radius*target_heading.x,
};
}
close.radius = target->radius;
far.radius = target->radius;
Line tangents[2];
get_tangents(&close, &far, tangents);
float d1 = sqrt(pow(tangents[0].initial.x - current_position.x, 2) + pow(tangents[0].initial.y - current_position.y, 2));
float d2 = sqrt(pow(tangents[1].initial.x - current_position.x, 2) + pow(tangents[1].initial.y - current_position.y, 2));
tangent = d1 < d2 ? tangents[0] : tangents[1];
progress = DUBINS_PATH_C1;
radius = target->radius;
recompute = 0;
}
plane = (Line) {
.initial = tangent.initial,
.direction = (Vector) {
.x = -tangent.direction.y,
.y = tangent.direction.x,
},
};
// before crossing first tangent point
if (belongs_to_half_plane(&plane, (Vector *)position)) {
char direction = current_heading.x*tangent.direction.y - current_heading.y*tangent.direction.x > 0 ? 1 : -1;
*setpoint = (int)followOrbit((float *)&close.center, close.radius, direction, position, heading);
} else {
plane.initial = (Vector) {
.x = tangent.initial.x + tangent.direction.x,
.y = tangent.initial.y + tangent.direction.y,
};
// before crossing second tangent point
if (belongs_to_half_plane(&plane, (Vector *)position)) {
*setpoint = (int)followStraightPath((float*)&tangent.direction, (float*)&plane.initial, position, heading);
} else {
plane = (Line) {
.initial = target_position,
.direction = (Vector) {
.x = -target_heading.y,
.y = target_heading.x,
},
};
// before crossing target waypoint
if (belongs_to_half_plane(&plane, (Vector *)position)) {
char direction = tangent.direction.x*target_heading.y - tangent.direction.y*target_heading.x > 0 ? 1 : -1;
*setpoint = (int)followOrbit((float *)&far.center, far.radius, direction, position, heading);
} else {
recompute = 1;
return next->index;
}
}
}
return current->index;
}
#else
char followWaypoints(PathData* currentWaypoint, float* position, float heading, int* sp_Heading){
float waypointPosition[3];
getCoordinates(currentWaypoint->longitude, currentWaypoint->latitude, (float*)&waypointPosition);
waypointPosition[2] = currentWaypoint->altitude;
if(currentWaypoint->next == NULL){
//Case for this being the last/only way point in the queue, avoid null pointers
*sp_Heading = followLastLineSegment(currentWaypoint, position, heading);
return currentWaypoint->index;
}
if(currentWaypoint->next->next == NULL){
//Case for only 2 way points remaining in queue
*sp_Heading = followLineSegment(currentWaypoint, position, heading);
return currentWaypoint->index;
}
PathData* targetWaypoint = currentWaypoint->next;
float targetCoordinates[3];
getCoordinates(targetWaypoint->longitude, targetWaypoint->latitude, (float*)&targetCoordinates);
targetCoordinates[2] = targetWaypoint->altitude;
PathData* nextWaypoint = targetWaypoint->next;
float nextCoordinates[3];
getCoordinates(nextWaypoint->longitude, nextWaypoint->latitude, (float*)&nextCoordinates);
nextCoordinates[2] = nextWaypoint->altitude;
float waypointDirection[3];
float norm = sqrt(pow(targetCoordinates[0] - waypointPosition[0],2) + pow(targetCoordinates[1] - waypointPosition[1],2) + pow(targetCoordinates[2] - waypointPosition[2],2));
waypointDirection[0] = (targetCoordinates[0] - waypointPosition[0])/norm;
waypointDirection[1] = (targetCoordinates[1] - waypointPosition[1])/norm;
waypointDirection[2] = (targetCoordinates[2] - waypointPosition[2])/norm;
float nextWaypointDirection[3];
float norm2 = sqrt(pow(nextCoordinates[0] - targetCoordinates[0],2) + pow(nextCoordinates[1] - targetCoordinates[1],2) + pow(nextCoordinates[2] - targetCoordinates[2],2));
nextWaypointDirection[0] = (nextCoordinates[0] - targetCoordinates[0])/norm2;
nextWaypointDirection[1] = (nextCoordinates[1] - targetCoordinates[1])/norm2;
nextWaypointDirection[2] = (nextCoordinates[2] - targetCoordinates[2])/norm2;
float turningAngle = acos(-deg2rad(waypointDirection[0] * nextWaypointDirection[0] + waypointDirection[1] * nextWaypointDirection[1] + waypointDirection[2] * nextWaypointDirection[2]));
float tangentFactor = targetWaypoint->radius/tan(turningAngle/2);
if (orbitPathStatus == PATH){
float halfPlane[3];
halfPlane[0] = targetCoordinates[0] - tangentFactor * waypointDirection[0];
halfPlane[1] = targetCoordinates[1] - tangentFactor * waypointDirection[1];
halfPlane[2] = targetCoordinates[2] - tangentFactor * waypointDirection[2];
float dotProduct = waypointDirection[0] * (position[0] - halfPlane[0]) + waypointDirection[1] * (position[1] - halfPlane[1]) + waypointDirection[2] * (position[2] - halfPlane[2]);
if (dotProduct > 0){
orbitPathStatus = ORBIT;
if (targetWaypoint->type == HOLD_WAYPOINT)
{
inHold = true;
}
}
*sp_Heading = (int)followStraightPath((float*)&waypointDirection, (float*)targetCoordinates, (float*)position, heading);
}
else{
float halfPlane[3];
halfPlane[0] = targetCoordinates[0] + tangentFactor * nextWaypointDirection[0];
halfPlane[1] = targetCoordinates[1] + tangentFactor * nextWaypointDirection[1];
halfPlane[2] = targetCoordinates[2] + tangentFactor * nextWaypointDirection[2];
char turnDirection = waypointDirection[0] * nextWaypointDirection[1] - waypointDirection[1] * nextWaypointDirection[0]>0?1:-1;
float euclideanWaypointDirection = sqrt(pow(nextWaypointDirection[0] - waypointDirection[0],2) + pow(nextWaypointDirection[1] - waypointDirection[1],2) + pow(nextWaypointDirection[2] - waypointDirection[2],2)) * ((nextWaypointDirection[0] - waypointDirection[0]) < 0?-1:1) * ((nextWaypointDirection[1] - waypointDirection[1]) < 0?-1:1) * ((nextWaypointDirection[2] - waypointDirection[2]) < 0?-1:1);
float turnCenter[3];
turnCenter[0] = targetCoordinates[0] + (tangentFactor * (nextWaypointDirection[0] - waypointDirection[0])/euclideanWaypointDirection);
turnCenter[1] = targetCoordinates[1] + (tangentFactor * (nextWaypointDirection[1] - waypointDirection[1])/euclideanWaypointDirection);
turnCenter[2] = targetCoordinates[2] + (tangentFactor * (nextWaypointDirection[2] - waypointDirection[2])/euclideanWaypointDirection);
// if target waypoint is a hold waypoint the plane will follow the orbit until the break hold method is called
if (inHold == true) {
*sp_Heading = (int)followOrbit((float*) &turnCenter, targetWaypoint->radius, turnDirection, (float*)position, heading);
return currentWaypoint->index;
}
float dotProduct = nextWaypointDirection[0] * (position[0] - halfPlane[0]) + nextWaypointDirection[1] * (position[1] - halfPlane[1]) + nextWaypointDirection[2] * (position[2] - halfPlane[2]);
if (dotProduct > 0){
orbitPathStatus = PATH;
return targetWaypoint->index;
}
//If two waypoints are parallel to each other (no turns)
if (euclideanWaypointDirection == 0){
orbitPathStatus = PATH;
return targetWaypoint->index;
}
*sp_Heading = (int)followOrbit((float*) &turnCenter,targetWaypoint->radius, turnDirection, (float*)position, heading);
}
return currentWaypoint->index;
}
#endif
int followLineSegment(PathData* currentWaypoint, float* position, float heading){
float waypointPosition[3];
getCoordinates(currentWaypoint->longitude, currentWaypoint->latitude, (float*)&waypointPosition);
waypointPosition[2] = currentWaypoint->altitude;
PathData* targetWaypoint = currentWaypoint->next;
float targetCoordinates[3];
getCoordinates(targetWaypoint->longitude, targetWaypoint->latitude, (float*)&targetCoordinates);
targetCoordinates[2] = targetWaypoint->altitude;
float waypointDirection[3];
float norm = sqrt(pow(targetCoordinates[0] - waypointPosition[0],2) + pow(targetCoordinates[1] - waypointPosition[1],2) + pow(targetCoordinates[2] - waypointPosition[2],2));
waypointDirection[0] = (targetCoordinates[0] - waypointPosition[0])/norm;
waypointDirection[1] = (targetCoordinates[1] - waypointPosition[1])/norm;
waypointDirection[2] = (targetCoordinates[2] - waypointPosition[2])/norm;
return (int)followStraightPath((float*)&waypointDirection, (float*)targetCoordinates, (float*)position, heading);
}
int followLastLineSegment(PathData* currentWaypoint, float* position, float heading){
float waypointPosition[3];
waypointPosition[0] = position[0];
waypointPosition[1] = position[1];
waypointPosition[2] = interchip_send_buffer.pm_data.altitude;
PathData* targetWaypoint = currentWaypoint;
float targetCoordinates[3];
getCoordinates(targetWaypoint->longitude, targetWaypoint->latitude, (float*)&targetCoordinates);
targetCoordinates[2] = targetWaypoint->altitude;
float waypointDirection[3];
float norm = sqrt(pow(targetCoordinates[0] - waypointPosition[0],2) + pow(targetCoordinates[1] - waypointPosition[1],2) + pow(targetCoordinates[2] - waypointPosition[2],2));
waypointDirection[0] = (targetCoordinates[0] - waypointPosition[0])/norm;
waypointDirection[1] = (targetCoordinates[1] - waypointPosition[1])/norm;
waypointDirection[2] = (targetCoordinates[2] - waypointPosition[2])/norm;
float dotProduct = waypointDirection[0] * (position[0] - targetCoordinates[0]) + waypointDirection[1] * (position[1] - targetCoordinates[1]) + waypointDirection[2] * (position[2] - targetCoordinates[2]);
if (dotProduct > 0){
returnHome = 1;
}
return (int)followStraightPath((float*)&waypointDirection, (float*)targetCoordinates, (float*)position, heading);
}
void DriverRadarPositioner::paint(QPainter *p, bool selected)
{
if (!driverData || driverData->getCarID() < 1)
return;
LTPackets::EventType eType = EventData::getInstance().getEventType();
if (!excluded && (eType != LTPackets::RACE_EVENT || !driverData->isRetired()))
{
QPoint point = getCoordinates();
QColor drvColor = ColorsManager::getInstance().getCarColor(driverData->getNumber());
p->setBrush(QBrush(drvColor));
QPen pen(drvColor);
if (driverData->getPosition() == 1)
{
pen.setColor(ColorsManager::getInstance().getDefaultColor(LTPackets::GREEN));
pen.setWidth(3);
}
if (driverData->isRetired() || qualiOut)
{
pen.setColor(QColor(255, 0, 0));
pen.setWidth(3);
}
if (selected)
{
pen.setColor(ColorsManager::getInstance().getDefaultColor(LTPackets::YELLOW));
pen.setWidth(3);
}
p->setPen(pen);
QPixmap helmet = ImagesFactory::getInstance().getHelmetsFactory().getHelmet(driverData->getNumber(), 24);
p->drawPixmap(point.x()-15, point.y()-15, helmet);
p->setFont(QFont("Arial", 8, 75));
QString number = SeasonData::getInstance().getDriverShortName(driverData->getDriverName());//QString::number(driverData->getNumber());
p->setBrush(drvColor);
if (inPits)
// p->drawRoundedRect(point.x()-15, point.y()-8, helmet.width()-4, 14, 4, 4);
p->drawRoundedRect(point.x()-16-helmet.width(), point.y(), helmet.width(), 14, 4, 4);
else
p->drawRoundedRect(point.x()-12, point.y()+15, helmet.width(), 14, 4, 4);
p->setPen(ColorsManager::getInstance().getColor(LTPackets::BACKGROUND));
int numX = point.x() - 18 + p->fontMetrics().width(number)/2;
int numY = point.y() + 20 + p->fontMetrics().height()/2;
// int numX = point.x() - 15 + p->fontMetrics().width(number)/2;
// int numY = point.y() - 2 + p->fontMetrics().height()/2;
if (inPits)
{
numX = point.x() - helmet.width() - 22 + p->fontMetrics().width(number)/2;
numY = point.y() + 5 + p->fontMetrics().height()/2;
}
p->drawText(numX, numY, number);
// p->setPen(QColor(20, 20, 20));
// QString name = SeasonData::getInstance().getDriverShortName(driverData->getDriverName());
// int numX = point.x() - p->fontMetrics().width(name)/2;
// int numY = point.y() + 12 - p->fontMetrics().height()/2;
// p->setFont(QFont("Arial", 10, 75));
// p->drawText(numX, numY, name);
}
}