void CardItem::_initialize() { setFlag(QGraphicsItem::ItemIsMovable); m_opacityAtHome = 1.0; m_currentAnimation = NULL; _m_width = G_COMMON_LAYOUT.m_cardNormalWidth; _m_height = G_COMMON_LAYOUT.m_cardNormalHeight; _m_showFootnote = true; m_isSelected = false; _m_isUnknownGeneral = false; auto_back = true; frozen = false; resetTransform(); setTransform(QTransform::fromTranslate(-_m_width / 2, -_m_height / 2), true); outerGlowEffectEnabled = false; outerGlowEffect = NULL; outerGlowColor = Qt::white; _transferButton = NULL; _transferable = false; _skinId = 0; }
void CoverPoint::unpackUpdate(NetConnection *conn, BitStream *stream) { Parent::unpackUpdate(conn, stream); mSize = (Size)stream->readInt(4); setOccupied(stream->readFlag()); mPeekLeft = stream->readFlag(); mPeekRight = stream->readFlag(); mPeekOver = stream->readFlag(); if(stream->readFlag()) // TransformMask { mathRead(*stream, &mObjToWorld); mathRead(*stream, &mObjScale); setTransform(mObjToWorld); } }
void DrawableImage::recalculateCoordinates (Expression::Scope* scope) { if (image.isValid()) { Point<float> resolved[3]; bounds.resolveThreePoints (resolved, scope); const Point<float> tr (resolved[0] + (resolved[1] - resolved[0]) / (float) image.getWidth()); const Point<float> bl (resolved[0] + (resolved[2] - resolved[0]) / (float) image.getHeight()); AffineTransform t (AffineTransform::fromTargetPoints (resolved[0].x, resolved[0].y, tr.x, tr.y, bl.x, bl.y)); if (t.isSingularity()) t = AffineTransform(); setTransform (t); } }
ProxyComponent (Component& c) { setWantsKeyboardFocus (false); setBounds (c.getBounds()); setTransform (c.getTransform()); setAlpha (c.getAlpha()); setInterceptsMouseClicks (false, false); if (Component* const parent = c.getParentComponent()) parent->addAndMakeVisible (this); else if (c.isOnDesktop() && c.getPeer() != nullptr) addToDesktop (c.getPeer()->getStyleFlags() | ComponentPeer::windowIgnoresKeyPresses); else jassertfalse; // seem to be trying to animate a component that's not visible.. image = c.createComponentSnapshot (c.getLocalBounds(), false, getDesktopScaleFactor()); setVisible (true); toBehind (&c); }
//! [0] DiagramDrawItem::DiagramDrawItem(const DiagramDrawItem& diagram) : DiagramItem(diagram.myContextMenu,diagram.parentItem(),0) { myDiagramType=diagram.myDiagramType; // copy from general GraphcsItem setBrush(diagram.brush()); setPen(diagram.pen()); setTransform(diagram.transform()); myPos2=diagram.myPos2; myPolygon=createPath(); setPolygon(myPolygon); setFlag(QGraphicsItem::ItemIsMovable, true); setFlag(QGraphicsItem::ItemIsSelectable, true); setAcceptHoverEvents(true); myHoverPoint=-1; mySelPoint=-1; myHandlerWidth=2.0; }
void PhysicsForce::processTick( const Move * ) { if ( isMounted() ) { MatrixF test( true ); test.setPosition( Point3F( 0, 4, 0 ) ); AssertFatal( test != mMount.xfm, "Error!" ); MatrixF mat( true ); mMount.object->getMountTransform( mMount.node, mMount.xfm, &mat ); setTransform( mat ); } // Nothing to do without a body or if physics hasn't ticked. if ( !mBody || !mPhysicsTick ) return; mPhysicsTick = false; // If we lost the body then release it. if ( !mBody->isDynamic() || !mBody->isSimulationEnabled() ) { detach(); return; } // Get our distance to the body. Point3F cMass = mBody->getCMassPosition(); Point3F vector = getPosition() - cMass; // Apply the force! F32 mass = mBody->getMass(); Point3F impulse = ( mass * vector ) / TickSec; // Counter balance the linear impulse. Point3F linVel = mBody->getLinVelocity(); Point3F currentForce = linVel * mass; // Apply it. mBody->applyImpulse( cMass, impulse - currentForce ); }
void QgsGrassPlugin::mapsetChanged() { QgsDebugMsg( "entered" ); if ( !QgsGrass::activeMode() ) { mRegionAction->setEnabled( false ); mRegionBand->reset(); mCloseMapsetAction->setEnabled( false ); } else { mRegionAction->setEnabled( true ); mCloseMapsetAction->setEnabled( true ); QSettings settings; bool on = settings.value( "/GRASS/region/on", true ).toBool(); mRegionAction->setChecked( on ); switchRegion( on ); QString gisdbase = QgsGrass::getDefaultGisdbase(); QString location = QgsGrass::getDefaultLocation(); try { mCrs = QgsGrass::crsDirect( gisdbase, location ); } catch ( QgsGrass::Exception &e ) { Q_UNUSED( e ); QgsDebugMsg( "Cannot read GRASS CRS : " + QString( e.what() ) ); mCrs = QgsCoordinateReferenceSystem(); } QgsDebugMsg( "mCrs: " + mCrs.toWkt() ); setTransform(); redrawRegion(); } if ( mTools ) { mTools->mapsetChanged(); } }
void RenderMeshExample::unpackUpdate(NetConnection *conn, BitStream *stream) { // Let the Parent read any info it sent Parent::unpackUpdate(conn, stream); if ( stream->readFlag() ) // TransformMask { mathRead(*stream, &mObjToWorld); mathRead(*stream, &mObjScale); setTransform( mObjToWorld ); } if ( stream->readFlag() ) // UpdateMask { stream->read( &mMaterialName ); if ( isProperlyAdded() ) updateMaterial(); } }
virtual void draw(const GLMatrix4 &parentTransform, const GLMatrix4 &parentNormalTransform) { if(tornado) { for(int i = 0; i < 4; i++) vertices[i].color = 0xFF0033CC; } else { for(int i = 0; i < 4; i++) vertices[i].color = 0xFFCC3300; } const GLMatrix4 &t = parentTransform * transform, &nt = parentNormalTransform * normTransform; bindVertexArray(vertices); setTransform(t, nt); glDrawArrays(GL_TRIANGLE_STRIP, 0, 4); drawChildren(t, nt); }
void QQHuntPixmapItem::loadNextPixMap() { if(! m_listPixmapProp.isEmpty()) { m_animPixmapTimer.stop(); QQPixmapProp pixmapProp = m_listPixmapProp.at(0); m_listPixmapProp.remove(0); m_currPixmapName = BASE_ANIM_PIX.append(pixmapProp.m_pixmapName); animateMove(); QPixmap p = pixmap(); int scaleX = pixmapProp.m_mirrorX ? -1 : 1; int scaleY = pixmapProp.m_mirrorY ? -1 : 1; setTransformOriginPoint(p.width() / 2, p.height() / 2); setTransform(QTransform().scale(scaleX, scaleY).rotateRadians(pixmapProp.m_rotationAngle, Qt::ZAxis)); setPixmap(p); if(m_animPixmapTimerEnabled) m_animPixmapTimer.start(); } }
void ColorSchemeGraphicsItem::updateBrushes() { const auto parent = dynamic_cast<QWidget*>(parentWidget()); const auto penWidthF = 1.0 / DpiAware::devicePixelRatio(parent); for (uint i = 0; i < m_classes; ++i) { auto rect(m_rects[i]); auto color = m_scheme.colors(m_classes)[m_classes - i - 1]; color = daltonize(color, m_deficiency); auto brush = RGBABrush(color); // this aligns the brush pattern with the rect (since there is no way to directly set the brush origin similar to a painter) brush.setTransform(QTransform::fromTranslate( // who cares ... :P m_margin + 1, (m_padding + penWidthF) * static_cast<qreal>(i) + m_margin + 1)); rect->setBrush(brush); } }
void AbstractDesktopWidget::setRotation(float angle) { d->mAngle = angle; setCacheMode(ItemCoordinateCache); QPointF center = boundingRect().center(); QTransform mat = QTransform(); mat.translate(center.x(), center.y()); mat.rotate(d->mAngle, Qt::YAxis); mat.translate(-center.x(), -center.y()); setTransform(mat); if (d->mAngle >= 180) { if (state() == ROTATED) { setState(VIEW); } else { setState(ROTATED); } resetMatrix(); //setCacheMode(DeviceCoordinateCache); d->mAngle = 0; } }
void TextureProjection::fitTexture (std::size_t width, std::size_t height, const Vector3& normal, const Winding& w, float s_repeat, float t_repeat) { if (w.size() < 3) { return; } Matrix4 st2tex = m_texdef.getTransform((float) width, (float) height); // the current texture transform Matrix4 local2tex = st2tex; { Matrix4 xyz2st; basisForNormal(normal, xyz2st); matrix4_multiply_by_matrix4(local2tex, xyz2st); } // the bounds of the current texture transform AABB bounds; for (Winding::const_iterator i = w.begin(); i != w.end(); ++i) { Vector3 texcoord = matrix4_transformed_point(local2tex, (*i).vertex); aabb_extend_by_point_safe(bounds, texcoord); } bounds.origin.z() = 0; bounds.extents.z() = 1; // the bounds of a perfectly fitted texture transform AABB perfect(Vector3(s_repeat * 0.5, t_repeat * 0.5, 0), Vector3(s_repeat * 0.5, t_repeat * 0.5, 1)); // the difference between the current texture transform and the perfectly fitted transform Matrix4 matrix(Matrix4::getTranslation(bounds.origin - perfect.origin)); matrix4_pivoted_scale_by_vec3(matrix, bounds.extents / perfect.extents, perfect.origin); matrix4_affine_invert(matrix); // apply the difference to the current texture transform matrix4_premultiply_by_matrix4(st2tex, matrix); setTransform((float) width, (float) height, st2tex); m_texdef.normalise((float) width, (float) height); }
bool DeferredLayerUpdater::apply() { bool success = true; // These properties are applied the same to both layer types mLayer->setColorFilter(mColorFilter); mLayer->setAlpha(mAlpha, mMode); if (mSurfaceTexture.get()) { if (mNeedsGLContextAttach) { mNeedsGLContextAttach = false; mSurfaceTexture->attachToContext(mLayer->getTexture()); } if (mUpdateTexImage) { mUpdateTexImage = false; doUpdateTexImage(); } if (mTransform) { mLayer->getTransform().load(*mTransform); setTransform(0); } } return success; }
void DesktopWidget::spin() { d->angle += 18; setCacheMode(ItemCoordinateCache); QPointF center = boundingRect().center(); QTransform mat = QTransform(); mat.translate(center.x() , center.y()); mat.rotate(d->angle, Qt::YAxis); mat.translate(- center.x() , - center.y()); setTransform(mat); if (d->angle >= 180) { if (state() == BACKSIDE) { setState(NORMALSIDE); } else { setState(BACKSIDE); } d->spintimer->stop(); resetMatrix(); setCacheMode(DeviceCoordinateCache); d->angle = 0; } }
virtual void resizeEvent(QResizeEvent *event) { QGraphicsView::resizeEvent(event); QGraphicsScene *scene = this->scene(); if (scene) { QRectF newSceneRect(0, 0, event->size().width(), event->size().height()); scene->setSceneRect(newSceneRect); if (scene->sceneRect().size() != event->size()) { QSizeF from(scene->sceneRect().size()); QSizeF to(event->size()); QTransform transform; transform.scale(to.width() / from.width(), to.height() / from.height()); setTransform(transform); } else { resetTransform(); } setSceneRect(scene->sceneRect()); } MainWindow *main_window = qobject_cast<MainWindow *>(parentWidget()); if (main_window) main_window->fitBackgroundBrush(); }
void LabelItem::applyDataLockedDimensions() { PlotRenderItem *render_item = dynamic_cast<PlotRenderItem *>(parentViewItem()); if (render_item) { qreal parentWidth = render_item->width(); qreal parentHeight = render_item->height(); qreal parentX = render_item->rect().x(); qreal parentY = render_item->rect().y(); qreal parentDX = render_item->plotItem()->xMax() - render_item->plotItem()->xMin(); qreal parentDY = render_item->plotItem()->yMax() - render_item->plotItem()->yMin(); QPointF drP1 = _dataRelativeRect.topLeft(); QPointF drP2 = _dataRelativeRect.bottomRight(); QPointF P1(parentX + parentWidth*(drP1.x()-render_item->plotItem()->xMin())/parentDX, parentY + parentHeight*(render_item->plotItem()->yMax() - drP1.y())/parentDY); QPointF P2(parentX + parentWidth*(drP2.x()-render_item->plotItem()->xMin())/parentDX, parentY + parentHeight*(render_item->plotItem()->yMax() - drP2.y())/parentDY); qreal theta = atan2(P2.y() - P1.y(), P2.x() - P1.x()); qreal height = rect().height(); qreal width = rect().width(); if (_fixleft) { setPos(P1); setViewRect(0, -height, width, height); } else { setPos(P2); setViewRect(-width, -height, width, height); } QTransform transform; transform.rotateRadians(theta); setTransform(transform); updateRelativeSize(); } else { qDebug() << "apply data locked dimensions called without a render item (!)"; } }
void Player::setMountObject (GameBase *object, int in_mountPoint) { if(mount) clearNotify(mount); setMaskBits(MountMask); mount = object; mountPoint = in_mountPoint; if(mount) { deleteNotify(mount); TMat3F tmat; mount->getObjectMountTransform(mountPoint, &tmat); if (!mountPoint) { Point3F rot = getRot (); tmat.set (EulerF (rot.x, rot.y, rot.z), tmat.p); } setTransform(tmat); EulerF angles; tmat.angles(&angles); if (mountPoint < 1) setRot (Point3F (0, 0, angles.z)); else if (mountPoint == 1) setRot (Point3F (0, 0, 0)); else { Point3F rot = getRot (); rot.z -= angles.z; setRot (rot); } setImageTriggerUp(0); setLinearVelocity (Point3F (0, 0, 0)); } }
//============================================================================== void FreeJoint::setSpatialMotion(const Eigen::Isometry3d* newTransform, const Frame* withRespectTo, const Eigen::Vector6d* newSpatialVelocity, const Frame* velRelativeTo, const Frame* velInCoordinatesOf, const Eigen::Vector6d* newSpatialAcceleration, const Frame* accRelativeTo, const Frame* accInCoordinatesOf) { if (newTransform) setTransform(*newTransform, withRespectTo); if (newSpatialVelocity) setSpatialVelocity(*newSpatialVelocity, velRelativeTo, velInCoordinatesOf); if (newSpatialAcceleration) { setSpatialAcceleration(*newSpatialAcceleration, accRelativeTo, accInCoordinatesOf); } }
//-------------------------------------------------------------------------------------------------- /// //-------------------------------------------------------------------------------------------------- void RivWellFracturePartMgr::addPartAtPositiveAndNegativeTranslation(cvf::ModelBasicList* model, cvf::Part* part, const cvf::Vec3d& translation) { { cvf::ref<cvf::Transform> partTransform = RivWellFracturePartMgr::createLocalTransformFromTranslation(translation); part->setTransform(partTransform.p()); model->addPart(part); } { // Create a copy of the part to be able to assign a transformation matrix representing the translation in the opposite // direction cvf::ref<cvf::Transform> partTransform = RivWellFracturePartMgr::createLocalTransformFromTranslation(-translation); auto copy = part->shallowCopy(); copy->setTransform(partTransform.p()); model->addPart(copy.p()); } }
void MyShip::updt_pos() { setPos(pos().x() + MyRes::vw_mvmnt, pos().y()); setTransform(QTransform().translate(pixmap().size().width() / 2, pixmap().size().height() / 2).rotate(-rtn, Qt::XAxis).translate( -pixmap().size().width() / 2, -pixmap().size().height() / 2)); QPointF tplft(scene()->views().first()->viewport()->mapToParent( scene()->views().first()->mapFromScene(mapToScene(boundingRect().topLeft())))); QPointF bttmrght(scene()->views().first()->viewport()->mapToParent( scene()->views().first()->mapFromScene(mapToScene(boundingRect().bottomRight())))); QRectF vw_rct(scene()->views().first()->viewport()->rect()); if (tplft.x() + vlc->x() >= MyRes::x_offset && bttmrght.x() + vlc->x() <= vw_rct.width() - MyRes::x_offset) { setPos(pos().x() + vlc->x(), pos().y()); } if ((tplft.y() + vlc->y() >= MyRes::y_offset && bttmrght.y() + vlc->y() <= vw_rct.height() - MyRes::y_offset) || (tplft.y() + vlc->y() < MyRes::y_offset && vlc->y() > 0) || ((bttmrght.y() + vlc->y() > vw_rct.height() - MyRes::y_offset) && vlc->y() < 0)) { setPos(pos().x(), pos().y() + vlc->y()); } cnstrct_shldpxmp(); }
bool RigidBody::onAdd() { if (!Parent::onAdd()) return false; /* if (isServerObject() && Physics::getPhysics(false)==NULL) mHasServerPhysic = true; */ if (!mPhysShape)// && (isClientObject() || !mDataBlock->mOnlyOnClient || mHasServerPhysic) ) createPhysShape(); if (mPhysShape) { // Initialize interpolation vars. mDelta.rot[1] = mDelta.rot[0] = mPhysShape->getRotation(); mDelta.pos = mPhysShape->getPosition(); mDelta.posVec = Point3F(0,0,0); if (isClientObject()) { setTransform(mPhysShape->getTransform()); //for interpolation } } /*if (mDataBlock->mOnlyOnClient && isServerObject()) mTypeMask &= ~VehicleObjectType;*/ addToScene(); if (isServerObject()) scriptOnAdd(); setEnabled(SimComponent().mEnabled); return true; }
void ScreeniePixmapItem::updateItemGeometry() { QTransform transform; QTransform scale; QTransform translateBack; qreal centerScale = 1.0 - 0.9 * d->screenieModel.getDistance() / SceneLimits::MaxDistance; scale = QTransform().scale(centerScale, centerScale); QPixmap pixmap = this->pixmap(); qreal dx = pixmap.width() / 2.0; qreal dy; if (d->screenieModel.isReflectionEnabled()) { dy = pixmap.height() / 4.0; } else { dy = pixmap.height() / 2.0; } transform.translate(dx, dy); transform.rotate(d->screenieModel.getRotation(), Qt::YAxis); translateBack.translate(-dx, -dy); transform = translateBack * scale * transform; setTransform(transform, false); }
QgsComposerItem::QgsComposerItem( qreal x, qreal y, qreal width, qreal height, QgsComposition* composition, bool manageZValue ) : QObject( 0 ) , QGraphicsRectItem( 0, 0, width, height, 0 ) , mComposition( composition ) , mBoundingResizeRectangle( 0 ) , mHAlignSnapItem( 0 ) , mVAlignSnapItem( 0 ) , mFrame( false ) , mBackground( true ) , mItemPositionLocked( false ) , mLastValidViewScaleFactor( -1 ) , mRotation( 0 ) , mBlendMode( QPainter::CompositionMode_SourceOver ) , mTransparency( 0 ) , mLastUsedPositionMode( UpperLeft ) , mId( "" ) , mUuid( QUuid::createUuid().toString() ) { init( manageZValue ); QTransform t; t.translate( x, y ); setTransform( t ); }
void ODEDrawer::DrawTriMeshes() { setColor(0.5,0.5,0.4,0.51); for (ODETriMesh *b : m_domain->getTriMeshes()) { glPushMatrix(); setTransform (b->getGeomPosition(),b->getGeomRotation()); int numi = b->numi; for (int i=0; i<numi/3; i++) { int i0 = b->world_indices[i*3+0]; int i1 = b->world_indices[i*3+1]; int i2 = b->world_indices[i*3+2]; double *v0 = (b->world_vertices+i0*3); double *v1 = (b->world_vertices+i1*3); double *v2 = (b->world_vertices+i2*3); drawTriangle(v0, v1, v2, false); } glPopMatrix(); } }
void SCgView::setScale(const QString& sc) { QTransform t = transform(); //Default transform t.reset(); //Getting percent value QString str = sc; str.remove("%"); double d = str.toDouble()/100.0; //Checking if value d in proper range if (d < SCgWindow::minScale) d = SCgWindow::minScale; else if (d > SCgWindow::maxScale) d = SCgWindow::maxScale; //Setting transformation setTransform(t.scale(d,d),false); emit(scaleChanged(transform().mapRect(QRectF(0, 0, 1, 1)).width())); }
void NavMesh::unpackUpdate(NetConnection *conn, BitStream *stream) { Parent::unpackUpdate(conn, stream); mathRead(*stream, &mObjToWorld); mathRead(*stream, &mObjScale); setTransform(mObjToWorld); mSaveIntermediates = stream->readFlag(); mFileName = stream->readSTString(); mRenderMode = (RenderMode)stream->readInt(8); mRenderInput = stream->readFlag(); mRenderConnections = stream->readFlag(); stream->read(&mCellSize); stream->read(&mCellHeight); stream->read(&mWalkableHeight); stream->read(&mWalkableClimb); stream->read(&mWalkableRadius); stream->read(&mWalkableSlope); }
void Map2DViewWidget::setZoom(qreal zoom) { if (zoom > 500) { zoom = 500; } else if (zoom < 10) { zoom = 10; } if (m_zoom != zoom) { const qreal scale = (0.01 * zoom); QTransform transform; transform.scale(scale, scale); setTransform(transform); m_zoom = zoom; emit zoomChanged(zoom); } }
// reaching the obstacle avoidance controller - SECOND CONTROLLER void obstacle_avoidance_controller(custom_pose robot_pose,icarsCarControlWebsocket::SimRefs &msg_ctr, double &New_A_lc,double &New_B_lc,double &A_lc,double &B_lc) { //initializations geometry_msgs::Pose closest_obstacle, diff_obs_goal,new_robot_pose, changednew_robot_pose; custom_pose robot_trajectory,robot_trajectory_d; Eigen::Matrix3d rot = Eigen::Matrix3d::Identity(); Eigen::Matrix4d updated_rot, TFLocal=Eigen::Matrix4d::Identity(); Eigen::Vector3d tr; double k_p = 0.6; double mou = 0.6; // double C = 30.0; double distance_1 = nearestobstacle(p_space); double distance_2 = nearestobstacle(o_space); if (distance_1<distance_2){ closest_obstacle=p_space; // actual A_lc and B_lc values A_lc = p_marker_size_A + margin + 2.588; B_lc = p_marker_size_B + margin + 1.5110; } else{ closest_obstacle=o_space; // actual A_lc and B_lc values A_lc = o_marker_size_A + margin + 2.588; B_lc = o_marker_size_B + margin + 1.5110; } // difference between obstacle and goal diff_obs_goal.position.x = closest_obstacle.position.x - final_goal.position.x; diff_obs_goal.position.y = closest_obstacle.position.y - final_goal.position.y; double theta_diff_obs_goal = ((atan(diff_obs_goal.position.y/diff_obs_goal.position.x))); // creating a specfic reference frame with obstacle as a origin facing towards the target tf::TransformBroadcaster br; tf::Transform transform; transform.setOrigin(tf::Vector3(closest_obstacle.position.x,closest_obstacle.position.y,0.0)); tf::Quaternion q; q.setRPY(0,0,theta_diff_obs_goal); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform,msg_pf_stat.header.stamp,"/FLUENCE/submap", "/obstacle_frame")); // changing the transformations of car w.r.t to specific reference frame TransformationMatrix(closest_obstacle,robot_pose,rot,theta_diff_obs_goal); tr << closest_obstacle.position.x, closest_obstacle.position.y, 0; setTransform(rot,tr,TFLocal); updated_rot = TFLocal.inverse(); // new car position w.r.t new reference frame new_robot_pose.position.x = (robot_pose.x * updated_rot(0,0)) + (robot_pose.y * updated_rot(0,1)) + (0 * updated_rot(0,2)) + (1 * updated_rot(0,3)); new_robot_pose.position.y = (robot_pose.x * updated_rot(1,0)) + (robot_pose.y * updated_rot(1,1)) + (0 * updated_rot(1,2)) + (1 * updated_rot(1,3)); changednew_robot_pose.position.x = new_robot_pose.position.y; changednew_robot_pose.position.y = new_robot_pose.position.x; // obtaining the new A_lc' and B_lc' values of the limit_cycle if (changednew_robot_pose.position.x <= 0){ New_A_lc = A_lc - margin_1; New_B_lc = B_lc - margin_1; // ROS_INFO("Attraction"); } else{ New_A_lc = New_A_lc + margin_2; New_B_lc = New_B_lc + margin_2; // ROS_INFO("Replusion"); } // deriving the cartesian equation of ellipse (A,B,C,D,E,f) double ellipse_angle = -45 * 3.14/180; double A = (pow((New_A_lc),2) * pow(sin(ellipse_angle),2)) + (pow((New_B_lc),2) * pow(cos(ellipse_angle),2)); double B = 2 * (pow((New_B_lc),2) - pow((New_A_lc),2)) * (sin(ellipse_angle) * cos(ellipse_angle)); double C = (pow((New_A_lc),2) * pow(cos(ellipse_angle),2)) + (pow((New_B_lc),2) * pow(sin(ellipse_angle),2)); double D = (-2*(A * closest_obstacle.position.x))-(closest_obstacle.position.y * (B)); double E = (-(B * closest_obstacle.position.x))-(2 * closest_obstacle.position.y * (C)); double F = pow((A * closest_obstacle.position.x),2) + (closest_obstacle.position.x * closest_obstacle.position.y * (B)) + pow((C * closest_obstacle.position.y),2) - (pow((New_A_lc),2)*pow((New_B_lc),2)); double Ae = A/abs(F); double Be = 2*B/abs(F); double Ce = C/abs(F); // calulcation of set points if (changednew_robot_pose.position.y >= 0){ // clockwise trajectory motion robot_trajectory.x = changednew_robot_pose.position.y + (mou * changednew_robot_pose.position.x) * (1-((pow(changednew_robot_pose.position.x,2)/pow(New_A_lc,2)))-((pow(changednew_robot_pose.position.y,2)/pow(New_B_lc,2)))-(C* changednew_robot_pose.position.x*changednew_robot_pose.position.y)); robot_trajectory.y = -changednew_robot_pose.position.x + (mou * changednew_robot_pose.position.y) * (1-((pow(changednew_robot_pose.position.x,2)/pow(New_A_lc,2)))-((pow(changednew_robot_pose.position.y,2)/pow(New_B_lc,2)))-(C* changednew_robot_pose.position.x*changednew_robot_pose.position.y)); /* robot_trajectory.x = 1 * (changednew_robot_pose.position.y * Ce + 0.5 * Be * changednew_robot_pose.position.x) + mou * changednew_robot_pose.position.x * (1-(Ae * pow(changednew_robot_pose.position.x,2)))-(Be * changednew_robot_pose.position.x * changednew_robot_pose.position.y)-(Ce * pow(changednew_robot_pose.position.y,2)); robot_trajectory.y = -1 * (changednew_robot_pose.position.x * Ae + 0.5 * Be * changednew_robot_pose.position.y) + mou * changednew_robot_pose.position.y * (1-(Ae * pow(changednew_robot_pose.position.x,2)))-(Be * changednew_robot_pose.position.x * changednew_robot_pose.position.y)-(Ce * pow(changednew_robot_pose.position.y,2)); */ //derivative of the previous trajectory robot_trajectory_d.x = 1 * (robot_trajectory.y * Ce + 0.5 * Be * robot_trajectory.x) + mou * robot_trajectory.x * (1-(Ae * pow(changednew_robot_pose.position.x,2)))-(Be * changednew_robot_pose.position.x * changednew_robot_pose.position.y)-(Ce * pow(changednew_robot_pose.position.y,2)) + mou * changednew_robot_pose.position.x * (1-( 2* Ae * changednew_robot_pose.position.x * robot_trajectory.x))-(Be * changednew_robot_pose.position.x * robot_trajectory.y)- (Be * changednew_robot_pose.position.y * robot_trajectory.x) -(2 * Ce * changednew_robot_pose.position.y * robot_trajectory.y); robot_trajectory_d.y = -1 * (robot_trajectory.x * Ae + 0.5 * Be * robot_trajectory.y) + mou * robot_trajectory.y * (1-(Ae * pow(changednew_robot_pose.position.x,2)))-(Be * changednew_robot_pose.position.x * changednew_robot_pose.position.y)-(Ce * pow(changednew_robot_pose.position.y,2)) + mou * changednew_robot_pose.position.x * (1-(2 * Ae * changednew_robot_pose.position.x * robot_trajectory.x))-(Be * changednew_robot_pose.position.x * robot_trajectory.y)- (Be * changednew_robot_pose.position.y * robot_trajectory.x) -(2 * Ce * changednew_robot_pose.position.y * robot_trajectory.y); // ROS_INFO("clockwise"); } else{ // counter clockwise trajectory motion robot_trajectory.x = -changednew_robot_pose.position.y + (mou * changednew_robot_pose.position.x) * (1-((pow(changednew_robot_pose.position.x,2)/pow(New_A_lc,2)))-((pow(changednew_robot_pose.position.y,2)/pow(New_B_lc,2)))-(C* changednew_robot_pose.position.x*changednew_robot_pose.position.y)); robot_trajectory.y = changednew_robot_pose.position.x + (mou * changednew_robot_pose.position.y) * (1-((pow(changednew_robot_pose.position.x,2)/pow(New_A_lc,2)))-((pow(changednew_robot_pose.position.y,2)/pow(New_B_lc,2)))-(C* changednew_robot_pose.position.x*changednew_robot_pose.position.y)); /* robot_trajectory.x = -1 * (changednew_robot_pose.position.y * Ce + 0.5 * Be * changednew_robot_pose.position.x) + mou * changednew_robot_pose.position.x * (1-(Ae * pow(changednew_robot_pose.position.x,2)))-(Be * changednew_robot_pose.position.x * changednew_robot_pose.position.y)-(Ce * pow(changednew_robot_pose.position.y,2)); robot_trajectory.y = 1 * (changednew_robot_pose.position.x * Ae + 0.5 * Be * changednew_robot_pose.position.y) + mou * changednew_robot_pose.position.y * (1-(Ae * pow(changednew_robot_pose.position.x,2)))-(Be * changednew_robot_pose.position.x * changednew_robot_pose.position.y)-(Ce * pow(changednew_robot_pose.position.y,2)); */ //derivative of the previous trajectory robot_trajectory_d.x = -1 * (robot_trajectory.y * Ce + 0.5 * Be * robot_trajectory.x) + mou * robot_trajectory.x * (1-(Ae * pow(changednew_robot_pose.position.x,2)))-(Be * changednew_robot_pose.position.x * changednew_robot_pose.position.y)-(Ce * pow(changednew_robot_pose.position.y,2)) + mou * changednew_robot_pose.position.x * (1-( 2* Ae * changednew_robot_pose.position.x * robot_trajectory.x))-(Be * changednew_robot_pose.position.x * robot_trajectory.y)- (Be * changednew_robot_pose.position.y * robot_trajectory.x) -(2 * Ce * changednew_robot_pose.position.y * robot_trajectory.y); robot_trajectory_d.y = 1 * (robot_trajectory.x * Ae + 0.5 * Be * robot_trajectory.y) + mou * robot_trajectory.y * (1-(Ae * pow(changednew_robot_pose.position.x,2)))-(Be * changednew_robot_pose.position.x * changednew_robot_pose.position.y)-(Ce * pow(changednew_robot_pose.position.y,2)) + mou * changednew_robot_pose.position.x * (1-(2 * Ae * changednew_robot_pose.position.x * robot_trajectory.x))-(Be * changednew_robot_pose.position.x * robot_trajectory.y)- (Be * changednew_robot_pose.position.y * robot_trajectory.x) -(2 * Ce * changednew_robot_pose.position.y * robot_trajectory.y); // ROS_INFO("counter clockwise"); } // new car position w.r.t new reference frame // robot_trajectory.x = (robot_trajectory.x * TFLocal(0,0)) + (robot_trajectory.y * TFLocal(0,1)) + (0 * TFLocal(0,2)) + (1 * TFLocal(0,3)); // robot_trajectory.y = (robot_trajectory.x * TFLocal(1,0)) + (robot_trajectory.y * TFLocal(1,1)) + (0 * TFLocal(1,2)) + (1 * TFLocal(1,3)); robot_trajectory.theta = atan(robot_trajectory.y/robot_trajectory.x); robot_trajectory_d.theta = atan(robot_trajectory_d.y/robot_trajectory_d.x); double theta_error = robot_trajectory.theta - robot_pose.theta; double linear_velocity = 0.000000000000000000000000000001; // double d = sqrt(pow((robot_pose.x-final_goal.position.x),2) + pow((robot_pose.y-final_goal.position.y),2)); // double linear_velocity = linear_velocity_max * exp(-1/d) * cos(theta_error); // double angular_velocity = (linear_velocity * sin(theta_error) / (d)) + k_p * theta_error; // double angular_velocity = (((robot_trajectory.theta + timeStep)- (robot_trajectory.theta - timeStep))/2*timeStep) + k_p * theta_error; // double dy = (((robot_trajectory.y + timeStep)- (robot_trajectory.y - timeStep))/2*timeStep); // double dx = (((robot_trajectory.x + timeStep)- (robot_trajectory.x - timeStep))/2*timeStep); double angular_velocity = (robot_trajectory_d.theta)+ k_p * theta_error; double steering_angle=angles::to_degrees(angular_velocity); // std::cout << "angular_velocity : " << angular_velocity << std::endl; msg_ctr.steerAng= angular_velocity; msg_ctr.brakeStrength=0.0; msg_ctr.gasForce=linear_velocity; // return msg_ctr; }
void DeviceSkin::setZoom( double z ) { setTransform(QMatrix().scale(z,z)); }