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));
}
