void Flying::handleMovement(Vector2D velocity)
{
Vector2D newPos = m_position;
newPos.y(newPos.y() + velocity.y());
// Check collisions in y axis
if (newPos.y() + getCollider().y <= 0
|| newPos.y() + getCollider().y + getCollider().h >= TheGame::Instance().getMapHeight()
|| checkCollideTile(newPos)
|| checkCollideObject(newPos))
{
m_bYCollision = !m_bYCollision;
}
else
{
m_position.y(newPos.y());
}
// Update new position
newPos = m_position;
newPos.x(m_position.x() + velocity.x());
// Check collisions in x axis
if (newPos.x() + getCollider().x <= 0
|| newPos.x() + getCollider().x + getCollider().w >= TheGame::Instance().getMapWidth()
|| checkCollideTile(newPos)
|| checkCollideObject(newPos))
{
// There have been a collision, so change direction
m_bXCollision = !m_bXCollision;
}
else
{
// Move to new x position
m_position.x(newPos.x());
}
}
void MarkerMapObject::paint(QPainter &painter, QRect view, int scale) {
// Save the painter state for this paint session
// so that it can be restored after without influencing other objects
painter.save(); {
// Translate, scale, and rotate...
QTransform t;
t.scale(1.0/scale, 1.0/scale);
t.translate(x - view.x()*scale, view.height()*scale - y + view.y()*scale);
painter.setTransform(t);
for(int index = 0; index < transformationCount; index++) {
long arrowLength = 1000*scale;
long ballSize = 20*scale;
Vector2D a = transformation[index].trans();
Vector2D b = (transformation[index] * Trafo2D::trans(0, arrowLength)).trans();
painter.setPen(transformationColors[index]);
QPen pen = painter.pen();
pen.setWidth(2*scale);
painter.setPen(pen);
painter.drawLine(a.x(), -a.y(), b.x(), -b.y());
painter.setBrush(QBrush(transformationColors[index]));
painter.drawEllipse(a.x()-ballSize/2, -(a.y()+ballSize/2), ballSize, ballSize);
}
} painter.restore();
}
void BounceSystem::update() {
for (size_t i = 0; i < entityList.size(); i++) {
BaseEntity* current = entityList[i];
DimensionComponent *dim = (DimensionComponent*) current->getComponent("dimension");
SDL_Rect *rect = dim->getRect();
Vector2D *position = dim->getPosition();
int centerx = position->x() + (rect->w / 2);
int centery = position->y() + (rect->h / 2);
if (centerx >= mWidth) {
position->x() = 1.f;
}
if (centerx < 0) {
position->x() = mWidth - rect->w - 1;
}
if (centery >= mHeight) {
position->y() = 1.f;
}
if (centery < 0) {
position->y() = mHeight - 1.f;
}
}
}
// Returns the rough angle in degrees the robot needs to turn in order to
// be properly aligned with the spline...
double SplineNavigation::getAngleForSplineAlignment() {
// Get two points, a and b, where a is the origin and b is very close to the origin node.
// The slope of the two will give us the angle
Vector2D a = Vector2D(navSplineX->getValue(0, 0.0), navSplineY->getValue(0, 0.0));
Vector2D b = Vector2D(navSplineX->getValue(0, 0.1), navSplineY->getValue(0, 0.1));
double angle = -Deg(std::atan2(b.x() - a.x(), b.y() - a.y()));
return angle;
}
void VectorTest::draw(){
Vector2D u = t * Vector2D(0,0);
Vector2D v = t * Vector2D(1,0);
Vector2D w = t * Vector2D(0,1);
graph->draw(u.x(), u.y(), v.x(), v.y(), w.x(), w.y(), sliderScale->value());
}
void RTreeLeaf::computeBoundingBox()
{
float lc_x = point.x();
float lc_y = point.y();
float hc_x = point.x();
float hc_y = point.y();
bounding_box = Rectangle(Vector2D(lc_x, lc_y), Vector2D(hc_x, hc_y));
bounding_box.ensureMinSideLength(MIN_BB_SIDE_LENGTH);
}
bool StructureMap::isPointOutside(Vector2D p){
// If the StructureMap isn't finish, no Point can be calculated
if (!finish) return false;
// Init variables
double x1, x2;
int crossings = 0;
double eps = 0.000001;
// Check each line
for(int i = 0; i < collisions.count(); i++){
// Make sure that it doesn't depend on the direction left to right or right to left.
if ( collisions.at(i).x() < collisions.at( (i+1) % collisions.count() ).x() ){
x1 = collisions.at(i).x();
x2 = collisions.at( (i+1) % collisions.count() ).x();
}
else {
x1 = collisions.at( (i+1) % collisions.count() ).x();
x2 = collisions.at(i).x();
}
// Check if a line is in the range of the checking Point
if (p.x() > x1 && p.x() <= x2 && ( p.y() < collisions.at(i).y() || p.y() < collisions.at( (i+1)%collisions.count() ).y() )){
double dx = collisions.at( (i+1)%collisions.count() ).x() - collisions.at(i).x();
double dy = collisions.at( (i+1)%collisions.count() ).y() - collisions.at(i).y();
double k = 0;
if ( std::abs(dx) < eps ){
k = INT_MAX;
}
else {
k = dy/dx;
}
double m = collisions.at(i).y() - (k * collisions.at(i).x());
double y2 = k * p.x() + m;
if (p.y() <= y2){
crossings++;
}
}
}
if (crossings %2 == 1){
// Point p is inside the polygon
return false;
}
return true;
}
Polyangle Polyangle::shrink(const double l) const
{
int length = lesPoints.size();
QVector<Vector2D> newPoints;
newPoints.resize(length);
for(int cpt = 0; cpt <length ; cpt++)
{
Vector2D a = lesPoints.at((cpt-1+length)%length);
Vector2D b = lesPoints.at((cpt));
Vector2D c = lesPoints.at((cpt+1)%length);
Vector2D ab = b-a;
Vector2D bc = c-b;
Vector2D ca = c-a;
//calcul des vecteurs orthogonaux
Vector2D orthoAB = Vector2D(-ab.y(), ab.x());
Vector2D orthoBC = Vector2D(-bc.y(), bc.x());
//on l'inverse si il est dans le mauvais sens
if(orthoAB*bc < 0)
{
orthoAB = -orthoAB;
}
if(orthoBC*ca > 0)
{
orthoBC = -orthoBC;
}
orthoAB.normalize();
orthoBC.normalize();
//création des droites afin de récupérer le point d'intersection
Vector2D aPrime = a + orthoAB * l;
Vector2D bPrime = b + orthoAB * l;
Vector2D cPrime;
Droite aPrimeBPrime(aPrime, bPrime - aPrime);
bPrime = b + orthoBC * l;
cPrime = c + orthoBC * l;
Droite bPrimeCPrime(bPrime, cPrime - bPrime);
aPrimeBPrime.getIntersection(bPrimeCPrime, aPrime);
newPoints[cpt] = aPrime;
}
return Polyangle(newPoints);
}
void
Flatland::Body::x( double value )
{
Vector2D tmp = position();
tmp.x( value );
return cpBodySetPos( _body, tmp );
}
void JoystickNavigationTask::process() {
if(!navigation) return;
Vector2D wheelSpeed = navigation->getWheelSpeed(0,0);
create->controller->setWheelSpeed((short)wheelSpeed.x(), (short)wheelSpeed.y());
}
Vector3D Terrain::norm(const Vector2D &p) const
{
Vector2D px(p.x()+epsilon,p.y());
Vector2D pxminus(p.x()-epsilon, p.y());
Vector2D py(p.x(),p.y()+epsilon);
Vector2D pyminus(p.x(),p.y()-epsilon);
double hpx=getHauteur(px);
double hpxminus = getHauteur(pxminus);
double hpy=getHauteur(py);
double hpyminus = getHauteur(pyminus);
Vector3D resu(-(hpx-hpxminus),2*epsilon,-(hpy-hpyminus));
resu.normalize();
return resu;
}
QImage Camera::printScreen(Terrain &t,const Vector2D& a, const Vector2D& b, const Vector3D& s, int l, int h)
{
QImage im(l,h,QImage::Format_ARGB32);
double min=t.getHauteurMin(a,b);
double max=t.getHauteurMax(a,b);
Vector3D aBox(a.x(),min,a.y());
Vector3D bBox(b.x(),max*1.5,b.y());
for(int i=0; i<l; ++i) {
for(int j=0; j<h; ++j) {
im.setPixel(i,h-1-j,ptScreen(t,aBox,bBox,s,i,j,l,h));
}
}
return im;
}
void SplineNavigation::processAddNavPoint(Vector2D point) {
Navigation::processAddNavPoint(point);
// Append points to our spline datastructures
navSplineX->addNode(point.x());
navSplineY->addNode(point.y());
// Calculate points for wheel splines...
calculateLeftRightWheelSpline();
}
Vector3D Terrain::norm(const Vector2D &p)
{
/* Vector2D px(p.x()+epsilon,p.y());
Vector2D py(p.x(),p.y()+epsilon);
double hp=getHauteur(p);
double hpx=getHauteur(px);
double hpy=getHauteur(py);
Vector3D vp(p.x(),p.y(),(double)hp);
Vector3D vpx(px.x(),px.y(),(double)hpx);
Vector3D vpy(py.x(),py.y(),(double)hpy);
Vector3D u=vpx-vp;
Vector3D v=vpy-vp;
Vector3D n(-u.z()/u.x(),1.0f,-v.z()/v.y());
n.normalize();
return n;*/
//std::cout<<p.x()<<" "<<p.x()+epsilon<<std::endl;
Vector2D px(p.x()+epsilon,p.y());
Vector2D pxminus(p.x()-epsilon, p.y());
Vector2D py(p.x(),p.y()+epsilon);
Vector2D pyminus(p.x(),p.y()-epsilon);
double hpx=getHauteur(px);
double hpxminus = getHauteur(pxminus);
double hpy=getHauteur(py);
double hpyminus = getHauteur(pyminus);
//std::cout<<hpx<<" "<<hpxminus<<std::endl;
Vector3D resu(-(hpx-hpxminus),2*epsilon,-(hpy-hpyminus));
resu.normalize();
return resu;
}
void Rectangle::ensureMinSideLength(float min_side_length)
{
float lc_x = low_corner.x();
float lc_y = low_corner.y();
float hc_x = high_corner.x();
float hc_y = high_corner.y();
float diff_x = hc_x - lc_x;
if (diff_x < min_side_length)
{
hc_x += 0.5f * (min_side_length - diff_x);
lc_x -= 0.5f * (min_side_length - diff_x);
}
float diff_y = hc_y - lc_y;
if (diff_y < min_side_length)
{
hc_y += 0.5f * (min_side_length - diff_y);
lc_y -= 0.5f * (min_side_length - diff_y);
}
low_corner = Vector2D(lc_x, lc_y);
high_corner = Vector2D(hc_x, hc_y);
}
ProbeGrid::ProbeGrid(const Rectangle2D &bbox, const Vector2D &divisions) :
bbox(bbox), divisions(divisions),
cellSize(bbox.getWidth() / divisions.x(), bbox.getLength() / divisions.y()) {
// Resize vectors
resize(divisions.y() + 1, Column_t(divisions.x() + 1));
// Fill in coords
double y = bbox.getMin().y();
for (ProbeGrid::row_iterator row = begin(); row != end(); row++) {
double x = bbox.getMin().x();
for (ProbeGrid::col_iterator col = row->begin(); col != row->end(); col++) {
col->x() = x;
col->y() = y;
x += cellSize.x();
}
y += cellSize.y();
}
}
void FingerprintNavigationTask::process(){
// Follow the wall
Vector2D wheelSpeed = navigation->getWheelSpeed(0,0);
core->controller->setWheelSpeed((short)wheelSpeed.x(), (short)wheelSpeed.y());
// Is it time for a fingerprint?
if(QTime::currentTime() > lastFingerprint.addMSecs(fingerprintInterval + fingerprintWaitTime)) {
// Create and send message with emulated data
if(core->remoteInterface) {
// Send a fingerprint message...
RemoteInterfaceMessage *message = new RemoteInterfaceMessage("fingerprint");
// Local position
message->properties->insert("local-position-x", QString("%1").arg(core->tracker->getX()));
message->properties->insert("local-position-y", QString("%1").arg(core->tracker->getY()));
// Get GPS coordinates based on an offset added with the degrees based on x,y meters at that offset
GPSPosition gps = Util::gpsPositionFromString(core->stringSetting("Map_GPSOffset"));
gps.latitude += gps.latitudesPerMeters(core->tracker->getX() / 1000.0); // convert mm to m
gps.longitude += gps.longitudesPerMeters(core->tracker->getY() / 1000.0); // convert mm to m
message->properties->insert("global-position-latitude", QString("%1").arg(gps.latitude));
message->properties->insert("global-position-longitude", QString("%1").arg(gps.longitude));
// Add additional map information
message->properties->insert("floor", core->stringSetting("Map_Floor"));
message->properties->insert("map-description", core->stringSetting("Map_Description"));
message->properties->insert("map-file", core->stringSetting("Map_PhysicalMap_Filename"));
message->properties->insert("wait-time", QString("%1").arg(fingerprintWaitTime));
// Send off the message as a broadcast to all connected
core->remoteInterface->broadcastMessage(message);
}
else {
Debug::warning("[FingerprintNavigationTask] no RemoteInterface available for sending fingerprint message!");
}
// Stop the robot and wait
core->controller->setWheelSpeed(0, 0);
lastFingerprint = QTime::currentTime();
SleeperThread::msleep(fingerprintWaitTime);
}
// Is Task finished
if (core->structureMap->isFinish() == true){
this->status = Finished;
Debug::print("[FingerprintTask] FingerprintTask Finished");
}
}
void SplineNavigation::calculateLeftRightWheelSpline() {
double wheelNodeDistanceBetweenNode = targetSpeed * ((double) interval / 1000.0);
// Init and drop nodes
wheelLeftSplineX->clearNodes();
wheelLeftSplineY->clearNodes();
wheelRightSplineX->clearNodes();
wheelRightSplineY->clearNodes();
// Calculate new nodes for wheel splines
for (int navNode = 0; navNode < navSplineX->getNodeCount() - 1; navNode++) {
// Get the distance between the two nav nodes
double distanceBetweenNodes = getDistanceBetweenNavNodes(navNode, navNode+1);
// Create the wheel nodes at equal distance between the two nodes
for (int wheelNode = 0; wheelNode < (distanceBetweenNodes / wheelNodeDistanceBetweenNode); wheelNode++) {
// Get the t value for the nav node, which is discretisized based on the wheel node distance between nodes
double t = (double) wheelNode / (double) (distanceBetweenNodes / wheelNodeDistanceBetweenNode);
// Get the node positions for both the left and right wheel splines
Vector2D c(navSplineX->getValue(navNode, t), navSplineY->getValue(navNode, t));
Vector2D c1(navSplineX->getFirstDerivative(navNode, t), navSplineY->getFirstDerivative(navNode, t));
Vector2D v = c1 / norm(c1);
Vector2D n = Vector2D(-v.y(), v.x());
Vector2D l = c + (wheelOffset * n);
Vector2D r = c - (wheelOffset * n);
wheelLeftSplineX->addNode(l.x());
wheelLeftSplineY->addNode(l.y());
wheelRightSplineX->addNode(r.x());
wheelRightSplineY->addNode(r.y());
//Debug::print("n=%3 nlx=%1 wlx=%2", l.x(), wheelLeftSplineX->getValue(wheelNode, 0.0), wheelNode);
}
}
}
std::pair<LocalPoint,LocalVector> secondOrderAccurate(
const GlobalPoint& startingPos,
const GlobalVector& startingDir,
double rho, const Plane& plane)
{
typedef Basic2DVector<float> Vector2D;
// translate problem to local frame of the plane
LocalPoint lPos = plane.toLocal(startingPos);
LocalVector lDir = plane.toLocal(startingDir);
LocalVector yPrime = plane.toLocal( GlobalVector(0,0,1.f));
float sinPhi=0, cosPhi=0;
Vector2D pos;
Vector2D dir;
sinPhi = yPrime.y();
cosPhi = yPrime.x();
pos = Vector2D( lPos.x()*cosPhi + lPos.y()*sinPhi,
-lPos.x()*sinPhi + lPos.y()*cosPhi);
dir = Vector2D( lDir.x()*cosPhi + lDir.y()*sinPhi,
-lDir.x()*sinPhi + lDir.y()*cosPhi);
double d = -lPos.z();
double x = pos.x() + dir.x()/lDir.z()*d - 0.5*rho*d*d;
double y = pos.y() + dir.y()/lDir.z()*d;
LocalPoint thePos( x*cosPhi - y*sinPhi,
x*sinPhi + y*cosPhi, 0);
float px = dir.x()+rho*d;
LocalVector theDir( px*cosPhi - dir.y()*sinPhi,
px*sinPhi + dir.y()*cosPhi, lDir.z());
return std::pair<LocalPoint,LocalVector>(thePos,theDir);
}
void Chaser::update()
{
if (m_playerPos != nullptr)
{
Vector2D diff = *m_playerPos - m_position;
// Check if player is inside Chaser field of vision
if (diff.length() <= m_viewDistance)
{
if (abs(diff.x()) < m_treshold)
{
// Player caught in x axis, stop
m_velocity.x(0);
}
else
{
if (diff.x() < 0)
{
// x speed can be negative so we have to take abs value
m_velocity.x(-abs(m_xSpeed));
}
else if (diff.x() > 0)
{
//m_velocity.x(m_xSpeed);
m_velocity.x(abs(m_xSpeed));
}
}
}
else
{
// Player is outside Chaser's vision camp
m_velocity.x(0);
}
m_velocity.y(m_ySpeed);
handleMovement(m_velocity);
}
handleAnimation();
}
double smooth_noiseBilinear(Vector2D p,int seed){
double x;
double y;
double fractional_x;
double fractional_y;
int integer_x;
int integer_y;
if(p.x()>=0){
x=p.x();
integer_x = (int)x;
fractional_x = x - integer_x;
}
else{
integer_x=((int)p.x())-1;
fractional_x=p.x()-integer_x;
}
if(p.y()>=0){
y=p.y();
integer_y = (int)y;
fractional_y = y - integer_y;
}
else{
integer_y=((int)p.y())-1;
fractional_y=p.y()-integer_y;
}
Vector2D pf(fractional_x,fractional_y);
double xy = noise(integer_x,integer_y,seed);
double x1y = noise(integer_x+1,integer_y,seed);
double xy1 = noise(integer_x,integer_y+1,seed);
double x1y1 = noise(integer_x+1,integer_y+1,seed);
return bilinearInterpolate(xy,x1y,xy1,x1y1,pf);
}
/**
* @Inherit
*/
inline virtual void draw(SimViewer::SimViewer& viewer,
const Vector2D& posRoot, scalar angleRoot,
const Vector2D& posLeaf, scalar angleLeaf,
scalar value)
{
Joint::draw(viewer, posRoot, angleRoot,
posLeaf, angleLeaf, value);
Vector2D pos = posRoot + Vector2D::rotate(
Joint::getPosRoot(), angleRoot);
viewer.drawJoint(pos.x(), pos.y(),
(Joint::getAngleRoot()+angleRoot)*180.0/M_PI,
(value)*180.0/M_PI);
}
double smooth_noise(Vector2D p,int seed){
double x=p.x();
double y=p.y();
int integer_x = (int)x;
double fractional_x = x - integer_x;
int integer_y = (int)y;
double fractional_y = y - integer_y;
double t0 = smooth_noise_firstdim(integer_x,
integer_y - 1, fractional_x,seed);
double t1 = smooth_noise_firstdim(integer_x,
integer_y, fractional_x,seed);
double t2 = smooth_noise_firstdim(integer_x,
integer_y + 1, fractional_x,seed);
double t3 = smooth_noise_firstdim(integer_x,
integer_y + 2, fractional_x,seed);
double resu= cubic_interpolate(t0, t1, t2, t3, fractional_y);
return MathUtils::fonctionQuadratique(-0.2,1.2,resu);
}
vector<ProbePoint *> ProbeGrid::find(const Vector2D &p) {
double xA = (p.x() - bbox.getMin().x()) / cellSize.x();
double yA = (p.y() - bbox.getMin().y()) / cellSize.y();
int x = floor(xA);
int y = floor(yA);
if (y < 0 || (int)size() <= y || x < 0 || (int)(*this)[y].size() <= x)
THROWS("Point " << p << " not in grid (" << x << ", " << y << ')'
<< " (" << (*this)[y].size() << ", " << size() << ')');
if (y == yA && y == (int)size() - 1) y--;
if (x == xA && x == (int)(*this)[y].size() - 1) x--;
vector<ProbePoint *> points;
points.push_back(&(*this)[y][x]);
points.push_back(&(*this)[y][x + 1]);
points.push_back(&(*this)[y + 1][x]);
points.push_back(&(*this)[y + 1][x + 1]);
return points;
}
/**
* @Inherit
*/
inline virtual void draw(SimViewer::SimViewer& viewer,
const Vector2D& posRoot, scalar angleRoot,
const Vector2D& posLeaf, scalar angleLeaf,
scalar value)
{
// Joint::draw(viewer, posRoot, angleRoot,
// posLeaf, angleLeaf, value);
Vector2D pos_j = posRoot + Vector2D::rotate(
Joint::getPosRoot(), angleRoot);
viewer.drawJoint(pos_j.x(), pos_j.y(),
(Joint::getAngleRoot()+angleRoot)*180.0/M_PI,
(value)*180.0/M_PI);
Vector2D pos = posLeaf + Vector2D::rotate(
Joint::getPosLeaf(), angleLeaf);
viewer.drawSegmentByEnd(posRoot.x(),posRoot.y(), pos_j.x(),pos_j.y() ,0.05,sf::Color(0,200,0,100));
// viewer.drawJoint(pos.x(), pos.y(),
// (Joint::getAngleRoot()+angleRoot)*180.0/M_PI,
// (value)*180.0/M_PI);
double ori=(Joint::getAngleLeaf()+angleLeaf);
Vector2D rot;
//Draw the cam
Vector2D pos_r=pos;
Vector2D pos_l=pos;
double x=0.0;
double y=0.0;
Leph::Symbolic::Bounder bounder;
SymbolPtr xs = Symbol::create("x");
for(int i=0;i<100;i++)
{
x+=.3/100.0;
xs->reset();
bounder.setValue(xs,x);
// y=F(x);
y=_F(xs)->evaluate(bounder);
rot=Vector2D::rotate(Vector2D(x,y),ori+M_PI);
viewer.drawSegmentByEnd(pos_r.x(),pos_r.y(), pos.x()+rot.x(),pos.y()+rot.y() ,0.01,sf::Color(200,200,200,100));
pos_r=Vector2D(pos.x()+rot.x(), pos.y()+rot.y());
xs->reset();
bounder.setValue(xs,-x);
y=_F(xs)->evaluate(bounder);
// y=F(-x);
rot=Vector2D::rotate(Vector2D(-x,y),ori+M_PI);
viewer.drawSegmentByEnd(pos_l.x(),pos_l.y(), pos.x()+rot.x(),pos.y()+rot.y() ,0.01,sf::Color(200,200,200,100));
pos_l=Vector2D(pos.x()+rot.x(), pos.y()+rot.y());
}
viewer.drawSegment(pos.x(),pos.y(),_sH*2.0,(Joint::getAngleLeaf()+angleLeaf-M_PI/2.0)*180.0/M_PI,0.05,sf::Color(255,255,255,100));
viewer.drawSegment(pos_j.x(),pos_j.y(),_sH,((Joint::getAngleRoot()+angleRoot)+M_PI/2.0-_sphi)*180.0/M_PI,0.01,sf::Color(255,0,255,100));
viewer.drawCircle(pos_j.x()+_sH*cos((Joint::getAngleRoot()+angleRoot)+M_PI/2.0-_sphi),pos_j.y()+_sH*sin((Joint::getAngleRoot()+angleRoot)+M_PI/2.0-_sphi),0.03,sf::Color(255,0,255,100));
viewer.drawCircle(pos.x(),pos.y(),0.03,sf::Color(255,255,255,255));
//Draw spring
Vector2D pos1 = posRoot + Vector2D::rotate(
Joint::getPosRoot(), angleRoot);
Vector2D pos2 = posLeaf + Vector2D::rotate(
Joint::getPosLeaf(), angleLeaf);
double zval=Vector2D::dist(pos1,pos2);
scalar tmp=zval/10.0;
scalar sig=1.0;
Vector2D tmppos=pos1;
scalar angle=atan2(pos2.y()-pos1.y(),pos2.x()-pos1.x());
Vector2D tmpnew=tmppos+Vector2D::rotate(Vector2D(0.1*sig,tmp), angle+M_PI/2.0+M_PI);
viewer.drawSegmentByEnd(tmppos.x(),tmppos.y(), tmpnew.x(),tmpnew.y(),0.01,sf::Color(200,200,200,100));
tmppos=tmpnew;
sig*=-1.0;
tmp=zval/5.0;
for(int i=1;i<5;i++)
{
tmpnew=tmppos+Vector2D::rotate(Vector2D(0.2*sig,tmp), angle+M_PI/2.0+M_PI);
viewer.drawSegmentByEnd(tmppos.x(),tmppos.y(), tmpnew.x(),tmpnew.y(),0.01,sf::Color(200,200,200,100));
tmppos=tmpnew;
sig*=-1.0;
}
tmp=zval/10.0;
tmpnew=tmppos+Vector2D::rotate(Vector2D(0.1*sig,tmp), angle+M_PI/2.0+M_PI);
viewer.drawSegmentByEnd(tmppos.x(),tmppos.y(), tmpnew.x(),tmpnew.y(),0.01,sf::Color(200,200,200,100));
}
/**
* @Inherit
*/
inline virtual void draw(SimViewer::SimViewer& viewer,
const Vector2D& posRoot, scalar angleRoot,
const Vector2D& posLeaf, scalar angleLeaf,
scalar value)
{
Joint::draw(viewer, posRoot, angleRoot,
posLeaf, angleLeaf, value);
// Vector2D pos = posRoot + Vector2D::rotate(
// Joint::getPosRoot(), angleRoot);
Vector2D pos = posLeaf + Vector2D::rotate(
Joint::getPosLeaf(), angleLeaf);
viewer.drawJoint(pos.x(), pos.y(),
(Joint::getAngleRoot()+angleRoot)*180.0/M_PI,
(value)*180.0/M_PI);
pos = posRoot + Vector2D::rotate(
Joint::getPosRoot(), angleRoot);
double ori=(Joint::getAngleRoot()+angleRoot);
Vector2D rot;
//Draw the cam
Vector2D pos_r=pos;
Vector2D pos_l=pos;
double x=0.0;
double y=0.0;
Leph::Symbolic::Bounder bounder;
SymbolPtr xs = Symbol::create("x");
for(int i=0;i<100;i++)
{
x+=.3/100.0;
xs->reset();
bounder.setValue(xs,x);
// y=F(x);
y=_F(xs)->evaluate(bounder);
// std::cout<<"x: "<<x<<" "<<xs->evaluate(bounder)<<" y: "<<y<<std::endl;
rot=Vector2D::rotate(Vector2D(x,y),ori);
viewer.drawSegmentByEnd(pos_r.x(),pos_r.y(), pos.x()+rot.x(),pos.y()+rot.y() ,0.01,sf::Color(200,200,200,100));
pos_r=Vector2D(pos.x()+rot.x(), pos.y()+rot.y());
xs->reset();
bounder.setValue(xs,-x);
y=_F(xs)->evaluate(bounder);
// y=F(-x);
rot=Vector2D::rotate(Vector2D(-x,y),ori);
viewer.drawSegmentByEnd(pos_l.x(),pos_l.y(), pos.x()+rot.x(),pos.y()+rot.y() ,0.01,sf::Color(200,200,200,100));
pos_l=Vector2D(pos.x()+rot.x(), pos.y()+rot.y());
}
viewer.drawSegment(pos.x(),pos.y(),_sH*2.0,((Joint::getAngleRoot()+angleRoot)+M_PI/2.0)*180.0/M_PI,0.05,sf::Color(255,255,255,100));
//Draw the lever
viewer.drawSegment(posLeaf.x(),posLeaf.y(),_sH,(angleLeaf-M_PI/2.0)*180.0/M_PI,0.01,sf::Color(255,0,255,100));
viewer.drawCircle(posLeaf.x()+_sH*cos(angleLeaf-M_PI/2.0),posLeaf.y()+_sH*sin(angleLeaf-M_PI/2.0),0.03,sf::Color(255,0,255,100));
viewer.drawCircle(pos.x(),pos.y(),0.03,sf::Color(255,255,255,255));
}
Vector2D Camera::radianAngle( const Vector2D& angle )
{
return Vector2D( angle.x() * M_PI / 180.0, angle.y() * M_PI / 180.0 );
}
Vector3D::Vector3D(const Vector2D &vector, double zpos):xp(vector.x()), yp(vector.y()), zp(zpos){}
Vector3D::Vector3D(const Vector2D &vector):xp(vector.x()), yp(vector.y()), zp(0.0){}
void DiscoveryTask::process() {
// Check navigation module
if( core->navigation->name == "System Of Weights" && !running){
((SystemOfWeightsNavigation*)core->navigation)->removeAllWeights();
((SystemOfWeightsNavigation*)core->navigation)->addWeight(new FullSpeedWeight(core));
((SystemOfWeightsNavigation*)core->navigation)->addWeight(new OrientationWeight(core));
((SystemOfWeightsNavigation*)core->navigation)->addWeight(new CollisionAvoidanceWeight(core));
((SystemOfWeightsNavigation*)core->navigation)->addWeight(new AccelerationFilterWeight(core));
((SystemOfWeightsNavigation*)core->navigation)->addWeight(new ControllerSpeedWeight(core));
running = true;
}
else if (core->navigation->name == "Spline"){
// using SplineNavigation
}
else if (!running){
Debug::warning("[DiscoveryTask] Navigation module is not of type Spline!");
Task::status = Task::Finished;
return;
}
// Check if the whole map is explored
if (core->navigation->isAtLastNavPoint()) {
if (isExplored() == true) {
core->controller->setWheelSpeed(0, 0);
Task::status = Task::Finished;
Debug::print("[DiscoveryTask] DiscoveryTask finished");
return;
} else {
Debug::print("[DiscoveryTask]Add new TerrainCutPoints");
core->navigation->clearNavPoints();
addNewTerrainCutPoints();
tick = 0;
return;
}
}
// Look ahead if it is good to move there using a sweep across a start and end angle...
// double heat = 0.0;
// for (int i = 0; i < core->longSetting("Task_DiscoveryTask_Range_mm"); i += 10) {
// for (int angle = -(core->longSetting("Task_DiscoveryTask_SweepAngle") / 2); angle <= (core->longSetting("Task_DiscoveryTask_SweepAngle") / 2); angle++) {
// Trafo2D checkpoint = core->movementTracker->transformation * Trafo2D::rot(-Rad(angle)) * Trafo2D::trans(0, i);
// heat += core->heatMap->getChannelValue(HeatMap::CollisionAreaChannel, (long) ((checkpoint.trans().x()) / core->scale), (long) ((checkpoint.trans().y()) / core->scale));
// if (i < core->longSetting("Task_DiscoveryTask_CriticalAreaDistance_mm") && heat > 0) {
// collisionInCriticalArea = true;
// }
// }
// }
//
// // Safe to move?
// if (heat > 0.0 && lastProcessWasCollision == false) {
//
// for (int angle = -(core->longSetting("Task_DiscoveryTask_SweepAngle") / 4); angle <= (core->longSetting("Task_DiscoveryTask_SweepAngle") / 4); angle++) {
// Trafo2D checkpoint = core->movementTracker->transformation * Trafo2D::rot(-Rad(angle)) * Trafo2D::trans(0, 2000);
// core->heatMap->registerHeat(HeatMap::CollisionAreaChannel, (long) ((checkpoint.trans().x()) / core->scale), (long) ((checkpoint.trans().y()) / core->scale));
// }
//
// // is the collision in the critical area?
// if (collisionInCriticalArea == false) {
// //Debug::print("\t[Start] Collision");
//// if (core->navigation->currentNavPoint < core->navigation->getNavPoints()->count()-2){
//// core->navigation->currentNavPoint++;
//// //if(core->navigation->isAtLastNavPoint()) return;
//// }
//// else {
// core->navigation->clearNavPoints();
// addNewTerrainCutPoints();
// tick = 0;
// lastProcessWasCollision = true;
//// }
//
// //Debug::print("\t[Start] No Collision");
// } else {
// //Debug::print("\t[Start] Turn away from Collision");
// // turn away from the collision
// ((EmssController*) core->controller)->turn(90,(int) core->doubleSetting("Navigation_SplineNavigation_TargetSpeed_mmps"));
// core->navigation->clearNavPoints();
// addNewTerrainCutPoints();
// tick = 0;
// lastProcessWasCollision = true;
// collisionInCriticalArea = false;
// //Debug::print("\t[End] Turn away from Collision");
// }
// //Debug::print("\t[End] Collision in Area?");
// } else {
// lastProcessWasCollision = false;
// }
// Align the robot and then crawl the spline...
Vector2D wheelSpeed = core->navigation->getWheelSpeed(tick, interval);
core->controller->setWheelSpeed((short) wheelSpeed.x(), (short) wheelSpeed.y());
tick++;
}
