void LineMove::adjustBounds(QMouseEvent *m)
{
FPoint mousePointDoc = m_canvas->globalToCanvas(m->globalPos());
bool constrainRatio = ((m->modifiers() & Qt::ControlModifier) != Qt::NoModifier);
double newX = mousePointDoc.x();
double newY = mousePointDoc.y();
if (m_doc->useRaster)
{
newX = qRound(newX / m_doc->guidesSettings.minorGridSpacing) * m_doc->guidesSettings.minorGridSpacing;
newY = qRound(newY / m_doc->guidesSettings.minorGridSpacing) * m_doc->guidesSettings.minorGridSpacing;
}
//<<#8099
FPoint np2 = m_doc->ApplyGridF(FPoint(newX, newY));
double nx = np2.x();
double ny = np2.y();
m_doc->ApplyGuides(&nx, &ny);
newX = qRound(nx);
newY = qRound(ny);
//>>#8099
m_bounds.setBottomRight(QPointF(newX, newY));
//Constrain rotation angle, when the mouse is being dragged around for a new line
if (constrainRatio)
{
double newRot = rotation();
newRot = constrainAngle(newRot, m_doc->opToolPrefs.constrain);
setRotation(newRot);
}
// qDebug() << "LineMove::adjustBounds" << m_bounds << rotation() << length() << m_bounds.bottomRight();
m_view->updateCanvas(m_bounds.normalized().adjusted(-10, -10, 20, 20));
}
// ============================================================================================
// back and forth with 1/4 wave sin wave sweep shaped rhythms
// ============================================================================================
bool toFro(){
bool updated = false;
int degreesPerBeat;
static int direction = 1;
static float startAngle = 0;
if(currentBeat == 0)
degreesPerBeat = 20;
else
degreesPerBeat = 12;
if(newPresence){
shufflePattern();
newPresence = false;
}
// quantize to something...
if (currentBeat != lastBeat){
lastBeat = currentBeat;
// counterFiftyHz = 0;
startAngle = curAngle;
if (abs(curAngle + direction * degreesPerBeat * beatPattern[currentBeat]) > MAX_ANGLE){
direction *= -1;
}
}
//fprintf_P(&usart_stream, PSTR("beat: %i\r\n"), currentBeat);
if(presenceDetected && !ignorePresence){
if(bottom){
curAngle = constrainAngle( startAngle + beatPattern[currentBeat] * direction * degreesPerBeat * sinSweep(0.2));
updated = true;
}
else{
// curAngle = neighborAngles;
curAngle = neighborData[BELOW].angleValue;
updated = true;
}
}
return updated;
}
// Is not called periodically from a timer slot
void ActorHighLevel::startPIDController()
{
while(enabled)
{
// Roboterposition abspeichern
robotPosition = MapData::getRobotPosition(ObstacleType::ME);
// we dont have a position right now -> try again in 50ms
if(robotPosition.certainty() == 0.0) {
QThread::msleep(50);
// Process Qt Event Queue and wait a little
QCoreApplication::processEvents();
QCoreApplication::sendPostedEvents();
continue;
}
// Zeit seit letzter Berechnung bestimmen, um korrekt zu integrieren / differenzieren
double elapsedMs = elapsedTime->restart();
double timeSinceStart = (QDateTime::currentMSecsSinceEpoch() / 1000.0) - timeOfStart;
switch (this->getState()) {
case StatePathProcessing::GATHER_PUCK:
{
double deltaX = releasePuckOrigin.x()- robotPosition.x();
double deltaY = releasePuckOrigin.y()- robotPosition.y();
double dist = sqrt(deltaX*deltaX+deltaY*deltaY); // Bereits gefahrene Distanz
double deltaL = config::actorGatherPuckDistance - dist; // Distanz zur Sollposition
// Sind wir weit genug gefahren?
if(dist >= config::actorGatherPuckDistance) {
if(config::enableDebugActorHighLevel)
qDebug() << "Puck gathered, weil dist == " << dist << " >= config::actorGatherPuckDistance == " << config::actorGatherPuckDistance;
resetPIDtempVars();
this->setState(StatePathProcessing::RUNNING);
Q_EMIT signalSendRobotControlParams(0, 0);
Q_EMIT signalPuckDone(); // Signal an KI, dass Gathering fertig ist
break;
}
// PID Regler Abstand
double dDeltaL = (elapsedMs) ? (deltaL-lastDeltaL)/elapsedMs : 0;
lastDeltaL = deltaL;
iDeltaL += deltaL * elapsedMs;
iDeltaL = qMin(qMax(iDeltaL, -config::actorMaxI), config::actorMaxI);
// Sollgeschwindigkeit
double robotV = +1.0 * (PID_V_P * deltaL + PID_V_I * iDeltaL + PID_V_D * dDeltaL);
// Daten zu history hinzufuegen fuer spaeteren plot
if(ActorHighLevel::streamPIDEnabled) {
mutexPidHist->lock();
pidHistTime.append(timeSinceStart);
pidHistWinkelIst.append(constrainAngle(robotPosition.rot()));
pidHistWinkelSoll.append(0.0);
pidHistDistIst.append(dist);
pidHistDistSoll.append(deltaL);
mutexPidHist->unlock();
}
// Emit the calculated control values to the low level actor module
Q_EMIT signalSendRobotControlParams(robotV, 0.0);
break;
}
case StatePathProcessing::PUSH_AND_RELEASE_PUCK:
{
double deltaX = releasePuckOrigin.x() - robotPosition.x();
double deltaY = releasePuckOrigin.y() - robotPosition.y();
double dist = sqrt(deltaX*deltaX+deltaY*deltaY); // Bereits gefahrene Distanz
// 25cm (NEIN 20cm) minus das bereits gefahrene
///\laurenz hier sollte doch 20cm unten und oben stehen?
double deltaL = /*config::actorReleasePuckDistance*/
config::actorPushPuckDistance
- dist; // Distanz zur Sollposition
// hier wird gefragt ob wir mehr als 20cm gefahren sind
// Sind wir weit genug gefahren?
if(dist >= config::actorPushPuckDistance) {
if(config::enableDebugActorHighLevel)
qDebug() << "Puck pushed, weil dist =" << dist;
resetPIDtempVars();
// nehme wieder die tatsächlich aktuelle position
releasePuckOrigin = MapData::getRobotPosition(ObstacleType::ME);
this->setState(StatePathProcessing::RELEASE_PUCK_FROM_PUSH);
if(config::enableDebugActorHighLevel)
qDebug() << "Starting Puck release";
Q_EMIT signalSendRobotControlParams(0, 0);
break; // springe raus
}
// PID Regler Abstand
double dDeltaL = (elapsedMs) ? (deltaL-lastDeltaL)/elapsedMs : 0;
lastDeltaL = deltaL;
iDeltaL += deltaL * elapsedMs;
iDeltaL = qMin(qMax(iDeltaL, -config::actorMaxI), config::actorMaxI);
double robotV = +1.0 * (PID_V_P * deltaL + PID_V_I * iDeltaL + PID_V_D * dDeltaL); // Sollgeschwindigkeit
// Daten zu history hinzufuegen fuer spaeteren plot
if(ActorHighLevel::streamPIDEnabled) {
mutexPidHist->lock();
pidHistTime.append(timeSinceStart);
pidHistWinkelIst.append(constrainAngle(robotPosition.rot()));
pidHistWinkelSoll.append(0.0);
pidHistDistIst.append(dist);
pidHistDistSoll.append(deltaL);
mutexPidHist->unlock();
}
// Emit the calculated control values to the low level actor module
Q_EMIT signalSendRobotControlParams(robotV, 0.0);
break;
}
case StatePathProcessing::RELEASE_PUCK_FROM_PUSH:
{
double deltaX = releasePuckOrigin.x() - robotPosition.x();
double deltaY = releasePuckOrigin.y() - robotPosition.y();
double dist = sqrt(deltaX*deltaX+deltaY*deltaY); // Bereits gefahrene Distanz
// (additional distance is 20 or 0) + 25cm + 2,5 cm
// Carlo: habe die unterscheidung eingebaut um den puck correct ins tor zu legen
// und den beim DUMP trotzdem weit genug zurück zu fahren
double distanceToGoBack = additionalReleasePuckDistance +
config::actorReleasePuckDistance +
config::actorPushAndReleaseAdditionalReverseDist;
// 25cm + 20cm + 2,5 cm
double deltaL = distanceToGoBack - dist; // Distanz zur Sollposition
// Sind wir weit genug gefahren?
if(dist >= distanceToGoBack)
{
if(config::enableDebugActorHighLevel)
qDebug() << "Puck released, weil dist =" << dist;
resetPIDtempVars();
this->setState(StatePathProcessing::RUNNING);
Q_EMIT signalSendRobotControlParams(0, 0);
Q_EMIT signalPuckDone(); // Signal an KI, dass Release fertig ist
break;
}
// PID Regler Abstand
double dDeltaL = (elapsedMs) ? (deltaL-lastDeltaL)/elapsedMs : 0;
lastDeltaL = deltaL;
iDeltaL += deltaL * elapsedMs;
iDeltaL = qMin(qMax(iDeltaL, -config::actorMaxI), config::actorMaxI);
// Sollgeschwindigkeit
double robotV = -1.0 * (PID_V_P * deltaL + PID_V_I * iDeltaL + PID_V_D * dDeltaL);
// Daten zu history hinzufuegen fuer spaeteren plot
if(ActorHighLevel::streamPIDEnabled) {
mutexPidHist->lock();
pidHistTime.append(timeSinceStart);
pidHistWinkelIst.append(constrainAngle(robotPosition.rot()));
pidHistWinkelSoll.append(0.0);
pidHistDistIst.append(dist);
pidHistDistSoll.append(deltaL);
mutexPidHist->unlock();
}
// Emit the calculated control values to the low level actor module
Q_EMIT signalSendRobotControlParams(robotV, 0.0);
break;
}
case StatePathProcessing::RELEASE_PUCK:
{
double deltaX = releasePuckOrigin.x() - robotPosition.x();
double deltaY = releasePuckOrigin.y() - robotPosition.y();
double dist = sqrt(deltaX*deltaX+deltaY*deltaY); // Bereits gefahrene Distanz
double deltaL = config::actorReleasePuckDistance - dist; // Distanz zur Sollposition
// Sind wir weit genug gefahren?
if(dist >= config::actorReleasePuckDistance) {
if(config::enableDebugActorHighLevel)
qDebug() << "Puck released, weil dist =" << dist;
resetPIDtempVars();
this->setState(StatePathProcessing::RUNNING);
Q_EMIT signalSendRobotControlParams(0, 0);
Q_EMIT signalPuckDone(); // Signal an KI, dass Release fertig ist
break;
}
// PID Regler Abstand
double dDeltaL = (elapsedMs) ? (deltaL-lastDeltaL)/elapsedMs : 0;
lastDeltaL = deltaL;
iDeltaL += deltaL * elapsedMs;
iDeltaL = qMin(qMax(iDeltaL, -config::actorMaxI), config::actorMaxI);
// Sollgeschwindigkeit
double robotV = -1.0 * (PID_V_P * deltaL + PID_V_I * iDeltaL + PID_V_D * dDeltaL);
// Daten zu history hinzufuegen fuer spaeteren plot
if(ActorHighLevel::streamPIDEnabled) {
mutexPidHist->lock();
pidHistTime.append(timeSinceStart);
pidHistWinkelIst.append(constrainAngle(robotPosition.rot()));
pidHistWinkelSoll.append(0.0);
pidHistDistIst.append(dist);
pidHistDistSoll.append(deltaL);
mutexPidHist->unlock();
}
// Emit the calculated control values to the low level actor module
Q_EMIT signalSendRobotControlParams(robotV, 0.0);
break;
}
case StatePathProcessing::RUNNING:
{
// Wenn es keine Wegpunkte gibt
if(internalWP.isEmpty() || MapData::getObstacle(ObstacleType::TARGET).isEmpty()) {
// Leave the state
this->setState(StatePathProcessing::STOP);
break;
}
// Turn off primitive collision avoidance
MapData::setDisableEmergency(true);
// Wenn der Roboter auf dem letzten Wegpunkt steht
Obstacle target = MapData::getFirstTarget();
bool targetHasOrientation = !std::isnan(target.getOrientation()) && !std::isinf(target.getOrientation());
double targetDist = robotPosition.getDistanceTo(target.getPosition());
double targetDistDiff = targetDist - targetDistLast;
targetDistLast = targetDist;
if(targetDistDiff == 0) targetDistDiff = targetDistDiffLast;
targetDistDiffLast = targetDistDiff;
bool targetSeemsReached = targetDistDiff > config::actorWaypointReachedDiffChange && targetDist <= config::actorWaypointReachedDistance;
if( positionWasReached || targetSeemsReached ) {
// Roboter ist/war an Position
if(positionWasReached == false){
qDebug() << "Ziel erreicht mit Abstand" << targetDist << " - Stelle jetzt Winkel perfekt ein";
}
positionWasReached = true; // Roboter hat das Ziel erreicht. Wenn das Ziel davonwackelt, ist es immernoch erreicht.
// Abweichung vom einzustellenden Winkel (kann undefinierte Werte annehmen, falls das Target keinen Winkel besitzt)
double angleDeviation = fabs(target.getOrientation() - robotPosition.rot());
if(std::isnan(angleDeviation)) angleDeviation = 0;
while(angleDeviation > M_PI) angleDeviation -= M_PI; // Die Winkelabweichung darf nie über 180° sein
if(!targetHasOrientation || (targetHasOrientation && angleDeviation <= config::actorWaypointMaxAngleDeviation ) ) {
// Winkel ist OK
if(config::enableDebugActorHighLevel)
qDebug() << "Ziel erreicht mit Winkelabweichung" << (angleDeviation/M_PI*180.0) << "°";
positionWasReached = false; // Zurücksetzen, weil als nächstes ein neues Ziel kommt
MapData::deleteFirstTarget(); // Delete this target
this->setState(StatePathProcessing::STOP); // Leave the state
Q_EMIT signalSendRobotControlParams(0.0, 0.0); // Zur Sicherheit Stop senden
break;
} else {
if(config::enableDebugActorHighLevel)
qDebug() << "Winkel noch nicht OK - Abweichung " << (angleDeviation/M_PI*180.0) << "°";
}
}
// Nähesten Punkt auf dem Spline finden.
double minP=0, minL=9999;
for(double p = 0; p<numWP; p+=0.001){ // Sicher wär ne Newton Iteration oder so besser, aber so gehts auch.
double deltaX = splineX(p) - robotPosition.x();
double deltaY = splineY(p) - robotPosition.y();
double deltaL = sqrt(deltaX*deltaX+deltaY*deltaY); // Distanz des Roboters zu diesem Punkt auf dem Spline
if(deltaL < minL){
minP = p;
minL = deltaL;
}
}
// Die Sollposition ist vom nähesten Spline Punkt 0.5m weiter auf dem Spline entfernt, jedoch nie weiter weg als das Ziel selbst
double progressOnSpline = minP + config::actorDistanceOfTargetOnSpline;
double sollX, sollY;// Sollposition
if(progressOnSpline <= numWP) {
sollX = splineX(progressOnSpline);
sollY = splineY(progressOnSpline);
} else {
sollX = target.getCoords().first;
sollY = target.getCoords().second;
}
double deltaX = sollX - robotPosition.x(); // Abweichung von der Sollposition
double deltaY = sollY - robotPosition.y(); // Abweichung von der Sollposition
double deltaL = sqrt(deltaX*deltaX+deltaY*deltaY); // Distanz zur Sollposition
double sollA;
if(targetHasOrientation && deltaL < config::actorWaypointReachedDistance) { // Roboter ist im Prinzip schon da und muss sich noch vernünftig ausrichten
sollA = /*constrainAngle*/(target.getOrientation()); // Vorgegebener Anfahrtswinkel
} else { // Roboter muss beim Fahren in Fahrtrichtung gucken
sollA = /*constrainAngle*/(atan2(deltaY, deltaX)); // Winkel zur Sollposition
}
double deltaA = constrainAngle(sollA) - constrainAngle(robotPosition.rot()); // Abweichung vom Winkel zur Sollposition
deltaA = constrainAngle(deltaA);
// PID Regler Rotation(sgeschwindigkeit)
// deltaA = SensorLowLevel::angleWeightedAverage(deltaA, config::actorLowPass, lastDeltaA, 1-config::actorLowPass); // Tiefpassfilter
double dDeltaA = (elapsedMs)? (deltaA-lastDeltaA)/elapsedMs : 0; // Differenzialanteil
lastDeltaA = deltaA;
iDeltaA += deltaA * elapsedMs; // Integralanteil
iDeltaA = qMin(qMax(iDeltaA, -config::actorMaxI), config::actorMaxI); // Begrenzen auf -10...+10
double robotR = PID_A_P * deltaA + PID_A_I * iDeltaA + PID_A_D * dDeltaA; // Solldrehgeschwindigkeit
if(config::enableDebugActorHighLevel)
qDebug() << "deltaA="<<deltaA << ", iDeltaA=" << iDeltaA << ", dDeltaA=" << dDeltaA << "robotR=" << robotR;
// PID Regler Abstand
double dDeltaL = (elapsedMs) ? (deltaL-lastDeltaL)/elapsedMs : 0;
lastDeltaL = deltaL;
iDeltaL += deltaL * elapsedMs;
iDeltaL = qMin(qMax(iDeltaL, -config::actorMaxI), config::actorMaxI);
double robotV = PID_V_P * deltaL + PID_V_I * iDeltaL + PID_V_D * dDeltaL; // Sollgeschwindigkeit
// Vorwärtsgeschwindigkeit reduzieren, wenn Winkelabweichung zu groß
double angleLimiter = 1;
if(fabs(deltaA) > config::actorMinAngleLimiter && fabs(deltaA) < config::actorMaxAngleLimiter) {
angleLimiter = 1 - (fabs(deltaA) - config::actorMinAngleLimiter) / (config::actorMaxAngleLimiter - config::actorMinAngleLimiter); // 0...1
} else if ( fabs(deltaA) > config::actorMaxAngleLimiter ) {
angleLimiter = 0;
}
robotV = robotV * angleLimiter;
// Wenn es nur noch um die Drehung geht, gar nicht vorwärts fahren
if(positionWasReached){
robotV = 0.0;
}
// if enemy position is next to target -> wait
Position enemyPosition = MapData::getRobotPosition(ObstacleType::OPPONENT);
if(enemyPosition.isConsimilarTo(target.getPosition(), 2.5 * config::SENSOR_RADIUS_ROBOT))
{
robotV = 0;
robotR = 0;
resetPIDtempVars();
}
// Emit the calculated control values to the low level actor module
Q_EMIT signalSendRobotControlParams(robotV,robotR);
// Daten zu history hinzufuegen fuer spaeteren plot
if(ActorHighLevel::streamPIDEnabled) {
mutexPidHist->lock();
pidHistTime.append(timeSinceStart);
pidHistWinkelIst.append(constrainAngle(robotPosition.rot()));
pidHistWinkelSoll.append(sollA);
pidHistDistIst.append(robotV);
pidHistDistSoll.append(deltaL);
mutexPidHist->unlock();
}
break;
}
case StatePathProcessing::STOP:
{
// Clear remaining waypoints
internalWP.clear();
// Stop robot motion
Q_EMIT signalSendRobotControlParams(0.0, 0.0);
// (Re-)Enable primitive collision avoidance in manual control mode
MapData::setDisableEmergency(false);
// avoid looping in here again all the time
enabled = false;
break;
}
} // switch
// Process Qt Event Queue and wait a little
QCoreApplication::processEvents();
QCoreApplication::sendPostedEvents();
// we dont want 100% cpu time for the PID
QThread::msleep(10);
}
}
void VisibilityLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){
boost::recursive_mutex::scoped_lock lock(lock_);
if (!enabled_)
return;
if( people_list.people.size() == 0 )
return;
hannrs_msgs::Person person;
costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
double res = costmap->getResolution();
for(unsigned int i = 0; i < transformed_people_.size(); i++){
person = transformed_people_[i];
double pos_x = person.pose.position.x, pos_y = person.pose.position.y;
double dx = safety_dist_, dy = safety_dist_;
unsigned int start_x, start_y, end_x, end_y;
if(costmap->worldToMap(pos_x - dx, pos_y - dy, start_x, start_y)){
}else{
start_x = 0;
start_y = 0;
}
if(costmap->worldToMap(pos_x + dx, pos_y + dy, end_x, end_y)){
}else{
end_x = costmap->getSizeInCellsX();
end_y = costmap->getSizeInCellsY();
}
if(start_x < min_i){
start_x = min_i;
}
if(start_y < min_j){
start_y = min_j;
}
if(end_x > max_i){
end_x = max_i;
}
if(end_y > max_j){
end_y = max_j;
}
double x,y;
double f;
for(int k = start_x; k < end_x; k++){
for(int l = start_y; l < end_y; l++){
unsigned char old_cost = costmap->getCost(k, l);
if(old_cost == costmap_2d::NO_INFORMATION)
continue;
costmap->mapToWorld(k,l,x,y);
double theta = tf::getYaw(person.pose.orientation) + M_PI;
theta = constrainAngle(theta*180/M_PI)*M_PI/180;
f = assymetric_gaussian(amplitude_, x, y, pos_x, pos_y, theta, sigma_h_, factor_s_*sigma_h_, factor_r_*sigma_h_);
if(f<cutoff_)
continue;
unsigned char cvalue = (unsigned char) f;
costmap->setCost(k, l, std::max(cvalue, old_cost));
}
}
}
}
void CreateMode::mouseMoveEvent(QMouseEvent *m)
{
const FPoint mousePointDoc = m_canvas->globalToCanvas(m->globalPos());
modifiers = m->modifiers();
double newX, newY;
PageItem *currItem;
QPoint np, np2, mop;
QPainter p;
QRect tx;
m->accept();
// qDebug() << "legacy mode move:" << m->x() << m->y() << m_canvas->globalToCanvas(m->globalPos()).x() << m_canvas->globalToCanvas(m->globalPos()).y();
// emit MousePos(m->x()/m_canvas->scale(),// + m_doc->minCanvasCoordinate.x(),
// m->y()/m_canvas->scale()); // + m_doc->minCanvasCoordinate.y());
if (commonMouseMove(m))
return;
if (GetItem(&currItem))
{
newX = mousePointDoc.x(); //m_view->translateToDoc(m->x(), m->y()).x());
newY = mousePointDoc.y(); //m_view->translateToDoc(m->x(), m->y()).y());
if (m_doc->DragP)
return;
}
else
{
if ((m_MouseButtonPressed) && (m->buttons() & Qt::LeftButton))
{
newX = mousePointDoc.x();
newY = mousePointDoc.y();
if (createObjectMode == modeDrawLine)
{
if (m_doc->SnapGrid)
{
newX = qRound(newX / m_doc->guidesPrefs().minorGridSpacing) * m_doc->guidesPrefs().minorGridSpacing;
newY = qRound(newY / m_doc->guidesPrefs().minorGridSpacing) * m_doc->guidesPrefs().minorGridSpacing;
}
if (m->modifiers() & Qt::ControlModifier)
{
QRectF bounds(QPointF(createObjectPos.x(), createObjectPos.y()), QPointF(newX, newY));
double newRot = xy2Deg(bounds.width(), bounds.height());
if (newRot < 0.0)
newRot += 360;
newRot = constrainAngle(newRot, m_doc->opToolPrefs().constrain);
double len = qMax(0.01, distance(bounds.width(), bounds.height()));
bounds.setSize(len * QSizeF(cosd(newRot), sind(newRot)));
newX = bounds.right();
newY = bounds.bottom();
}
}
//CB: #8099: Readd snapping for drag creation of lines by commenting this else..
//else
//{
FPoint np2 = m_doc->ApplyGridF(FPoint(newX, newY));
double nx = np2.x();
double ny = np2.y();
m_doc->ApplyGuides(&nx, &ny);
m_doc->ApplyGuides(&nx, &ny,true);
if(nx!=np2.x())
xSnap = nx;
if(ny!=np2.y())
ySnap = ny;
// #8959 : suppress qRound here as this prevent drawing line with angle constrain
// precisely and does not allow to stick precisely to grid or guides
newX = /*qRound(*/nx/*)*/;
newY = /*qRound(*/ny/*)*/;
//}
canvasCurrCoord.setXY(newX, newY);
m_view->HaveSelRect = true;
double wSize = canvasCurrCoord.x() - createObjectPos.x();
double hSize = canvasCurrCoord.y() - createObjectPos.y();
QRectF createObjectRect(createObjectPos.x(), createObjectPos.y(), wSize, hSize);
createObjectRect = createObjectRect.normalized();
if (createObjectMode != modeDrawLine)
{
if (modifiers == Qt::ControlModifier)
hSize = wSize;
m_canvas->displaySizeHUD(m->globalPos(), wSize, hSize, false);
}
else
{
double angle = -xy2Deg(wSize, hSize);
if (angle < 0.0)
angle = angle + 360;
double trueLength = sqrt(pow(createObjectRect.width(), 2) + pow(createObjectRect.height(), 2));
m_canvas->displaySizeHUD(m->globalPos(), trueLength, angle, true);
}
// Necessary for drawControls to be called
m_canvas->repaint();
}
else
m_canvas->displayCorrectedXYHUD(m->globalPos(), mousePointDoc.x(), mousePointDoc.y());
}
}
// convert to [-360,360]
inline double angleConv(double angle) {
return fmod(constrainAngle(angle), 2 * M_PI);
}
void CanvasMode_Rotate::getNewItemPosition(PageItem* item, FPoint& pos, double& rotation)
{
double newAngle = xy2Deg(m_canvasCurrCoord.x() - m_rotCenter.x(), m_canvasCurrCoord.y() - m_rotCenter.y());
if (m_angleConstrained)
{
newAngle = constrainAngle(newAngle, m_doc->toolSettings.constrain);
/*double oldAngle = constrainAngle(m_startAngle, m_doc->toolSettings.constrain);
newAngle = m_doc->m_Selection->isMultipleSelection() ? (newAngle - oldAngle) : newAngle;*/
m_view->oldW = constrainAngle(m_view->oldW, m_doc->toolSettings.constrain);
newAngle = m_doc->m_Selection->isMultipleSelection() ? (newAngle - m_view->oldW) : newAngle;
}
else if (m_doc->m_Selection->isMultipleSelection())
newAngle = (newAngle - m_startAngle);
else
newAngle = item->rotation() - (m_startAngle - newAngle);
if (m_doc->m_Selection->isMultipleSelection())
{
QMatrix ma;
ma.translate(m_rotCenter.x(), m_rotCenter.y());
ma.scale(1, 1);
ma.rotate(newAngle);
FPoint n(item->xPos() - m_rotCenter.x(), item->yPos() - m_rotCenter.y());
pos.setXY(ma.m11() * n.x() + ma.m21() * n.y() + ma.dx(), ma.m22() * n.y() + ma.m12() * n.x() + ma.dy());
rotation = item->rotation() + newAngle;
}
else if (m_rotMode != 0)
{
FPoint n(0,0);
QMatrix ma;
ma.translate(item->xPos(), item->yPos());
ma.scale(1, 1);
ma.rotate(item->rotation());
double ro = newAngle - item->rotation();
switch (m_rotMode)
{
case 2:
ma.translate(item->width()/2.0, item->height()/2.0);
n = FPoint(-item->width()/2.0, -item->height()/2.0);
break;
case 4:
ma.translate(item->width(), item->height());
n = FPoint(-item->width(), -item->height());
break;
case 3:
ma.translate(0, item->height());
n = FPoint(0, -item->height());
break;
case 1:
ma.translate(item->width(), 0);
n = FPoint(-item->width(), 0);
break;
}
ma.rotate(ro);
pos.setXY(ma.m11() * n.x() + ma.m21() * n.y() + ma.dx(), ma.m22() * n.y() + ma.m12() * n.x() + ma.dy());
rotation = newAngle;
}
else
{
pos.setXY(item->xPos(), item->yPos());
rotation = newAngle;
}
}
void CanvasMode_Rotate::mouseReleaseEvent(QMouseEvent *m)
{
#ifdef GESTURE_FRAME_PREVIEW
clearPixmapCache();
#endif // GESTURE_FRAME_PREVIEW
const FPoint mousePointDoc = m_canvas->globalToCanvas(m->globalPos());
PageItem *currItem;
m_canvas->m_viewMode.m_MouseButtonPressed = false;
m_canvas->resetRenderMode();
m->accept();
// m_view->stopDragTimer();
if ((GetItem(&currItem)) && (m->button() == Qt::RightButton))
{
createContextMenu(currItem, mousePointDoc.x(), mousePointDoc.y());
return;
}
m_inItemRotation = false;
if (m_view->moveTimerElapsed() && (GetItem(&currItem)))
{
// m_view->stopDragTimer();
//Always start group transaction as a rotate action is generally a combination of
//a change of rotation + a change of position
if (!m_view->groupTransactionStarted() /*&& m_doc->m_Selection->isMultipleSelection()*/)
{
m_view->startGroupTransaction(Um::Rotate, "", Um::IRotate);
}
double newW = xy2Deg(mousePointDoc.x()-m_view->RCenter.x(), mousePointDoc.y()-m_view->RCenter.y()); //xy2Deg(m->x()/sc - m_view->RCenter.x(), m->y()/sc - m_view->RCenter.y());
if (m->modifiers() & Qt::ControlModifier)
{
newW=constrainAngle(newW, m_doc->toolSettings.constrain);
m_view->oldW=constrainAngle(m_view->oldW, m_doc->toolSettings.constrain);
//RotateGroup uses MoveBy so its pretty hard to constrain the result
if (m_doc->m_Selection->isMultipleSelection())
m_doc->rotateGroup(newW-m_view->oldW, m_view->RCenter);
else
m_doc->RotateItem(newW, currItem->ItemNr);
}
else
{
if (m_doc->m_Selection->isMultipleSelection())
m_doc->rotateGroup(newW - m_view->oldW, m_view->RCenter);
else
m_doc->RotateItem(currItem->rotation() - (m_view->oldW - newW), currItem->ItemNr);
}
m_view->oldW = newW;
m_canvas->setRenderModeUseBuffer(false);
if (!m_doc->m_Selection->isMultipleSelection())
{
m_doc->setRedrawBounding(currItem);
currItem->OwnPage = m_doc->OnPage(currItem);
if (currItem->asLine())
m_view->updateContents();
}
}
m_doc->RotMode = m_oldRotMode;
m_view->RCenter = m_oldRotCenter;
if (!PrefsManager::instance()->appPrefs.stickyTools)
m_view->requestMode(modeNormal);
else
{
int appMode = m_doc->appMode;
m_view->requestMode(appMode);
}
if (GetItem(&currItem))
{
if (m_doc->m_Selection->count() > 1)
{
m_doc->m_Selection->setGroupRect();
double x, y, w, h;
m_doc->m_Selection->getGroupRect(&x, &y, &w, &h);
m_view->updateContents(QRect(static_cast<int>(x-5), static_cast<int>(y-5), static_cast<int>(w+10), static_cast<int>(h+10)));
}
// Handled normally automatically by Selection in sendSignals()
/*else
currItem->emitAllToGUI();*/
}
m_canvas->setRenderModeUseBuffer(false);
m_doc->leaveDrag = false;
m_view->MidButt = false;
if (m_view->groupTransactionStarted())
{
for (int i = 0; i < m_doc->m_Selection->count(); ++i)
m_doc->m_Selection->itemAt(i)->checkChanges(true);
m_view->endGroupTransaction();
}
for (int i = 0; i < m_doc->m_Selection->count(); ++i)
m_doc->m_Selection->itemAt(i)->checkChanges(true);
//Make sure the Zoom spinbox and page selector dont have focus if we click on the canvas
m_view->zoomSpinBox->clearFocus();
m_view->pageSelector->clearFocus();
}
double Math::angle(cv::Point p1, cv::Point p2) {
return constrainAngle(atan2(p1.y - p2.y, p1.x - p2.x)*(180/M_PI));
}
