void DrawPointTool::updateStatusText()
{
if (isDragging())
{
auto angle = qRadiansToDegrees(preview_object->getRotation());
setStatusBarText( trUtf8("<b>Angle:</b> %1° ").arg(QLocale().toString(angle, 'f', 1)) + QLatin1String("| ") +
tr("<b>%1</b>: Fixed angles. ").arg(ModifierKey::control()) );
}
else if (static_cast<const PointSymbol*>(preview_object->getSymbol())->isRotatable())
{
auto parts = QStringList {
tr("<b>Click</b>: Create a point object."),
tr("<b>Drag</b>: Create an object and set its orientation."),
};
auto angle = qRadiansToDegrees(preview_object->getRotation());
if (!qIsNull(angle))
{
parts.push_front(trUtf8("<b>Angle:</b> %1° ").arg(QLocale().toString(angle, 'f', 1)) + QLatin1Char('|'));
parts.push_back(tr("<b>%1, 0</b>: Reset rotation.").arg(ModifierKey::escape()));
}
setStatusBarText(parts.join(QLatin1Char(' ')));
}
else
{
// Same as click_text above.
setStatusBarText(tr("<b>Click</b>: Create a point object."));
}
}
/*!
\since 5.5
Calculates \a roll, \a pitch, and \a yaw Euler angles (in degrees)
that corresponds to this quaternion.
\sa fromEulerAngles()
*/
void QQuaternion::getEulerAngles(float *pitch, float *yaw, float *roll) const
{
Q_ASSERT(pitch && yaw && roll);
// Algorithm from:
// http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q37
float xx = xp * xp;
float xy = xp * yp;
float xz = xp * zp;
float xw = xp * wp;
float yy = yp * yp;
float yz = yp * zp;
float yw = yp * wp;
float zz = zp * zp;
float zw = zp * wp;
const float lengthSquared = xx + yy + zz + wp * wp;
if (!qFuzzyIsNull(lengthSquared - 1.0f) && !qFuzzyIsNull(lengthSquared)) {
xx /= lengthSquared;
xy /= lengthSquared; // same as (xp / length) * (yp / length)
xz /= lengthSquared;
xw /= lengthSquared;
yy /= lengthSquared;
yz /= lengthSquared;
yw /= lengthSquared;
zz /= lengthSquared;
zw /= lengthSquared;
}
*pitch = std::asin(-2.0f * (yz - xw));
if (*pitch < M_PI_2) {
if (*pitch > -M_PI_2) {
*yaw = std::atan2(2.0f * (xz + yw), 1.0f - 2.0f * (xx + yy));
*roll = std::atan2(2.0f * (xy + zw), 1.0f - 2.0f * (xx + zz));
} else {
// not a unique solution
*roll = 0.0f;
*yaw = -std::atan2(-2.0f * (xy - zw), 1.0f - 2.0f * (yy + zz));
}
} else {
// not a unique solution
*roll = 0.0f;
*yaw = std::atan2(-2.0f * (xy - zw), 1.0f - 2.0f * (yy + zz));
}
*pitch = qRadiansToDegrees(*pitch);
*yaw = qRadiansToDegrees(*yaw);
*roll = qRadiansToDegrees(*roll);
}
/*!
\since 5.5
Extracts a 3D axis (\a x, \a y, \a z) and a rotating angle \a angle (in degrees)
that corresponds to this quaternion.
\sa fromAxisAndAngle()
*/
void QQuaternion::getAxisAndAngle(float *x, float *y, float *z, float *angle) const
{
Q_ASSERT(x && y && z && angle);
// The quaternion representing the rotation is
// q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
float length = xp * xp + yp * yp + zp * zp;
if (!qFuzzyIsNull(length)) {
*x = xp;
*y = yp;
*z = zp;
if (!qFuzzyIsNull(length - 1.0f)) {
length = std::sqrt(length);
*x /= length;
*y /= length;
*z /= length;
}
*angle = 2.0f * std::acos(wp);
} else {
// angle is 0 (mod 2*pi), so any axis will fit
*x = *y = *z = *angle = 0.0f;
}
*angle = qRadiansToDegrees(*angle);
}
void BurstShot::doLogic(double delta)
{
Q_UNUSED(delta);
//set target to point directly above shot
target.rx() = getX();
target.ry() = getY()-1000;
targetDistance = pos.distanceTo(target);
//target closest enemy if exists
if(game->getEnemyEntities().size() > 0){
for(e_ptr enemy: game->getEnemyEntities()){
if(enemy->getType() == ENEMY){//if the target is not a bullet
if(pos.distanceTo(enemy->getPos()) < targetDistance){
//if distance to enemy is shorter than current target
//set it as new target
target.rx() = enemy->getX();
target.ry() = enemy->getY();
targetDistance = pos.distanceTo( target);
}
}
}
}
angle = fmod((angle + 360),360.0f);//bind range between 0 and 360
targetAngle = qRadiansToDegrees(qAtan2(target.x()-getX(),target.y()-getY()));
angleDistance = targetAngle - angle;
if (angleDistance > 180) {
angleDistance -= 360;
} else if (angleDistance < -180) {
angleDistance += 360;
}
}
BurstShot::BurstShot(GameState *game, int x, int y, float angle,
polarType polarity): ShotEntity(game,x,y,polarity)
{
// this->game = game;
// this->x = x;
// this->y = y;
// this->polarity = polarity;
this->angle = angle;
this->dmg = 10;
shotSpeed = 750;
rotatesSpeed = 1080;
type = BURST;
// ticker = 0;
// ticks = 0;
// lastTick = 0;
// angle = 90;
trail.prepend(Point(x,y));
target.rx() = getX();
target.ry() = getY()-1000;
targetAngle = qRadiansToDegrees(qAtan2(target.x()-getX(),target.y()-getY()));
angleDistance = targetAngle - angle;
if (angleDistance > 180) {
angleDistance -= 360;
} else if (angleDistance < -180) {
angleDistance += 360;
}
}
void Connexion::paint(QPainter *painter,
const QStyleOptionGraphicsItem *option,QWidget *widget)
{
Q_UNUSED(option);
Q_UNUSED(widget);
painter->save();
//MODIFICATION DES COORDONNEES DE DESSINS
qreal angle = qRadiansToDegrees(qAtan2(n1.y()-n2.y(),n1.x()-n2.x()))+90;
painter->rotate(angle);
painter->translate(0, n1.getRadius());
//DESSIN DE LA CONNEXION
painter->save();
QColor c1 = n1.getOwner() != 0 ? n1.getOwner()->getColor() : VACANT_COLOR;
QColor c2 = n2.getOwner() != 0 ? n2.getOwner()->getColor() : VACANT_COLOR;
QLinearGradient linearGrad(0, 0, 0, distance);
linearGrad.setColorAt(0, c1);
linearGrad.setColorAt(1, c2);
painter->setPen(Qt::NoPen);
painter->fillRect(-2, 0, 4, distance, linearGrad);
painter->restore();
//DESSIN DES SQUADS
foreach(Squad *s, lstSquad1To2)
{
squadPatern(painter, s);
}
double DataMotion::calculerAngleA(QVector<float> A, QVector<float> B, QVector<float> C)
{
double Xa = A.at(Coordonnees::X);
double Xb = B.at(Coordonnees::X);
double Xc = C.at(Coordonnees::X);
double Ya = A.at(Coordonnees::Y);
double Yb = B.at(Coordonnees::Y);
double Yc = C.at(Coordonnees::Y);
double Za = A.at(Coordonnees::Z);
double Zb = B.at(Coordonnees::Z);
double Zc = C.at(Coordonnees::Z);
double ABx = Xb - Xa;
double ABy = Yb - Ya;
double ABz = Zb - Za;
double ACx = Xc - Xa;
double ACy = Yc - Ya;
double ACz = Zc - Za;
double numerateur = (ABx) * (ACx) + (ABy) * (ACy) + (ABz) * (ACz);
double denominateur = qSqrt(qPow(ABx, 2) + qPow(ABy, 2) + qPow(ABz, 2)) * qSqrt(qPow(ACx, 2) + qPow(ACy, 2) + qPow(ACz, 2));
return qRadiansToDegrees(qCos(numerateur / denominateur));
}
/**
* @param x x coordinate
* @param y y coordinate
*/
void CanvasView::moveDrag(int x, int y)
{
const int dx = _dragx - x;
const int dy = _dragy - y;
if(_isdragging==DRAG_ROTATE) {
qreal preva = qAtan2( width()/2 - _dragx, height()/2 - _dragy );
qreal a = qAtan2( width()/2 - x, height()/2 - y );
setRotation(rotation() + qRadiansToDegrees(preva-a));
} else if(_isdragging==DRAG_ZOOM) {
if(dy!=0) {
float delta = qBound(-1.0, dy / 100.0, 1.0);
if(delta>0) {
setZoom(_zoom * (1+delta));
} else if(delta<0) {
setZoom(_zoom / (1-delta));
}
}
} else if(_isdragging==DRAG_QUICKADJUST1) {
if(dy!=0) {
float delta = qBound(-2.0, dy / 10.0, 2.0);
doQuickAdjust1(delta);
}
} else {
QScrollBar *ver = verticalScrollBar();
ver->setSliderPosition(ver->sliderPosition()+dy);
QScrollBar *hor = horizontalScrollBar();
hor->setSliderPosition(hor->sliderPosition()+dx);
}
_dragx = x;
_dragy = y;
}
void WireCreator::collision(QVector3D point, QPair<QVector3D, QVector3D> box)
{
auto collidedPoint= point;
//qDebug()<<point<<" "<<collidedPoint;
collidedPoint.setX(0);
auto axis = QVector3D::crossProduct(collidedPoint, QVector3D(0.0f, 1.0f, 0.0f));
float angle= calcMinimumAngle(collidedPoint, box);
angle=qRadiansToDegrees(angle);
//qDebug()<<"angle: "<<angle;
//qDebug()<<"axis: "<<axis;
QQuaternion quat= QQuaternion::fromAxisAndAngle(axis, angle);
QMatrix3x3 rot3= quat.toRotationMatrix();
QMatrix4x4 result;
for(int i=0;i<3;i++){
for(int j=0; j<3; j++){
if(i<3 && j<3)result(i,j)=rot3(i,j);
}
}
//currPath=applyMatrix(currPath, result);
//qDebug()<<"collision handled";
emit collisionHandled();
}
float Collider::angleBetweenVectors(QVector2D v1, QVector2D v2)
{
float dot = QVector2D::dotProduct(v1,v2);
float det = v1.x ()*v2.y() - v1.y ()*v2.x();
return (-qRadiansToDegrees(atan2(det,dot)));
}
void RotateTool::updateStatusText()
{
QString text;
if (isDragging())
{
auto display_rotation = qRadiansToDegrees(current_rotation);
if (display_rotation <= -180)
display_rotation += 360;
else if (display_rotation > 180)
display_rotation -= 360;
else if (display_rotation > -0.05 && display_rotation < 0.0)
display_rotation = +0.0;
text = trUtf8("<b>Rotation:</b> %1° ").arg(QLocale().toString(display_rotation, 'f', 1));
if (!angle_helper->isActive())
text += QLatin1String("| ");
}
else
{
text = tr("<b>Click</b>: Set the center of rotation. ") +
tr("<b>Drag</b>: Rotate the selected objects. ");
}
if (!angle_helper->isActive())
{
text += tr("<b>%1</b>: Fixed angles. ").arg(ModifierKey::control());
}
setStatusBarText(text);
}
void RotateTool::drawImpl(QPainter* painter, MapWidget* widget)
{
const auto center = widget->mapToViewport(rotation_center);
painter->save();
if (isDragging())
{
painter->translate(center);
painter->rotate(qRadiansToDegrees(current_rotation));
painter->translate(-center);
}
map()->drawSelection(painter, true, widget);
painter->restore();
const auto saved_hints = painter->renderHints();
painter->setRenderHint(QPainter::Antialiasing, true);
painter->setPen(Qt::white);
painter->setBrush(Qt::NoBrush);
/// \todo Use dpi-scaled dimensions
painter->drawEllipse(center, 3, 3);
painter->setPen(Qt::black);
painter->drawEllipse(center, 4, 4);
painter->setRenderHints(saved_hints);
}
void CondorDatasource::rxValue(QString ¶m, QString &value) {
bool ok = false;
double val = value.toDouble(&ok);
if(ok) {
m_updatesReceived++;
} else {
qDebug() << Q_FUNC_INFO << "Failed to read value for " << param;
return;
}
QString refName = refMap.key(param);
if(refName.isEmpty()) return;
// Magic happens here
if(param == "pitch"
|| param == "bank"
|| param == "yaw"
|| param == "slipball")
val = qRadiansToDegrees(val);
if(param == "bank" || param == "slipball") val = -val;
// Other units seem to be ok..
//
for(FloatDataRef *ref : floatRefs) {
if(ref->name() == refName) {
qDebug() << Q_FUNC_INFO << "Updating ref " << ref->name() << " to " << val;
ref->updateValue(val);
}
}
}
void QVRVNCViewer::createSceneGeometry(QVector<QVector3D>& positions,
QVector<QVector2D>& texcoords, QVector<unsigned int>& indices)
{
if (_screenType == screenTypeWall) {
QVector3D bl(_screenWall[0], _screenWall[1], _screenWall[2]);
QVector3D br(_screenWall[3], _screenWall[4], _screenWall[5]);
QVector3D tl(_screenWall[6], _screenWall[7], _screenWall[8]);
QVector3D tr = br + (tl - bl);
positions.append(bl);
positions.append(br);
positions.append(tr);
positions.append(tl);
texcoords.append(QVector2D(0.0f, 1.0f));
texcoords.append(QVector2D(1.0f, 1.0f));
texcoords.append(QVector2D(1.0f, 0.0f));
texcoords.append(QVector2D(0.0f, 0.0f));
indices.append(0);
indices.append(1);
indices.append(3);
indices.append(1);
indices.append(2);
indices.append(3);
} else if (_screenType == screenTypeCylinder) {
QVector3D center(_screenCylinder[0], _screenCylinder[1], _screenCylinder[2]);
QVector3D up(_screenCylinder[3], _screenCylinder[4], _screenCylinder[5]);
float radius = _screenCylinder[6];
float phiCenter = _screenCylinder[7];
float phiRange = _screenCylinder[8];
float thetaRange = _screenCylinder[9];
float py = radius * std::tan(thetaRange / 2.0f);
QVector3D rotAxis = QVector3D::crossProduct(QVector3D(0.0f, 1.0f, 0.0f), up);
float rotAngle = qRadiansToDegrees(
std::acos(QVector3D::dotProduct(QVector3D(0.0f, 1.0f, 0.0f), up)
/ std::sqrt(QVector3D::dotProduct(up, up))));
QMatrix4x4 M;
M.rotate(90.0f, 0.0f, 1.0f, 0.0f);
M.rotate(rotAngle, rotAxis);
M.translate(center);
const int N = 1000;
for (int x = 0; x <= N; x++) {
float xf = x / static_cast<float>(N);
float phi = phiCenter + (xf - 0.5f) * phiRange;
float px = radius * std::cos(phi);
float pz = radius * std::sin(phi);
positions.append(M * QVector3D(px, py, pz));
texcoords.append(QVector2D(xf, 0.0f));
positions.append(M * QVector3D(px, -py, pz));
texcoords.append(QVector2D(xf, 1.0f));
if (x > 0) {
indices.append(2 * (x - 1));
indices.append(2 * (x - 1) + 1);
indices.append(2 * x + 1);
indices.append(2 * (x - 1));
indices.append(2 * x + 1);
indices.append(2 * x);
}
}
}
}
TextRenderable::TextRenderable(const TextSymbol* symbol, const TextObject* text_object, const MapColor* color, double anchor_x, double anchor_y)
: Renderable { color }
, anchor_x { anchor_x }
, anchor_y { anchor_y }
, rotation { 0.0 }
, scale_factor { symbol->getFontSize() / TextSymbol::internal_point_size }
{
path.setFillRule(Qt::WindingFill); // Otherwise, when text and an underline intersect, holes appear
const QFont& font(symbol->getQFont());
const QFontMetricsF& metrics(symbol->getFontMetrics());
int num_lines = text_object->getNumLines();
for (int i=0; i < num_lines; i++)
{
const TextObjectLineInfo* line_info = text_object->getLineInfo(i);
double line_y = line_info->line_y;
double underline_x0 = 0.0;
double underline_y0 = line_info->line_y + metrics.underlinePos();
double underline_y1 = underline_y0 + metrics.lineWidth();
auto num_parts = line_info->part_infos.size();
for (size_t j=0; j < num_parts; j++)
{
const TextObjectPartInfo& part(line_info->part_infos.at(j));
if (font.underline())
{
if (j > 0)
{
// draw underline for gap between parts as rectangle
// TODO: watch out for inconsistency between text and gap underline
path.moveTo(underline_x0, underline_y0);
path.lineTo(part.part_x, underline_y0);
path.lineTo(part.part_x, underline_y1);
path.lineTo(underline_x0, underline_y1);
path.closeSubpath();
}
underline_x0 = part.part_x;
}
path.addText(part.part_x, line_y, font, part.part_text);
}
}
QTransform t { 1.0, 0.0, 0.0, 1.0, anchor_x, anchor_y };
t.scale(scale_factor, scale_factor);
auto rotation_rad = text_object->getRotation();
if (!qIsNull(rotation_rad))
{
rotation = -qRadiansToDegrees(rotation_rad);
t.rotate(rotation);
}
extent = t.mapRect(path.controlPointRect());
}
void Robotcl::updateRobotInfo(SSL_DetectionRobot detectedRobot)
{
px = detectedRobot.x();
py = detectedRobot.y();
if(detectedRobot.has_orientation())
Orientation = qRadiansToDegrees( detectedRobot.orientation() );
else
Orientation = -1;
}
void AutoFindPath::onTimeout()
{
qreal x1 = tempPointF_.x();
qreal y1 = tempPointF_.y();
qreal x2 = toPointF_.x();
qreal y2 = toPointF_.y();
qreal xNegative = (x2 - x1) > 0 ? 1.0 : -1.0;
qreal yNegative = (y2 - y1) > 0 ? 1.0 : -1.0;
xNegative = 1.0;
yNegative = 1.0;
qreal fromDistance = qSqrt(qPow(x2-x1, 2) + qPow(y2-y1, 2));
qreal xdif = xNegative * pace_ * (x2 - x1) / fromDistance;
qreal ydif = yNegative * pace_ * (y2 - y1) / fromDistance;
qreal x3 = x1 + xdif;
qreal y3 = y1 + ydif;
qreal toDistance = qSqrt(qPow(x3-x2, 2) + qPow(y3-y2, 2));
// qDebug() << tempPointF_ << toPointF_<< xNegative << yNegative<<fromDistance<<toDistance<< xdif << ydif;
tempPointF_.setX(x3);
tempPointF_.setY(y3);
if (toDistance <= radius_) {
// qDebug() << "STOP!";
timer_.stop();
if (rolePathMap_.contains(role_)) {
rolePathMap_.remove(role_);
}
this->deleteLater();
}
qreal ration = qAtan2(ydif, xdif);
qreal degree = qRadiansToDegrees(ration);
// qDebug() << ration << degree;
role_->setRotation(degree + 90);
// setRotation(rotation() + dx);
role_->setPos(tempPointF_);
// role_->setEyeDirection();
// role_->update();
// QRectF rect = role_->mapRectToScene(role_->boundingRect());
QPointF pos = role_->mapToScene(role_->pos());
QRectF rect;
rect.setTopLeft(pos);
rect.setWidth(20);
rect.setHeight(20);
role_->scene()->invalidate();
}
void Scene::addCar(Car *car) {
Point<double> coordinates = car->getCoordinates();
double x = coordinates.getX();
double y = coordinates.getY();
QPixmap pixmap = Settings::getInstance().getCarPixmap(car->getColor());
QGraphicsPixmapItem *pixmapItem = addPixmap(pixmap);
pixmapItem->setOffset(-0.5 * QPointF(pixmap.width(), pixmap.height()));
pixmapItem->setPos(x, y);
pixmapItem->setRotation(qRadiansToDegrees(car->getDirection()));
carsItems[car] = pixmapItem;
}
void TrikGyroscopeAdapter::countTilt(const QVector<int> &oldFormat)
{
const quint64 timeStamp = getTimeValue(mModel.data());
if (!mTimeInited) {
mTimeInited = true;
mLastUpdateTimeStamp = timeStamp;
} else {
mResult[0] = oldFormat[0];
mResult[1] = oldFormat[1];
mResult[2] = oldFormat[2];
mResult[3] = convertToTrikRuntimeTime(timeStamp);
const qreal scale = static_cast<qreal>(timeStamp - mLastUpdateTimeStamp) / twoDModel::timeQuant;
const qreal x = static_cast<qreal>(mResult[0]) / twoDModel::gyroscopeConstant * scale;
const qreal y = static_cast<qreal>(mResult[1]) / twoDModel::gyroscopeConstant * scale;
const qreal z = static_cast<qreal>(mResult[2]) / twoDModel::gyroscopeConstant * scale;
mLastUpdateTimeStamp = timeStamp;
const float c1 = static_cast<float>(qCos(x / 2));
const float s1 = static_cast<float>(qSin(x / 2));
const float c2 = static_cast<float>(qCos(y / 2));
const float s2 = static_cast<float>(qSin(y / 2));
const float c3 = static_cast<float>(qCos(z / 2));
const float s3 = static_cast<float>(qSin(z / 2));
QQuaternion deltaQuaternion;
deltaQuaternion.setScalar(c1 * c2 * c3 + s1 * s2 * s3);
deltaQuaternion.setX(s1 * c2 * c3 - c1 * s2 * s3);
deltaQuaternion.setY(c1 * s2 * c3 + s1 * c2 * s3);
deltaQuaternion.setZ(c1 * c2 * s3 - s1 * s2 * c3);
mQuaternion *= deltaQuaternion;
mQuaternion.normalize();
mResult[4] = static_cast<int>(degreeToMilidegree(qRadiansToDegrees(getPitch<qreal>(mQuaternion))));
mResult[5] = static_cast<int>(degreeToMilidegree(qRadiansToDegrees(getRoll<qreal>(mQuaternion))));
mResult[6] = static_cast<int>(degreeToMilidegree(qRadiansToDegrees(getYaw<qreal>(mQuaternion))));
}
}
void _drawArrow(QPainter *p, const QPointF & A, const QPointF & B) //根据传入线段的端点画箭头
{
QPointF temB(std::sqrt(std::pow((A.x()-B.x()), 2) + std::pow(A.y()-B.y(), 2)),0);
p->save();
p->setPen (QPen (Qt::darkYellow));
//想不通，要先平移旋转！
p->translate(A);
p->rotate(qRadiansToDegrees(std::atan2(B.y()-A.y(),B.x()-A.x())));
p->drawLine(temB.x(),temB.y(),temB.x()-5,temB.y()+4);
p->drawLine(temB.x(),temB.y(),temB.x()-5,temB.y()-4);
p->restore();
}
void PhysicalItem::advance(int phase) {
// phase 0: The scene is about to advance
// phase 1: The scene is advancing
if (phase == 0) return;
if (body_ != nullptr and body_->IsAwake()) {
setPos(gameScene()->mapFromWorld(body_->GetPosition()));
setRotation(qRadiansToDegrees(body_->GetAngle()));
}
QGraphicsItem::advance(phase); // Advance children
}
void Body::updateAfterPhysics() {
// Don't update inactive or static bodies, as they can't possibly have moved as a result of
// internal physics updates. Setting Actor position and transform fires off a bunch of signals
// and invalidates internal state, which can get expensive.
if (!isActive() || getBodyType() == StaticBody) {
return;
}
Actor* actor = getActor();
if (actor) {
actor->setPosition(getPosition());
actor->setRotation(qRadiansToDegrees(getAngleInRadians()));
}
}
void ray_options::restore()
{
blockSignals(true);
ui->red_slider->setValue(initial_color.red());
ui->green_slider->setValue(initial_color.green());
ui->blue_slider->setValue(initial_color.blue());
ui->x_spin_box->setValue(initial_pos.x());
ui->y_spin_box->setValue(initial_pos.y());
set_color(initial_color.red(),
initial_color.green(),
initial_color.blue());
set_angle(qRadiansToDegrees(initial_dir));
set_pos(initial_pos.x(), initial_pos.y());
blockSignals(false);
}
bool RotateTool::keyPress(QKeyEvent* event)
{
switch (event->key())
{
case Qt::Key_Control:
angle_helper->clearAngles();
angle_helper->addDefaultAnglesDeg(-qRadiansToDegrees(original_rotation));
angle_helper->setActive(true, rotation_center);
reapplyConstraintHelpers();
break;
default:
; // nothing
}
return false;
}
void Window::mouseMoveEvent(QMouseEvent* event) {
if (m_mouse_draging) {
//Transform mouse coordinates from pixels to world units
QVector2D mouse_current_in_pixels = QVector2D(event->pos());
QVector2D window_center = 0.5f * QVector2D(size().width(), size().height());
QVector2D scale_factors = QVector2D(2.0f / size().width(), -2.0f / size().height());
QVector2D mouse_current = scale_factors * (mouse_current_in_pixels - window_center);
QVector2D mouse_start = scale_factors * (m_mouse_start_drag - window_center);
//Now we have mouse_current and mouse_start in world coordinates
//Use the algorithm
QVector3D v_1 = QVector3D(mouse_current, projection_on_curve(mouse_current));
QVector3D v_2 = QVector3D(mouse_start, projection_on_curve(mouse_start));
v_1.normalize();
v_2.normalize();
QVector3D axis = QVector3D::crossProduct(v_1, v_2);
float angle = qAcos(QVector3D::dotProduct(v_1, v_2));
m_new_rotation = QQuaternion::fromAxisAndAngle(axis, qRadiansToDegrees(angle));
m_new_rotation.normalize();
}
}
Robotcl::Robotcl(SSL_DetectionRobot detectedRobot, Teams team, QObject *parent):QObject(parent)
{
px = detectedRobot.x();
py = detectedRobot.y();
Team = team;
if(detectedRobot.has_height())
Height = detectedRobot.height();
else
Height = -1;
if(detectedRobot.has_orientation())
Orientation = qRadiansToDegrees( detectedRobot.orientation() );
else
Orientation = -1;
if(detectedRobot.has_robot_id())
ID = detectedRobot.robot_id();
else
ID = -1;
diagonal = 300/*170*/;
}
/**
* @param x x coordinate
* @param y y coordinate
*/
void CanvasView::moveDrag(int x, int y)
{
const int dx = _dragx - x;
const int dy = _dragy - y;
if(_isdragging==ROTATE) {
qreal preva = qAtan2( width()/2 - _dragx, height()/2 - _dragy );
qreal a = qAtan2( width()/2 - x, height()/2 - y );
setRotation(rotation() + qRadiansToDegrees(preva-a));
} else {
QScrollBar *ver = verticalScrollBar();
ver->setSliderPosition(ver->sliderPosition()+dy);
QScrollBar *hor = horizontalScrollBar();
hor->setSliderPosition(hor->sliderPosition()+dx);
}
_dragx = x;
_dragy = y;
// notify of scene change
sceneChanged();
}
void Scene::updateCars() {
std::vector<Car*> cars = world->getCars();
std::set<Car*> carsSet(cars.begin(), cars.end());
for (const auto &car : cars) {
if (!carsItems.count(car)) {
addCar(car);
}
Point<double> coordinates = car->getCoordinates();
QGraphicsItem *rect = carsItems[car];
rect->setPos(coordinates.toQPoint());
rect->setRotation(qRadiansToDegrees(car->getDirection()));
}
std::vector<Car*> toRemoveCars;
for (const auto &carItem : carsItems) {
if (!carsSet.count(carItem.first)) {
toRemoveCars.push_back(carItem.first);
}
}
for (const auto &car : toRemoveCars) {
removeItem(carsItems[car]);
carsItems.erase(car);
}
}
//==============================================================
// Перевод радианов в градусы
//==============================================================
double radToDeg(const double &rad)
{
return qRadiansToDegrees(rad);
}
void Remapper::dataIn( Node::DataType type, QMutex *mutex, void *data, size_t bytes, qint64 timeStamp ) {
switch( type ) {
case DataType::Input: {
// Copy incoming data to our own buffer
GamepadState gamepad;
{
mutex->lock();
gamepad = *reinterpret_cast<GamepadState *>( data );
mutex->unlock();
}
int instanceID = gamepad.instanceID;
int joystickID = gamepad.joystickID;
QString GUID( QByteArray( reinterpret_cast<const char *>( gamepad.GUID.data ), 16 ).toHex() );
// Do initial calibration if not done yet
{
if( deadzoneFlag[ GUID ] ) {
deadzoneFlag[ GUID ] = false;
// Apply default value
for( int i = 0; i < gamepad.joystickNumAxes; i++ ) {
deadzones[ GUID ][ i ] = 10000;
// Check analog value at this moment. If its magnitude is less than 30000 then it's most likely
// an analog stick. Otherwise, it might be a trigger (with a centered value of -32768)
deadzoneModes[ GUID ][ i ] = ( qAbs( static_cast<int>( gamepad.joystickAxis[ i ] ) ) < 30000 );
}
// TODO: Replace with stored value from disk
}
}
// Inject deadzone settings into gamepad
{
for( int i = 0; i < gamepad.joystickNumAxes; i++ ) {
gamepad.deadzone[ i ] = deadzones[ GUID ][ i ];
gamepad.deadzoneMode[ i ] = deadzoneModes[ GUID ][ i ];
}
}
// Send raw joystick data to the model
// Do this before we apply joystick deadzones so the user sees completely unprocessed data
{
// Copy current gamepad into buffer
this->mutex.lock();
gamepadBuffer[ gamepadBufferIndex ] = gamepad;
this->mutex.unlock();
// Send buffer on its way
emit rawJoystickData( &( this->mutex ), reinterpret_cast<void *>( &gamepadBuffer[ gamepadBufferIndex ] ) );
// Increment the index
gamepadBufferIndex = ( gamepadBufferIndex + 1 ) % 100;
}
// Apply deadzones to each stick and both triggers independently
{
qreal deadzone = 0.0;
bool deadzoneMode = false;
for( int i = 0; i < 4; i++ ) {
int xAxis;
int yAxis;
// For the analog sticks, average the underlying joystick axes together to get the final deadzone value
// If either axis has deadzone mode set to true, it'll apply to both
// FIXME: If users complain about this, expand the code to handle this case (one axis true and one axis false) and treat axes indepenently
switch( i ) {
case 0: {
xAxis = SDL_CONTROLLER_AXIS_LEFTX;
yAxis = SDL_CONTROLLER_AXIS_LEFTY;
Val val = gameControllerToJoystick[ GUID ][ Key( AXIS, SDL_CONTROLLER_AXIS_LEFTX ) ];
int axisID = val.second.first;
if( val.first == AXIS ) {
deadzone = deadzones[ GUID ][ axisID ];
deadzoneMode = deadzoneModes[ GUID ][ axisID ];
}
Val val2 = gameControllerToJoystick[ GUID ][ Key( AXIS, SDL_CONTROLLER_AXIS_LEFTY ) ];
axisID = val2.second.first;
if( val2.first == AXIS ) {
deadzone += deadzones[ GUID ][ axisID ];
deadzone /= 2.0;
deadzoneMode = deadzoneMode || deadzoneModes[ GUID ][ axisID ];
}
break;
}
case 1: {
xAxis = SDL_CONTROLLER_AXIS_RIGHTX;
yAxis = SDL_CONTROLLER_AXIS_RIGHTY;
Val val = gameControllerToJoystick[ GUID ][ Key( AXIS, SDL_CONTROLLER_AXIS_RIGHTX ) ];
int axisID = val.second.first;
if( val.first == AXIS ) {
deadzone = deadzones[ GUID ][ axisID ];
deadzoneMode = deadzoneModes[ GUID ][ axisID ];
}
Val val2 = gameControllerToJoystick[ GUID ][ Key( AXIS, SDL_CONTROLLER_AXIS_RIGHTY ) ];
axisID = val2.second.first;
if( val2.first == AXIS ) {
deadzone += deadzones[ GUID ][ axisID ];
deadzone /= 2.0;
deadzoneMode = deadzoneMode || deadzoneModes[ GUID ][ axisID ];
}
break;
}
// For simplicity, just map the triggers to the line y = x
case 2: {
xAxis = SDL_CONTROLLER_AXIS_TRIGGERLEFT;
yAxis = SDL_CONTROLLER_AXIS_TRIGGERLEFT;
Val val = gameControllerToJoystick[ GUID ][ Key( AXIS, SDL_CONTROLLER_AXIS_TRIGGERLEFT ) ];
int axisID = val.second.first;
if( val.first == AXIS ) {
deadzone = deadzones[ GUID ][ axisID ];
deadzoneMode = deadzoneModes[ GUID ][ axisID ];
}
break;
}
case 3: {
xAxis = SDL_CONTROLLER_AXIS_TRIGGERRIGHT;
yAxis = SDL_CONTROLLER_AXIS_TRIGGERRIGHT;
Val val = gameControllerToJoystick[ GUID ][ Key( AXIS, SDL_CONTROLLER_AXIS_TRIGGERRIGHT ) ];
int axisID = val.second.first;
if( val.first == AXIS ) {
deadzone = deadzones[ GUID ][ axisID ];
deadzoneMode = deadzoneModes[ GUID ][ axisID ];
}
break;
}
}
// Map from [-32768, 32767] to [0, 32767]
if( !deadzoneMode ) {
gamepad.axis[ xAxis ] /= 2;
gamepad.axis[ yAxis ] /= 2;
gamepad.axis[ xAxis ] += 16384;
gamepad.axis[ yAxis ] += 16384;
}
// Get axis coords in cartesian coords
// Bottom right is positive -> top right is positive
qreal xCoord = gamepad.axis[ xAxis ];
qreal yCoord = -gamepad.axis[ yAxis ];
// Get radius from center
QVector2D position( static_cast<float>( xCoord ), static_cast<float>( yCoord ) );
qreal radius = static_cast<qreal>( position.length() );
if( !( radius > deadzone ) ) {
gamepad.axis[ xAxis ] = 0;
gamepad.axis[ yAxis ] = 0;
if( xAxis == SDL_CONTROLLER_AXIS_TRIGGERLEFT ) {
gamepad.digitalL2 = false;
}
if( xAxis == SDL_CONTROLLER_AXIS_TRIGGERRIGHT ) {
gamepad.digitalR2 = false;
}
} else {
if( xAxis == SDL_CONTROLLER_AXIS_TRIGGERLEFT ) {
gamepad.digitalL2 = true;
}
if( xAxis == SDL_CONTROLLER_AXIS_TRIGGERRIGHT ) {
gamepad.digitalR2 = true;
}
}
}
}
// Apply deadzones to all joystick axes
// Used only when detecting input for remapping
{
for( int i = 0; i < 16; i++ ) {
qreal deadzoneRadius = deadzones[ GUID ][ i ];
qreal coord = gamepad.joystickAxis[ i ];
if( !deadzoneModes[ GUID ][ i ] ) {
coord += 32768;
}
if( !( qAbs( coord ) > deadzoneRadius ) ) {
gamepad.joystickAxis[ i ] = 0;
}
}
}
// If ignoreMode is set, the user hasn't let go of the button they were remapping to
// Do not let the button go through until they let go
{
if( ignoreMode && ignoreModeGUID == GUID && ignoreModeInstanceID == gamepad.instanceID ) {
if( ignoreModeVal.first == BUTTON ) {
if( gamepad.joystickButton[ ignoreModeVal.second.first ] == SDL_PRESSED ) {
gamepad.joystickButton[ ignoreModeVal.second.first ] = SDL_RELEASED;
} else {
ignoreMode = false;
}
} else if( ignoreModeVal.first == HAT ) {
if( gamepad.joystickHat[ ignoreModeVal.second.first ] != SDL_HAT_CENTERED ) {
gamepad.joystickHat[ ignoreModeVal.second.first ] = SDL_HAT_CENTERED;
} else {
ignoreMode = false;
}
} else if( ignoreModeVal.first == AXIS ) {
if( gamepad.joystickAxis[ ignoreModeVal.second.first ] != 0 ) {
gamepad.joystickAxis[ ignoreModeVal.second.first ] = 0;
} else {
ignoreMode = false;
}
}
}
}
// If we opened an SDL2 Game controller handle from this class, keep it and inject it into all gamepads we send out
{
if( gameControllerHandles[ instanceID ] ) {
gamepad.gamecontrollerHandle = gameControllerHandles[ instanceID ];
}
}
// If we are in remap mode, check for a button press from the stored GUID, and remap the stored button to that button
// All game controller states are cleared past this point if in remap mode
{
if( remapMode && GUID == remapModeGUID ) {
// Find a button press, the first one we encounter will be the new remapping
bool foundInput = false;
Key key = remapModeKey;
Val value = Val( INVALID, VHat( -1, -1 ) );
// Prioritize buttons and hats over analog sticks
for( int joystickButton = 0; joystickButton < 256; joystickButton++ ) {
if( gamepad.joystickButton[ joystickButton ] == SDL_PRESSED ) {
qCDebug( phxInput ).nospace() << "Button b" << joystickButton
<< " from GUID " << GUID << " now activates " << keyToMappingString( key );
foundInput = true;
value.first = BUTTON;
value.second = VHat( joystickButton, -1 );
break;
}
}
for( int joystickHat = 0; joystickHat < 16; joystickHat++ ) {
if( gamepad.joystickHat[ joystickHat ] != SDL_HAT_CENTERED ) {
qCDebug( phxInput ).nospace() << "Hat h" << joystickHat << "." << gamepad.joystickHat[ joystickHat ]
<< " from GUID " << GUID << " now activates " << keyToMappingString( key );
foundInput = true;
value.first = HAT;
value.second = VHat( joystickHat, gamepad.joystickHat[ joystickHat ] );
break;
}
}
for( int joystickAxis = 0; joystickAxis < 16; joystickAxis++ ) {
if( gamepad.joystickAxis[ joystickAxis ] != 0 ) {
qCDebug( phxInput ).nospace() << "Axis a" << joystickAxis
<< " from GUID " << GUID << " now activates " << keyToMappingString( key );
foundInput = true;
value.first = AXIS;
value.second = VHat( joystickAxis, -1 );
break;
}
}
if( foundInput ) {
// Store the new remapping internally
gameControllerToJoystick[ GUID ][ remapModeKey ] = value;
// Tell SDL2 about it
QString mappingString;
QString platform( SDL_GetPlatform() );
{
QString friendlyName = QString( SDL_JoystickName( gamepad.joystickHandle ) );
mappingString.append( GUID ).append( "," ).append( friendlyName ).append( "," );
for( Key key : gameControllerToJoystick[ GUID ].keys() ) {
if( gameControllerToJoystick[ GUID ][ key ].first != INVALID ) {
mappingString.append( keyToMappingString( key ) ).append( ':' )
.append( valToMappingString( gameControllerToJoystick[ GUID ][ key ] ) ).append( ',' );
}
}
mappingString.append( "platform:" ).append( platform ).append( "," );
qDebug().nospace() << mappingString;
// Give SDL the new mapping string
SDL_GameControllerAddMapping( mappingString.toUtf8().constData() );
// If this is not a game controller, reopen as one
SDL_GameController *gamecontrollerHandle = nullptr;
if( !gameControllerHandles[ instanceID ] ) {
gamecontrollerHandle = SDL_GameControllerOpen( joystickID );
gamepad.gamecontrollerHandle = gamecontrollerHandle;
// Store internally so we can inject it into all future events from this instanceID
gameControllerHandles[ instanceID ] = gamecontrollerHandle;
qDebug() << "Opened newly remapped joystick as a game controller:" << gamecontrollerHandle;
}
}
// Store this mapping to disk
{
if( !userDataPath.isEmpty() ) {
QFile mappingFile( userDataPath + "/gamecontrollerdb.txt" );
mappingFile.open( QIODevice::ReadWrite | QIODevice::Text );
QByteArray mappingFileData = mappingFile.readAll();
if( !mappingFile.isOpen() ) {
qWarning() << "Unable to open mapping file for reading" << mappingFile.errorString();
}
mappingFile.close();
QTextStream mappingFileStreamIn( &mappingFileData );
mappingFileStreamIn.setCodec( "UTF-8" );
mappingFile.open( QIODevice::WriteOnly | QIODevice::Text );
if( !mappingFile.isOpen() ) {
qWarning() << "Unable to open mapping file for writing" << mappingFile.errorString();
}
QTextStream mappingFileStreamOut( &mappingFile );
mappingFileStreamOut.setCodec( "UTF-8" );
QString line = "";
while( !line.isNull() ) {
line = mappingFileStreamIn.readLine();
// We want to replace the line (any line) for our platform that contains our GUID
// We'll also filter out empty lines
if( line.isEmpty() || ( line.contains( GUID ) && line.contains( platform ) ) ) {
continue;
}
mappingFileStreamOut << line << endl;
}
mappingFileStreamOut << mappingString << endl;
mappingFile.close();
} else {
qWarning() << "Unable to open controller mapping file, user data path not set";
}
}
// End remap mode, start ignore mode
{
remapMode = false;
ignoreMode = true;
ignoreModeGUID = GUID;
ignoreModeVal = value;
ignoreModeInstanceID = gamepad.instanceID;
}
// Tell the model we're done
{
emit setMapping( GUID, keyToMappingString( key ), valToFriendlyString( value ) );
emit remappingEnded();
}
}
// Clear all game controller states (joystick states are untouched)
for( int i = 0; i < SDL_CONTROLLER_BUTTON_MAX; i++ ) {
gamepad.button[ i ] = 0;
}
for( int i = 0; i < SDL_CONTROLLER_AXIS_MAX; i++ ) {
gamepad.axis[ i ] = 0;
}
} else if( remapMode ) {
// Clear all gamepad states
for( int i = 0; i < SDL_CONTROLLER_BUTTON_MAX; i++ ) {
gamepad.button[ i ] = 0;
}
for( int i = 0; i < SDL_CONTROLLER_AXIS_MAX; i++ ) {
gamepad.axis[ i ] = 0;
}
}
}
// OR all joystick button, hat and analog states together by GUID for RemapperModel to indicate presses
{
for( int i = 0; i < 256; i++ ) {
pressed[ GUID ] |= gamepad.joystickButton[ i ];
}
for( int i = 0; i < 16; i++ ) {
pressed[ GUID ] |= ( gamepad.joystickHat[ i ] != SDL_HAT_CENTERED );
}
for( int i = 0; i < 16; i++ ) {
pressed[ GUID ] |= ( gamepad.joystickAxis[ i ] != 0 );
}
}
// Apply axis to d-pad, if enabled
// This will always be enabled if we're not currently playing so GlobalGamepad can use the analog stick
{
if( analogToDpad[ GUID ] || !playing ) {
// TODO: Support other axes?
int xAxis = SDL_CONTROLLER_AXIS_LEFTX;
int yAxis = SDL_CONTROLLER_AXIS_LEFTY;
// TODO: Let user configure these
qreal threshold = 16384.0;
// Size in degrees of the arc covering and centered around each cardinal direction
// If <90, there will be gaps in the diagonals
// If >180, this code will always produce diagonal inputs
qreal rangeDegrees = 180.0 - 45.0;
// Get axis coords in cartesian coords
// Bottom right is positive -> top right is positive
qreal xCoord = gamepad.axis[ xAxis ];
qreal yCoord = -gamepad.axis[ yAxis ];
// Get radius from center
QVector2D position( static_cast<float>( xCoord ), static_cast<float>( yCoord ) );
qreal radius = static_cast<qreal>( position.length() );
// Get angle in degrees
qreal angle = qRadiansToDegrees( qAtan2( yCoord, xCoord ) );
if( angle < 0.0 ) {
angle += 360.0;
}
if( radius > threshold ) {
qreal halfRange = rangeDegrees / 2.0;
if( angle > 90.0 - halfRange && angle < 90.0 + halfRange ) {
gamepad.button[ SDL_CONTROLLER_BUTTON_DPAD_UP ] = true;
}
if( angle > 270.0 - halfRange && angle < 270.0 + halfRange ) {
gamepad.button[ SDL_CONTROLLER_BUTTON_DPAD_DOWN ] = true;
}
if( angle > 180.0 - halfRange && angle < 180.0 + halfRange ) {
gamepad.button[ SDL_CONTROLLER_BUTTON_DPAD_LEFT ] = true;
}
if( angle > 360.0 - halfRange || angle < 0.0 + halfRange ) {
gamepad.button[ SDL_CONTROLLER_BUTTON_DPAD_RIGHT ] = true;
}
}
}
}
// Apply d-pad to axis, if enabled
{
if( dpadToAnalog[ GUID ] ) {
gamepad = mapDpadToAnalog( gamepad );
}
}
// Send updated data out
{
// Copy current gamepad into buffer
this->mutex.lock();
gamepadBuffer[ gamepadBufferIndex ] = gamepad;
this->mutex.unlock();
// Send buffer on its way
emit dataOut( DataType::Input, &( this->mutex ),
reinterpret_cast<void *>( &gamepadBuffer[ gamepadBufferIndex ] ), 0,
nodeCurrentTime() );
// Increment the index
gamepadBufferIndex = ( gamepadBufferIndex + 1 ) % 100;
}
break;
}
case DataType::KeyboardInput: {
// Unpack keyboard states and write to gamepad according to remap data
{
mutex->lock();
KeyboardState keyboard = *reinterpret_cast<KeyboardState *>( data );
for( int i = keyboard.head; i < keyboard.tail; i = ( i + 1 ) % 128 ) {
int key = keyboard.key[ i ];
bool pressed = keyboard.pressed[ i ];
if( keyboardKeyToSDLButton.contains( key ) ) {
keyboardGamepad.button[ keyboardKeyToSDLButton[ key ] ] = pressed ? SDL_PRESSED : SDL_RELEASED;
}
}
mutex->unlock();
}
// OR all key states together and store that value
for( int i = 0; i < SDL_CONTROLLER_BUTTON_MAX; i++ ) {
keyboardKeyPressed |= keyboardGamepad.button[ i ];
}
// Apply d-pad to axis, if enabled
if( dpadToAnalogKeyboard ) {
keyboardGamepad = mapDpadToAnalog( keyboardGamepad, true );
}
// Send gamepad on its way
{
// Copy current gamepad into buffer
this->mutex.lock();
gamepadBuffer[ gamepadBufferIndex ] = keyboardGamepad;
this->mutex.unlock();
// Send buffer on its way
emit dataOut( DataType::Input, &( this->mutex ),
reinterpret_cast< void * >( &gamepadBuffer[ gamepadBufferIndex ] ), 0,
nodeCurrentTime() );
// Increment the index
gamepadBufferIndex = ( gamepadBufferIndex + 1 ) % 100;
}
break;
}
default: {
emit dataOut( type, mutex, data, bytes, timeStamp );
break;
}
}
}
