void MotionRequest::draw() const
{
DECLARE_DEBUG_DRAWING("representation:MotionRequest", "drawingOnField"); // drawing of a request walk vector
if(motion == walk)
{
switch(walkRequest.mode)
{
case WalkRequest::targetMode:
{
LINE("representation:MotionRequest", 0, 0, walkRequest.target.translation.x(), walkRequest.target.translation.y(), 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0));
CROSS("representation:MotionRequest", walkRequest.target.translation.x(), walkRequest.target.translation.y(), 50, 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0));
Vector2f rotation(500.f, 0.f);
rotation.rotate(walkRequest.target.rotation);
ARROW("representation:MotionRequest", walkRequest.target.translation.x(), walkRequest.target.translation.y(), walkRequest.target.translation.x() + rotation.x(), walkRequest.target.translation.y() + rotation.y(), 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0, 127));
break;
}
case WalkRequest::speedMode:
case WalkRequest::percentageSpeedMode:
{
Vector2f translation = walkRequest.mode == WalkRequest::speedMode ? walkRequest.speed.translation * 10.f : walkRequest.speed.translation * 1000.f;
ARROW("representation:MotionRequest", 0, 0, translation.x(), translation.y(), 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0));
if(walkRequest.target.rotation != 0.0f)
{
translation.x() = translation.norm();
translation.y() = 0;
translation.rotate(walkRequest.speed.rotation);
ARROW("representation:MotionRequest", 0, 0, translation.x(), translation.y(), 0, Drawings::solidPen, ColorRGBA(0xcd, 0, 0, 127));
}
break;
}
}
}
}
void TextEditComponent::onCursorChanged()
{
if(isMultiline())
{
Vector2f textSize = mFont->getWrappedTextCursorOffset(mText, getTextAreaSize().x(), mCursor);
if(mScrollOffset.y() + getTextAreaSize().y() < textSize.y() + mFont->getHeight()) //need to scroll down?
{
mScrollOffset[1] = textSize.y() - getTextAreaSize().y() + mFont->getHeight();
}else if(mScrollOffset.y() > textSize.y()) //need to scroll up?
{
mScrollOffset[1] = textSize.y();
}
}else{
Vector2f cursorPos = mFont->sizeText(mText.substr(0, mCursor));
if(mScrollOffset.x() + getTextAreaSize().x() < cursorPos.x())
{
mScrollOffset[0] = cursorPos.x() - getTextAreaSize().x();
}else if(mScrollOffset.x() > cursorPos.x())
{
mScrollOffset[0] = cursorPos.x();
}
}
}
void RRTWidget::drawTerminalState(QPainter &painter, const Vector2f &pos, const Vector2f &vel, const QColor &color) {
// draw point
painter.setPen(QPen(color, 6));
QPointF rootLoc(pos.x(), pos.y());
painter.drawEllipse(rootLoc, 2, 2);
Vector2f tipOffset = vel * VelocityDrawingMultiplier;
Vector2f tipLocVec = pos + tipOffset;
QPointF tipLoc(tipLocVec.x(), tipLocVec.y());
// draw arrow shaft
painter.setPen(QPen(color, 3));
painter.drawLine(rootLoc, tipLoc);
// draw arrow head
Vector2f headBase = tipLocVec - tipOffset.normalized()*4;
Vector2f perp = Vector2f(-tipOffset.y(), tipOffset.x()).normalized();
Vector2f tipLeftVec = headBase + perp*4;
Vector2f tipRightVec = headBase - perp*4;
QPointF trianglePts[] = {
tipLoc,
QPointF(tipLeftVec.x(), tipLeftVec.y()),
QPointF(tipRightVec.x(), tipRightVec.y())
};
painter.drawPolygon(trianglePts, 3);
}
/** Render a string of text at the specified starting poisition. The string encoding
* is assumed to be Latin-1 (ISO 8859-1).
*
* Note that this method will not draw anything on systems with OpenGL ES 2.0 (typically mobile
* devices.) For compatibility with all systems, use renderStringToBuffer instead.
*/
Vector2f
TextureFont::render(const string& text, const Vector2f& startPosition) const
{
Vector2f currentPosition = startPosition;
#ifndef VESTA_NO_IMMEDIATE_MODE_3D
glBegin(GL_QUADS);
for (unsigned int i = 0; i < text.length(); ++i)
{
// The cast to unsigned char is critical for glyph lookup to work correctly;
// otherwise, extended characters will generate negative indices.
const Glyph* glyph = lookupGlyph((unsigned char) text[i]);
if (glyph)
{
Vector2f p = currentPosition + glyph->offset;
glTexCoord2fv(glyph->textureCoords[0].data());
glVertex2f(p.x(), p.y());
glTexCoord2fv(glyph->textureCoords[1].data());
glVertex2f(p.x() + glyph->size.x(), p.y());
glTexCoord2fv(glyph->textureCoords[2].data());
glVertex2f(p.x() + glyph->size.x(), p.y() + glyph->size.y());
glTexCoord2fv(glyph->textureCoords[3].data());
glVertex2f(p.x(), p.y() + glyph->size.y());
currentPosition.x() += glyph->advance;
}
}
glEnd();
#endif
return currentPosition;
}
void MandelbrotProcessorCPU::process(Vector2f from, Vector2f to,
Image<unsigned short>& iterations) {
assert (MAX_ITERATIONS < std::numeric_limits<unsigned short>::max());
assert (iterations.cn == 1);
size_t width = iterations.width;
size_t height = iterations.height;
float x_step = (to.x() - from.x()) / width;
float y_step = (to.y() - from.y()) / height;
#pragma omp parallel for
for (size_t py = 0; py < height; py++) {
float y0 = from.y() + y_step * py;
for (size_t px = 0; px < width; px++) {
float x0 = from.x() + x_step * px;
unsigned short iteration;
float x = 0.0f;
float y = 0.0f;
for (iteration = 0; iteration < MAX_ITERATIONS; iteration++) {
float xn = x * x - y * y + x0;
y = 2 * x * y + y0;
x = xn;
if (x * x + y * y > INFINITY) {
break;
}
}
iterations(py, px) = iteration;
}
}
}
void TeamData::draw() const
{
DECLARE_DEBUG_DRAWING("representation:TeamData", "drawingOnField");
for(auto const& teammate : teammates)
{
ColorRGBA posCol;
if(teammate.status == Teammate::PLAYING)
posCol = ColorRGBA::green;
else if(teammate.status == Teammate::FALLEN)
posCol = ColorRGBA::yellow;
else
posCol = ColorRGBA::red;
const Vector2f& rPos = teammate.theRobotPose.translation;
const float radius = std::max(50.f, teammate.theRobotPose.deviation);
Vector2f dirPos = teammate.theRobotPose * Vector2f(radius, 0.f);
// Circle around Player
CIRCLE("representation:TeamData", rPos.x(), rPos.y(), radius, 20, Drawings::solidPen,
posCol, Drawings::noBrush, ColorRGBA::white);
// Direction of the Robot
LINE("representation:TeamData", rPos.x(), rPos.y(), dirPos.x(), dirPos.y(), 20,
Drawings::solidPen, posCol);
// Player number
DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y(), 100, ColorRGBA::black, teammate.number);
// Ball position
const Vector2f ballPos = teammate.theRobotPose * teammate.theBallModel.estimate.position;
CIRCLE("representation:TeamData", ballPos.x(), ballPos.y(), 50, 20, Drawings::solidPen, ColorRGBA::yellow, Drawings::solidBrush, ColorRGBA::yellow);
}
}
Vector4f::Vector4f( const Vector2f& xy, const Vector2f& zw )
{
m_elements[0] = xy.x();
m_elements[1] = xy.y();
m_elements[2] = zw.x();
m_elements[3] = zw.y();
}
bool GridStateSpace::stateValid(const Vector2f &pt) const {
return pt.x() >= 0
&& pt.y() >= 0
&& pt.x() < width()
&& pt.y() < height()
&& !obstacleAt(gridSquareForState(pt));
}
/**
* @brief Draw all the nodes generated in the RRT algorithm.
* @param painter
*/
void RenderArea::drawNodes(QPainter &painter)
{
painter.save();
painter.setRenderHint(QPainter::Antialiasing);
painter.setPen(Qt::black);
painter.setBrush(QBrush(Qt::black));
Vector2f pos;
for(int i = 0; i < (int)rrt->nodes.size(); i++) {
for(int j = 0; j < (int)rrt->nodes[i]->children.size(); j++) {
pos = rrt->nodes[i]->children[j]->position;
painter.drawEllipse(pos.x()-1.5, pos.y()-1.5, 3, 3);
}
pos = rrt->nodes[i]->position;
painter.drawEllipse(pos.x() - NODE_RADIUS, pos.y() - NODE_RADIUS, 2 * NODE_RADIUS, 2 * NODE_RADIUS);
}
painter.setPen(Qt::red);
painter.setBrush(QBrush(Qt::red));
// if a path exists, draw it.
for(int i = 0; i < (int)rrt->path.size() - 1; i++) {
QPointF p1(rrt->path[i]->position.x(), rrt->path[i]->position.y());
QPointF p2(rrt->path[i+1]->position.x(), rrt->path[i+1]->position.y());
painter.drawLine(p1, p2);
}
painter.restore();
}
// static
Vector3f Vector2f::cross( const Vector2f& v0, const Vector2f& v1 ) {
return Vector3f
(
0,
0,
v0.x() * v1.y() - v0.y() * v1.x()
);
}
void test_autodiff_scalar()
{
Vector2f p = Vector2f::Random();
typedef AutoDiffScalar<Vector2f> AD;
AD ax(p.x(),Vector2f::UnitX());
AD ay(p.y(),Vector2f::UnitY());
AD res = foo<AD>(ax,ay);
VERIFY_IS_APPROX(res.value(), foo(p.x(),p.y()));
}
bool BoxFilterFilm::AddSample(const rose::Spectrum &s, const Vector2ui &pixel, const Vector2f &sample) {
// Check we are inside pixel boundaries
if (sample.x() < 0.f || sample.x() > 1.f || sample.y() < 0.f || sample.y() > 1.f) { return false; }
// Add sample
raster[pixel.y() * width + pixel.x()] += s;
// Increase number of samples of the pixel
num_samples[pixel.y() * width + pixel.x()]++;
return true;
}
SwingFoot::SwingFoot(bool isLeft,
const Vector2f& p,
float footHeight,
AngDeg ang,
AngDeg rotateFoot,
float bodyHeight,
float duration,
Task* primary)
:LowerLimbsMotion(duration, primary),
mIsLeft(isLeft)
{
if ( mIsLeft ){
// left foot
mDesiredFootL.rotationZ(ang);
mDesiredFootL.p() = Vector3f(p.x(),p.y(),footHeight);
// right foot
mDesiredFootR.identity();
}
else{
// right foot
mDesiredFootR.rotationZ(ang);
mDesiredFootR.p() = Vector3f(p.x(),p.y(),footHeight);
// left foot
mDesiredFootL.identity();
}
mDesiredFootL.p().x() -= HUMANOID.getHalfFeetWidth();
mDesiredFootR.p().x() += HUMANOID.getHalfFeetWidth();
mDesiredFootL.p().z() += HUMANOID.getMinFootHeight();
mDesiredFootR.p().z() += HUMANOID.getMinFootHeight();
// body
AngDeg dirR = mDesiredFootR.rotatedAngZ();
AngDeg dirL = mDesiredFootL.rotatedAngZ();
if ( dirR > dirL ){
std::swap(dirR, dirL);
}
AngDeg dirBody = calBisectorTwoAngles(dirR,dirL);
mDesiredBody.rotationX(-10);
mDesiredBody.rotateLocalZ( dirBody );
mDesiredBody.pos() = (mDesiredFootL.p()+mDesiredFootR.p())*0.5f;
// mDesiredBody.pos().x() = 0;
mDesiredBody.pos().z() = bodyHeight;
//dynamic step
//mDesiredBody.p().y()-=addStepY;
if ( p.y()>0 ){
if(mIsLeft){
mDesiredFootL.rotateLocalX(rotateFoot);
}
else{
mDesiredFootR.rotateLocalX(rotateFoot);
}
}
}
void TeamPlayersLocator::update(TeamPlayersModel& teamPlayersModel)
{
teamPlayersModel.obstacles.clear();
std::vector<Obstacle> ownTeam;
teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOpponentGoalPost, theFieldDimensions.yPosLeftGoal), GOALPOST);
teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOpponentGoalPost, theFieldDimensions.yPosRightGoal), GOALPOST);
teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOwnGoalPost, theFieldDimensions.yPosLeftGoal), GOALPOST);
teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOwnGoalPost, theFieldDimensions.yPosRightGoal), GOALPOST);
if(theRobotInfo.penalty == PENALTY_NONE && theGroundContactState.contact)
{
ownTeam.emplace_back(theRobotPose.covariance.topLeftCorner(2, 2), theRobotPose.translation); // pose and covariance of the robot itself
if(theFallDownState.state == theFallDownState.upright) //why is this?
for(const auto& obstacle : theObstacleModel.obstacles)
{
if(obstacle.type == GOALPOST)
continue;
// if seen robots are of the opponent team and are not detected outside the field
const Vector2f p = theRobotPose * obstacle.center;
if(std::abs(p.x()) <= theFieldDimensions.xPosOpponentFieldBorder && std::abs(p.y()) <= theFieldDimensions.yPosLeftFieldBorder)
teamPlayersModel.obstacles.emplace_back(obstacle.covariance, p, obstacle.type);
}
}
for(auto const& teammate : theTeammateData.teammates)
{
if(teammate.status == Teammate::FULLY_ACTIVE)
{
if(teammate.pose.deviation < 50.f) //todo, magic number will be replaced by parameter. 50.f might be too low
ownTeam.emplace_back(teammate.pose.covariance.topLeftCorner(2, 2), teammate.pose.translation); //position of teammates
for(const auto& obstacle : teammate.obstacleModel.obstacles)
{
if(obstacle.type == GOALPOST)
continue;
// if seen robots are of the opponent team and are not detected outside the field
const Vector2f p = teammate.pose * obstacle.center;
if(std::abs(p.x()) <= theFieldDimensions.xPosOpponentFieldBorder && std::abs(p.y()) <= theFieldDimensions.yPosLeftFieldBorder)
{
Matrix2f covariance = (Matrix2f() << obstacle.covXX, obstacle.covXY, obstacle.covXY, obstacle.covYY).finished();
Obstacle converted = Obstacle(rotateCovariance(covariance, teammate.pose.rotation), p, obstacle.type);
merge(converted, teamPlayersModel.obstacles);
}
}
}
}
for(auto& teammate : ownTeam)
{
removeAround(teammate, teamPlayersModel.obstacles);
}
}
void DebugDrawing::arrow(Vector2f start, Vector2f end,
Drawings::PenStyle penStyle, int width, ColorRGBA color)
{
Vector2f startToEnd((end.x() - start.x()) / 4, (end.y() - start.y()) / 4);
Vector2f perpendicular(startToEnd.y(), -1 * startToEnd.x());
// start to endpoint
line((int)start.x(), (int)start.y(), (int)(end.x()), (int)(end.y()), penStyle, width, color);
// endpoint to left and right
line((int)(end.x()), (int)(end.y()), (int)(end.x() - startToEnd.x() + perpendicular.x()), (int)(end.y() - startToEnd.y() + perpendicular.y()), penStyle, width, color);
line((int)(end.x()), (int)(end.y()), (int)(end.x() - startToEnd.x() - perpendicular.x()), (int)(end.y() - startToEnd.y() - perpendicular.y()), penStyle, width, color);
}
bool BoxFilterFilmBlock::AddSample(const Spectrum &s, const Vector2ui &global_pixel, const Vector2f &sample) {
// Check we are inside pixel boundaries
if (sample.x() < 0.f || sample.x() > 1.f || sample.y() < 0.f || sample.y() > 1.f) { return false; }
// Compute local pixel coordinates
Vector2ui local_pixel = global_pixel - Vector2ui(width_offset, height_offset);
// Add sample
raster[local_pixel.y() * width + local_pixel.x()] += s;
// Increase number of samples of the pixel
num_samples[local_pixel.y() * width + local_pixel.x()]++;
return true;
}
bool Console::scrollEvent(const Vector2i &/* p */, const Vector2f &rel) {
float scrollAmount = rel.y() * (mSize.y() / 20.0f);
scrollAmount = rel.y()*5;
//float scrollh = height() *
// std::min(1.0f, height() / (float)mChildPreferredHeight);
//mScroll = std::max((float) 0.0f, std::min((float) 1.0f,
// mScroll - scrollAmount / (float)(mSize.y() - 8 - scrollh)));
//std::cout << "Scroll amount = " << scrollAmount << std::endl;
mScroll = scrollAmount;
return true;
}
void GlobalFieldCoverageProvider::setCoverageAtFieldPosition(GlobalFieldCoverage& globalFieldCoverage, const Vector2f& positionOnField, const int coverage) const
{
if(theFieldDimensions.isInsideField(positionOnField))
{
ASSERT(std::isfinite(positionOnField.x()));
ASSERT(std::isfinite(positionOnField.y()));
const int x = static_cast<int>((positionOnField.x() - theFieldDimensions.xPosOwnGroundline) / cellLengthX);
const int y = static_cast<int>((positionOnField.y() - theFieldDimensions.yPosRightSideline) / cellLengthY);
globalFieldCoverage.grid[y * numOfCellsX + x].timestamp = theFrameInfo.time;
globalFieldCoverage.grid[y * numOfCellsX + x].coverage = coverage;
}
}
void TeamData::draw() const
{
DECLARE_DEBUG_DRAWING("representation:TeamData", "drawingOnField");
for(auto const& teammate : teammates)
{
ColorRGBA posCol;
if(teammate.status == Teammate::PLAYING)
posCol = ColorRGBA::green;
else if(teammate.status == Teammate::FALLEN)
posCol = ColorRGBA::yellow;
else
posCol = ColorRGBA::red;
const Vector2f& rPos = teammate.theRobotPose.translation;
const float radius = std::max(50.f, teammate.theRobotPose.deviation);
Vector2f dirPos = teammate.theRobotPose * Vector2f(radius, 0.f);
// Circle around Player
CIRCLE("representation:TeamData", rPos.x(), rPos.y(), radius, 20, Drawings::solidPen,
posCol, Drawings::noBrush, ColorRGBA::white);
// Direction of the Robot
LINE("representation:TeamData", rPos.x(), rPos.y(), dirPos.x(), dirPos.y(), 20,
Drawings::solidPen, posCol);
// Player number
DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y(), 100, ColorRGBA::black, teammate.number);
// Role
DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y() - 150, 100,
ColorRGBA::black, Role::getName(teammate.theBehaviorStatus.role));
// Time to reach ball
int ttrb = teammate.theBehaviorStatus.role == Role::striker
? static_cast<int>(teammate.theBehaviorStatus.timeToReachBall.timeWhenReachBallStriker)
: static_cast<int>(teammate.theBehaviorStatus.timeToReachBall.timeWhenReachBall);
if(Blackboard::getInstance().exists("FrameInfo"))
{
const FrameInfo& theFrameInfo = (const FrameInfo&) Blackboard::getInstance()["FrameInfo"];
ttrb = theFrameInfo.getTimeSince(ttrb);
}
DRAWTEXT("representation:TeamData", rPos.x() + 100, rPos.y() - 300, 100, ColorRGBA::black, "TTRB: " << ttrb);
//Line from Robot to WalkTarget
LINE("representation:TeamData", rPos.x(), rPos.y(),
teammate.theSPLStandardBehaviorStatus.walkingTo.x(),
teammate.theSPLStandardBehaviorStatus.walkingTo.y(),
10, Drawings::dashedPen, ColorRGBA::magenta);
// Ball position
const Vector2f ballPos = teammate.theRobotPose * teammate.theBallModel.estimate.position;
CIRCLE("representation:TeamData", ballPos.x(), ballPos.y(), 50, 20, Drawings::solidPen, ColorRGBA::yellow, Drawings::solidBrush, ColorRGBA::yellow);
}
}
inline Vector2f VectorLocalization2D::observationFunction(line2f l, Vector2f p) const
{
static const bool debug = false;
Vector2f attraction(0.0,0.0), dir(V2COMP(l.Dir())), p0(V2COMP(l.P0())), p1(V2COMP(l.P1()));
float location = (p-p0).dot(dir);
attraction = (p-p0) - dir*location ;
if(debug){
debugLines.push_back( line2f( vector2f(p.x(),p.y()) ,vector2f((p-attraction).x(),(p-attraction).y()) ) );
const float crossSize = 0.002;
debugLines.push_back( line2f( vector2f(p.x()+crossSize,p.y()) , vector2f(p.x()-crossSize,p.y())) );
debugLines.push_back( line2f( vector2f(p.x(),p.y()+crossSize) , vector2f(p.x(),p.y()-crossSize)) );
}
return attraction;
}
void UKFPose2D::landmarkSensorUpdate(const Vector2f& landmarkPosition, const Vector2f& reading, const Matrix2f& readingCov)
{
generateSigmaPoints();
// computeLandmarkReadings
Vector2f landmarkReadings[7];
for(int i = 0; i < 7; ++i)
{
Pose2f pose(sigmaPoints[i].z(), sigmaPoints[i].head<2>());
Vector2f landmarkPosRel = pose.invert() * landmarkPosition; // TODO: optimize this
landmarkReadings[i] = Vector2f(landmarkPosRel.x(), landmarkPosRel.y());
}
// computeMeanOfLandmarkReadings
Vector2f landmarkReadingMean = landmarkReadings[0];
for(int i = 1; i < 7; ++i)
landmarkReadingMean += landmarkReadings[i];
landmarkReadingMean *= 1.f / 7.f;
// computeCovOfLandmarkReadingsAndSigmaPoints
Matrix2x3f landmarkReadingAndMeanCov = Matrix2x3f::Zero();
for(int i = 0; i < 3; ++i)
{
const Vector2f d1 = landmarkReadings[i * 2 + 1] - landmarkReadingMean;
landmarkReadingAndMeanCov += (Matrix2x3f() << d1 * l(0, i), d1 * l(1, i), d1 * l(2, i)).finished();
const Vector2f d2 = landmarkReadings[i * 2 + 2] - landmarkReadingMean;
landmarkReadingAndMeanCov += (Matrix2x3f() << d2 * -l(0, i), d2 * -l(1, i), d2 * -l(2, i)).finished();
}
landmarkReadingAndMeanCov *= 0.5f;
// computeCovOfLandmarkReadingsReadings
const Vector2f d = landmarkReadings[0] - landmarkReadingMean;
Matrix2f landmarkReadingCov = Matrix2f::Zero();
landmarkReadingCov << d * d.x(), d * d.y();
for(int i = 1; i < 7; ++i)
{
const Vector2f d = landmarkReadings[i] - landmarkReadingMean;
landmarkReadingCov += (Matrix2f() << d * d.x(), d * d.y()).finished();
}
landmarkReadingCov *= 0.5f;
const Matrix3x2f kalmanGain = landmarkReadingAndMeanCov.transpose() * (landmarkReadingCov + readingCov).inverse();
Vector2f innovation = reading - landmarkReadingMean;
const Vector3f correction = kalmanGain * innovation;
mean += correction;
mean.z() = Angle::normalize(mean.z());
cov -= kalmanGain * landmarkReadingAndMeanCov;
Covariance::fixCovariance(cov);
}
void SideConfidenceProvider::findAgreemates()
{
// If I have not seen the ball recently, no matches can be found anyway ...
if(!ballModelCanBeUsed(theBallModel, theWorldModelPrediction.robotPose))
return;
const Vector2f myGlobalBallPosition = theWorldModelPrediction.robotPose * theBallModel.estimate.position;
const Vector2f myGlobalBallPositionMirrored(-myGlobalBallPosition.x(), -myGlobalBallPosition.y());
// Check all teammates:
for(const auto& teammate : theTeamData.teammates)
{
// Robots that are not playing are of no interest
if(teammate.status != Teammate::PLAYING)
continue;
// Only trust B-Human robots
if(teammate.mateType != Teammate::TeamOrigin::BHumanRobot)
continue;
if(ballModelCanBeUsed(teammate.theBallModel, teammate.theRobotPose))
{
const Vector2f teammateGlobalBallPosition = teammate.theRobotPose * teammate.theBallModel.estimate.position;
const TeammateBallCompatibility bc = compareBallWithTeammateBall(myGlobalBallPosition, myGlobalBallPositionMirrored,
teammateGlobalBallPosition);
addToAgreemateList(teammate, bc);
}
}
}
Vector4f::Vector4f( float x, const Vector2f& yz, float w )
{
m_elements[0] = x;
m_elements[1] = yz.x();
m_elements[2] = yz.y();
m_elements[3] = w;
}
Vector4f::Vector4f( float x, float y, const Vector2f& zw )
{
m_elements[0] = x;
m_elements[1] = y;
m_elements[2] = zw.x();
m_elements[3] = zw.y();
}
Vector4f::Vector4f( const Vector2f& xy, float z, float w )
{
m_elements[0] = xy.x();
m_elements[1] = xy.y();
m_elements[2] = z;
m_elements[3] = w;
}
void ManualHeadMotionProvider::update(HeadMotionRequest& headMotionRequest)
{
bool parametersChanged = xImg != currentX || yImg != currentY;
if(parametersChanged && camera == theCameraInfo.camera)
{
currentX = xImg;
currentY = yImg;
Vector2f targetOnField;
if(Transformation::imageToRobot(currentX, currentY, theCameraMatrix, theCameraInfo, targetOnField))
{
headMotionRequest.target.x() = targetOnField.x();
headMotionRequest.target.y() = targetOnField.y();
headMotionRequest.target.z() = 0;
headMotionRequest.mode = HeadMotionRequest::targetOnGroundMode;
headMotionRequest.watchField = false;
//Use the camera that the user is seeing right now.
switch(camera)
{
case CameraInfo::lower:
headMotionRequest.cameraControlMode = HeadMotionRequest::lowerCamera;
break;
case CameraInfo::upper:
headMotionRequest.cameraControlMode = HeadMotionRequest::upperCamera;
break;
default:
ASSERT(false);
}
headMotionRequest.speed = 150_deg;
}
}
}
void Matrix2f::setCol( int j, const Vector2f& v )
{
int colStart = 2 * j;
m_elements[ colStart ] = v.x();
m_elements[ colStart + 1 ] = v.y();
}
inline Vector2f VectorLocalization2D::attractorFunction(line2f l, Vector2f p, float attractorRange, float margin)
{
static const bool debug = false;
Vector2f attraction(0.0,0.0), dir(V2COMP(l.Dir())), p0(V2COMP(l.P0())), p1(V2COMP(l.P1()));
float location = (p-p0).dot(dir);
if(location<-margin || location>l.Length()+margin){
return attraction;
}
attraction = (p-p0) - dir*location ;
float d = attraction.norm();
/*
if(d>0.5*attractorRange){
float d2 = max(0.0f, attractorRange - d);
attraction *= 2.0f*d2/attractorRange;
}
*/
if(d>attractorRange){
attraction = Vector2f::Zero();
}
if(debug){
debugLines.push_back( line2f( vector2f(p.x(),p.y()) ,vector2f((p-attraction).x(),(p-attraction).y()) ) );
}
return attraction;
}
int main()
{
Vector2fVector scan, reducedScan;
Vector2f a(1., 2.);
Vector2f b(1., 2.);
Vector2f c(2., 2.);
Vector2f d(1., 3.);
Vector2f e(2., 1);
Vector2f f(-1, 1);
scan.push_back(a);
scan.push_back(b);
scan.push_back(c);
scan.push_back(d);
scan.push_back(e);
scan.push_back(f);
float resolution = 0.03;
float kernelRange = 0.5;
int hV = 10;
float radius = 50;
ScanMatcher sm(resolution, radius, kernelRange/resolution, kernelRange);
sm.subsample(reducedScan, scan);
cout << "Scan size is: " << scan.size() << ", reducedScan size is: " << reducedScan.size() << endl;
for(Vector2fVector::const_iterator it = reducedScan.begin(); it != reducedScan.end(); ++it)
{
const Vector2f p = *it;
cout << "current point is: " << p.x() << ", " << p.y() << endl;
}
}
Vector3f Vector2f::cross( const Vector2f& v0 ) {
return Vector3f
(
0,
0,
m_elements[0] * v0.y() - m_elements[1] * v0.x()
);
}
