Glyph::Glyph(const glm::vec4& destRect, const glm::vec4& uvRect, GLuint Texture, float Depth, const ColorRGBA8& color, float angle) : texture(Texture), depth(Depth) { glm::vec2 halfDims(destRect.z / 2.0f, destRect.w / 2.0f); // Get points centered at origin glm::vec2 tl(-halfDims.x, halfDims.y); glm::vec2 bl(-halfDims.x, -halfDims.y); glm::vec2 br(halfDims.x, -halfDims.y); glm::vec2 tr(halfDims.x, halfDims.y); // Rotate the points tl = rotatePoint(tl, angle) + halfDims; bl = rotatePoint(bl, angle) + halfDims; br = rotatePoint(br, angle) + halfDims; tr = rotatePoint(tr, angle) + halfDims; topLeft.color = color; topLeft.setPosition(destRect.x + tl.x, destRect.y + tl.y); topLeft.setUV(uvRect.x, uvRect.y + uvRect.w); bottomLeft.color = color; bottomLeft.setPosition(destRect.x + bl.x, destRect.y + bl.y); bottomLeft.setUV(uvRect.x, uvRect.y); bottomRight.color = color; bottomRight.setPosition(destRect.x + br.x, destRect.y + br.y); bottomRight.setUV(uvRect.x + uvRect.z, uvRect.y); topRight.color = color; topRight.setPosition(destRect.x + tr.x, destRect.y + tr.y); topRight.setUV(uvRect.x + uvRect.z, uvRect.y + uvRect.w); }
/*** Draw a rectangle outline on the current screen. @function linedRectangle @tparam integer x rectangle origin horizontal coordinate, in pixels @tparam integer y rectangle origin vertical coordinate, in pixels @tparam integer width rectangle width, in pixels @tparam integer height rectangle height, in pixels @tparam[opt=1] integer lineWidth line's thickness, in pixels @tparam[opt=0] number angle rectangle rotation, in radians @tparam[opt=default color] integer color drawing color */ static int gfx_linedRectangle(lua_State *L) { int x = luaL_checkinteger(L, 1); int y = luaL_checkinteger(L, 2); int width = luaL_checkinteger(L, 3); int height = luaL_checkinteger(L, 4); float lineWidth = luaL_optnumber(L, 5, 1.0f); float angle = luaL_optnumber(L, 6, 0); u32 color = luaL_optinteger(L, 7, color_default); // Corner coordinates int x2 = x + width, y2 = y; int x3 = x2, y3 = y + height; int x4 = x, y4 = y3; // Rotate corners if (angle != 0) { int cx = x + width/2, cy = y + height/2; rotatePoint(x, y, cx, cy, angle, &x, &y ); rotatePoint(x2, y2, cx, cy, angle, &x2, &y2); rotatePoint(x3, y3, cx, cy, angle, &x3, &y3); rotatePoint(x4, y4, cx, cy, angle, &x4, &y4); } // Draw lines sf2d_draw_line(x, y, x2, y2, lineWidth, color); sf2d_draw_line(x2, y2, x3, y3, lineWidth, color); sf2d_draw_line(x3, y3, x4, y4, lineWidth, color); sf2d_draw_line(x4, y4, x, y, lineWidth, color); return 0; }
Glyph::Glyph(const glm::vec4 &destRect, const glm::vec4 &uvRect, const GLuint &Texture, const float &Depth, const ColorRGBA8 &color, const float &angle) : texture(Texture), depth(Depth) { const glm::vec2 halfDims(destRect.z / 2.0f, destRect.w / 2.0f); glm::vec2 tl(-halfDims.x, halfDims.y); glm::vec2 bl(-halfDims.x, -halfDims.y); glm::vec2 br(halfDims.x, -halfDims.y); glm::vec2 tr(halfDims.x, halfDims.y); // rotate the points tl = rotatePoint(tl, angle) + halfDims; bl = rotatePoint(bl, angle) + halfDims; br = rotatePoint(br, angle) + halfDims; tr = rotatePoint(tr, angle) + halfDims; topLeft.setColor(color.r, color.g, color.b, color.a); topLeft.setPosition(destRect.x + tl.x, destRect.y + tl.y); topLeft.setUV(uvRect.x, uvRect.y + uvRect.w); topLeft.setColor(color.r, color.g, color.b, color.a); bottomLeft.setPosition(destRect.x + bl.x, destRect.y + bl.y); bottomLeft.setUV(uvRect.x, uvRect.y); topLeft.setColor(color.r, color.g, color.b, color.a); bottomRight.setPosition(destRect.x + br.x, destRect.y + br.y); bottomRight.setUV(uvRect.x + uvRect.z, uvRect.y); topLeft.setColor(color.r, color.g, color.b, color.a); topRight.setPosition(destRect.x + tr.x, destRect.y + tr.y); topRight.setUV(uvRect.x + uvRect.z, uvRect.y + uvRect.w); }
Glyph::Glyph(OShader *shader, const maths::vec2 &position, const maths::vec2 &dimensions, const maths::vec4 &uvRect, GLuint texture, unsigned int color, float zOrder, float angle) :textureID(texture), a_zOrder(zOrder), currentShader(shader) { maths::vec2 halfDims(dimensions.x * 0.5f, dimensions.y * 0.5f); // Get points centered at origin maths::vec2 tl(-halfDims.x, halfDims.y); maths::vec2 bl(-halfDims.x, -halfDims.y); maths::vec2 br(halfDims.x, -halfDims.y); maths::vec2 tr(halfDims.x, halfDims.y); // Rotate the points tl = rotatePoint(tl, angle) + halfDims; bl = rotatePoint(bl, angle) + halfDims; br = rotatePoint(br, angle) + halfDims; tr = rotatePoint(tr, angle) + halfDims; topLeft.setColor(color); topLeft.setPosition(position.x + tl.x, position.y + tl.y); topLeft.setUV(uvRect.x, uvRect.y + uvRect.w); bottomLeft.setColor(color); bottomLeft.setPosition(position.x + bl.x, position.y + bl.y); bottomLeft.setUV(uvRect.x, uvRect.y); bottomRight.setColor(color); bottomRight.setPosition(position.x + br.x, position.y + br.y); bottomRight.setUV(uvRect.x + uvRect.z, uvRect.y); topRight.setColor(color); topRight.setPosition(position.x + tr.x, position.y + tr.y); topRight.setUV(uvRect.x + uvRect.z, uvRect.y + uvRect.w); }
int BoxIntoBox(camBox* a, camBox* b){//check if a has points in b glm::vec3 aCenter = rotatePoint(glm::vec3(a->x,a->y,a->z),a->amt,a->ax);//rotate to a-box space glm::vec3 testPoint = aCenter; int ret = 0;//box starts with assumed no collision testPoint.x+=a->w;//add width testPoint.y+=a->h;//height testPoint.z+=a->d;//depth testPoint = rotatePoint(glm::vec3(a->x,a->y,a->z),-a->amt,a->ax);//return to world space +++ ret |= pointBox(testPoint,b);//test against b OR in the result, if any return true then box corner is colliding testPoint = aCenter; testPoint.x+=a->w;//add width testPoint.y+=a->h;//height testPoint.z-=a->d;//depth testPoint = rotatePoint(glm::vec3(a->x,a->y,a->z),-a->amt,a->ax);//return to world space ++- ret |= pointBox(testPoint,b);//test against b testPoint = aCenter; testPoint.x+=a->w;//add width testPoint.y-=a->h;//height testPoint.z+=a->d;//depth testPoint = rotatePoint(glm::vec3(a->x,a->y,a->z),-a->amt,a->ax);//return to world space +-+ ret |= pointBox(testPoint,b);//test against b testPoint = aCenter; testPoint.x+=a->w;//add width testPoint.y-=a->h;//height testPoint.z-=a->d;//depth testPoint = rotatePoint(glm::vec3(a->x,a->y,a->z),-a->amt,a->ax);//return to world space +-- ret |= pointBox(testPoint,b);//test against b testPoint = aCenter; testPoint.x-=a->w;//add width testPoint.y+=a->h;//height testPoint.z+=a->d;//depth testPoint = rotatePoint(glm::vec3(a->x,a->y,a->z),-a->amt,a->ax);//return to world space -++ ret |= pointBox(testPoint,b);//test against b testPoint = aCenter; testPoint.x-=a->w;//add width testPoint.y+=a->h;//height testPoint.z-=a->d;//depth testPoint = rotatePoint(glm::vec3(a->x,a->y,a->z),-a->amt,a->ax);//return to world space -+- ret |= pointBox(testPoint,b);//test against b testPoint = aCenter; testPoint.x-=a->w;//add width testPoint.y-=a->h;//height testPoint.z+=a->d;//depth testPoint = rotatePoint(glm::vec3(a->x,a->y,a->z),-a->amt,a->ax);//return to world space --+ ret |= pointBox(testPoint,b);//test against b testPoint = aCenter; testPoint.x-=a->w;//add width testPoint.y-=a->h;//height testPoint.z-=a->d;//depth testPoint = rotatePoint(glm::vec3(a->x,a->y,a->z),-a->amt,a->ax);//return to world space --- ret |= pointBox(testPoint,b);//test against b }
//generate the locus of pure pursuit static std::vector<geometry_msgs::Point> generateTrajectoryCircle(geometry_msgs::Point target) { std::vector<geometry_msgs::Point> traj_circle_array; double radius = calcRadius(target); for (double i = 0; i < M_PI / 2; i += 0.1) { //calc a point of circumference geometry_msgs::Point p; p.x = radius * cos(i); p.y = radius * sin(i); //transform to (radius,0) geometry_msgs::Point relative_p; relative_p.x = p.x - radius; relative_p.y = p.y; //rotate -90° geometry_msgs::Point rotate_p = rotatePoint(relative_p,-90); //transform to vehicle plane geometry_msgs::Point tf_p = calcAbsoluteCoordinate(rotate_p, _current_pose.pose); traj_circle_array.push_back(tf_p); } //reverse vector std::reverse(traj_circle_array.begin(), traj_circle_array.end()); for (double i = 0; i > (-1) * M_PI / 2; i -= 0.1) { //calc a point of circumference geometry_msgs::Point p; p.x = radius * cos(i); p.y = radius * sin(i); //transform to (radius,0) geometry_msgs::Point relative_p; relative_p.x = p.x - radius; relative_p.y = p.y; //rotate -90° geometry_msgs::Point rotate_p = rotatePoint(relative_p,-90); //transform to vehicle plane geometry_msgs::Point tf_p = calcAbsoluteCoordinate(rotate_p, _current_pose.pose); traj_circle_array.push_back(tf_p); } return traj_circle_array; }
void GeoParticle::setTriangles() { for (int i = 0; i < m_numTriangles; ++i) { m_triangles[i].verts[0] = rotatePoint(m_originalVerts[0], m_triangles[i].center, m_triangles[i].angle) + ofNoise(m_noiseOffsets[0]) * 0.2f; m_triangles[i].verts[1] = rotatePoint(m_originalVerts[i + 1], m_triangles[i].center, m_triangles[i].angle) + ofNoise(m_noiseOffsets[i + 1]) * 0.2f; if (i == m_numTriangles - 1) m_triangles[i].verts[2] = rotatePoint(m_originalVerts[1], m_triangles[i].center, m_triangles[i].angle) + ofNoise(m_noiseOffsets[1]) * 0.2f; else m_triangles[i].verts[2] = rotatePoint(m_originalVerts[i + 2], m_triangles[i].center, m_triangles[i].angle) + ofNoise(m_noiseOffsets[i + 2]) * 0.2f; } }
void rotatePoly( tmatrix mo, short p) { // rotates (permanently) the 4 points of poligon of index p point pa, pb, pc, pd, pt; // check if not rotated already (as part of previous poly) if ( !flaglist[ pylist[p][0]]++) rotatePoint( mo, pylist[p][0]); if ( !flaglist[pylist[p][1]]++) rotatePoint( mo, pylist[p][1]); if ( !flaglist[pylist[p][2]]++) rotatePoint( mo, pylist[p][2]); if ( !flaglist[pylist[p][3]]++) rotatePoint( mo, pylist[p][3]); } // rotatePoly
IWORKPathPtr_t makeStarPath(const IWORKSize &size, const unsigned points, const double innerRadius) { // user space canvas: [-1:1] x [-1:1] // create outer points const deque<Point> outerPoints = rotatePoint(Point(0, -1), points); // create inner points const double angle = etonyek_two_pi / points; deque<Point> innerPoints(outerPoints); transform(innerPoints, scale(innerRadius, innerRadius) * rotate(angle / 2)); // merge them together deque<Point> pathPoints; assert(outerPoints.size() == innerPoints.size()); for (deque<Point>::const_iterator itO = outerPoints.begin(), itI = innerPoints.begin(); (itO != outerPoints.end()) && (itI != innerPoints.end()); ++itO, ++itI) { pathPoints.push_back(*itO); pathPoints.push_back(*itI); } // create the path transform(pathPoints, scale(size.m_width, size.m_height) * scale(0.5, 0.5) * translate(1, 1)); const IWORKPathPtr_t path = makePolyLine(pathPoints); return path; }
void rotatePoints(Point *ps, Point* pd, double angle, int np) { double sina = sin(angle); double cosa = cos(angle); for (int i=0; i<np; i++) rotatePoint(&ps[i], &pd[i], sina, cosa); }
void DebugRenderer::drawBox(const glm::vec4& destRect, const Color& color, float angle){ int i = _verts.size(); _verts.resize(_verts.size() + 4); glm::vec2 halfDims(destRect.z / 2.0f, destRect.w / 2.0f); //Get points centered at origin glm::vec2 TL(-halfDims.x, halfDims.y); glm::vec2 BL(-halfDims.x, -halfDims.y); glm::vec2 BR(halfDims.x, -halfDims.y); glm::vec2 TR(halfDims.x, halfDims.y); glm::vec2 positionOffset(destRect.x, destRect.y); //Rotate the points _verts[i].position = rotatePoint(TL, angle) + halfDims + positionOffset; _verts[i + 1].position = rotatePoint(BL, angle) + halfDims + positionOffset; _verts[i + 2].position = rotatePoint(BR, angle) + halfDims + positionOffset; _verts[i + 3].position = rotatePoint(TR, angle) + halfDims + positionOffset; for (int j = i; j < i + 4; j++){ _verts[j].color = color; } _indices.reserve(_indices.size() + 8); //Line 1 _indices.push_back(i); _indices.push_back(i + 1); //Line 2 _indices.push_back(i + 1); _indices.push_back(i + 2); //Line 3 _indices.push_back(i + 2); _indices.push_back(i + 3); //Line 4 _indices.push_back(i + 3); _indices.push_back(i); }
void displayFcn(){ glClear(GL_COLOR_BUFFER_BIT); glColor3f(0.0,0.0,0.0); Point ejeX1; ejeX1.x = 20; ejeX1.y = 300; Point ejeX2; ejeX2.x = 580; ejeX2.y = 300; Point ejeY1; ejeY1.x = 300; ejeY1.y = 20; Point ejeY2; ejeY2.x = 300; ejeY2.y = 580; drawLineXnpio(ejeX1, ejeX2, window); drawLineXnpio(ejeY1, ejeY2, window); glBegin(GL_POINTS); glVertex2f(origen.x, origen.y); glEnd(); Point A; A.x = 5; A.y = 6; Point B; B.x = 7; B.y = 8; Point C; C.x = 3; C.y = 7; Point A_2 = convertirPunto(A); Point B_2 = convertirPunto(B); Point C_2 = convertirPunto(C); changeColor(ROJO); drawTriangle(A_2, B_2, C_2, window); Point eje; eje.x = 2; eje.y = 2; A.x += eje.x; A.y += eje.y; B.x += eje.x; B.y += eje.y; C.x += eje.x; B.y += eje.y; A_2 = convertirPunto(A); B_2 = convertirPunto(B); C_2 = convertirPunto(C); changeColor(AZUL); drawTriangle(A_2, B_2, C_2, window); A = rotatePoint(A, eje, 90); B = rotatePoint(B, eje, 90); C = rotatePoint(C, eje, 90); A_2 = convertirPunto(A); B_2 = convertirPunto(B); C_2 = convertirPunto(C); changeColor(VIOLETA); drawTriangle(A_2, B_2, C_2, window); A.y = -A.y; B.y = -B.y; C.y = -C.y; A_2 = convertirPunto(A); B_2 = convertirPunto(B); C_2 = convertirPunto(C); changeColor(MAGENTA); drawTriangle(A_2, B_2, C_2, window); glFlush(); }
QPair<Vector2,Vector2> Cloth::rotateTriangle(float _w,int angle) { angle*=-1; float h = (sqrt(3)/2.0) * _w; float w = _w; if(h<0) w*=-1; Vector2 p1 = Vector2(0,(2.0/3.0)*h); Vector2 p2 = Vector2(-.5*w,-(1.0/3.0)*h); Vector2 p3 = Vector2(.5*w,-(1.0/3.0)*h); Vector2 v1 = rotatePoint(p1,angle); Vector2 v2 = rotatePoint(p2,angle); Vector2 v3 = rotatePoint(p3,angle); Vector2 width = v3-v2; Vector2 height = v1-((v2+v3)*.5f); return QPair<Vector2,Vector2>(width,height); }
// Rotate all points in a vector (more or less the same as rotating the image) // This is in Pixels since it uses the width and height of an image void Pixels::rotateVector(std::vector<Coord>& v, const Coord& point, double rad) const { const double sin_rad = std::sin(rad); const double cos_rad = std::cos(rad); for (Coord& m : v) { const Coord c = rotatePoint(point, m, sin_rad, cos_rad); if (c != default_coord) m = c; } }
bool getTriangleFromSegmentSize(double _szA, double _szB, double _szC, double x0, double y0, double _angle, double _scale, ofVec2f & _ptA, ofVec2f & _ptB, ofVec2f & _ptC) { double r0 = _szB * _scale; double x1 = 0 + _szA * _scale; double y1 = 0; double r1 = _szC * _scale; vector<ofVec2f> _points; circlesIntersection(0, 0, r0, x1, y1, r1, _points); if(_points.size() == 0) return false; // dont intersect --- sides dont make a triangle _ptA.set(0, 0); _ptB.set(x1, y1); _ptC.set(_points[1].x, _points[1].y); _ptC.x = _ptB.x + (_ptA.x - _ptC.x); _ptC.y = _ptB.y + (_ptA.y - _ptC.y); double _sin = sin(ofDegToRad(_angle)); double _cos = cos(ofDegToRad(_angle)); ofVec2f _pivot = (_ptA + _ptB + _ptC) / 3.0; _ptA -= _pivot; _ptB -= _pivot; _ptC -= _pivot; _ptA = rotatePoint(_ptA.x, _ptA.y, 0, 0, 0, 0, _sin, _cos); _ptB = rotatePoint(_ptB.x, _ptB.y, 0, 0, 0, 0, _sin, _cos); _ptC = rotatePoint(_ptC.x, _ptC.y, 0, 0, 0, 0, _sin, _cos); _ptA += ofVec2f(x0,y0); _ptB += ofVec2f(x0,y0); _ptC += ofVec2f(x0,y0); return true; }
IWORKPathPtr_t makePolygonPath(const IWORKSize &size, const unsigned edges) { // user space canvas: [-1:1] x [-1:1] deque<Point> points = rotatePoint(Point(0, -1), edges); // FIXME: the shape should probably be scaled to whole width/height. // Check. transform(points, scale(size.m_width, size.m_height) * scale(0.5, 0.5) * translate(1, 1)); const IWORKPathPtr_t path = makePolyLine(points); return path; }
void Adina::DebugRenderer::drawBox(const glm::vec4& destRect, const ColorRGBA8& color, float angle) { int i = m_verts.size(); m_verts.resize(m_verts.size() + 4); glm::vec2 halfDims(destRect.z / 2.0f, destRect.w / 2.0f); // Get points centered at origin glm::vec2 tl(-halfDims.x, halfDims.y); glm::vec2 bl(-halfDims.x, -halfDims.y); glm::vec2 br(halfDims.x, -halfDims.y); glm::vec2 tr(halfDims.x, halfDims.y); glm::vec2 positionOffset(destRect.x, destRect.y); // Rotate the points m_verts[i].position = rotatePoint(tl, angle) + halfDims + positionOffset; m_verts[i + 1].position = rotatePoint(bl, angle) + halfDims + positionOffset; m_verts[i + 2].position = rotatePoint(br, angle) + halfDims + positionOffset; m_verts[i + 3].position = rotatePoint(tr, angle) + halfDims + positionOffset; for (int j = i; j < i + 4; j++) { m_verts[j].color = color; } m_indices.reserve(m_indices.size() + 8); m_indices.push_back(i); m_indices.push_back(i + 1); m_indices.push_back(i + 1); m_indices.push_back(i + 2); m_indices.push_back(i + 2); m_indices.push_back(i + 3); m_indices.push_back(i + 3); m_indices.push_back(i); }
IWORKPathPtr_t makeRoundedRectanglePath(const IWORKSize &size, const double radius) { // user space canvas: [-1:1] x [-1:1] // TODO: draw rounded corners (void) radius; deque<Point> points = rotatePoint(Point(1, 1), 4); transform(points, scale(size.m_width, size.m_height) * scale(0.5, 0.5) * translate(1, 1)); const IWORKPathPtr_t path = makePolyLine(points); return path; }
void Hexagon::odbOutputLayerFeature( OdbFeatureFile& file, QString polarity, QPointF location, Xform *xform) { Contour surface; QPointF p(0, 0.5 * m_length.inch()); surface.polygon().setPolyBegin(p); for (int i = 0; i < 6; ++i) { p = rotatePoint(p, 60); surface.polygon().addSegment(p); } surface.odbOutputLayerFeature(file, polarity, location, xform); }
// In the future, it may be a good idea to implement something like the "Rotation by // Area Mapping" talked about on http://www.leptonica.com/rotation.html void Pixels::rotate(double rad, const Coord& point) { // Right size, default to white (255 or 1111 1111) std::vector<std::vector<unsigned char>> copy(h, std::vector<unsigned char>(w, 0xff)); // -rad because we're calculating the rotation to get from the new rotated // image to the original image. We're walking the new image instead of the // original so as to not get blank spots from rounding. const double sin_rad = std::sin(-rad); const double cos_rad = std::cos(-rad); for (int y = 0; y < h; ++y) { for (int x = 0; x < w; ++x) { const Coord c = rotatePoint(point, Coord(x,y), sin_rad, cos_rad); if (c != default_coord) copy[y][x] = p[c.y][c.x]; } } p = copy; // Rotate marks as well. This time we'll rotate to the new image, calculating the new // point instead of looking for what goes at every pixel in the new image. const double mark_sin_rad = std::sin(rad); const double mark_cos_rad = std::cos(rad); for (Mark& m : marks) { const Coord c = rotatePoint(point, m.coord, mark_sin_rad, mark_cos_rad); if (c != default_coord) m.coord = c; } }
void DebugRenderer::drawTriangle(const glm::vec4& destRect, const Color& color, float angle){ int i = _verts.size(); _verts.resize(_verts.size() + 3); glm::vec2 halfDims(destRect.z / 2.0f, destRect.w / 2.0f); glm::vec2 Top(0, halfDims.y); glm::vec2 BL(-halfDims.x, -halfDims.y); glm::vec2 BR(halfDims.x, -halfDims.y); glm::vec2 positionOffset(destRect.x, destRect.y); _verts[i].position = rotatePoint(Top, angle) + halfDims + positionOffset; _verts[i + 1].position = rotatePoint(BL, angle) + halfDims + positionOffset; _verts[i + 2].position = rotatePoint(BR, angle) + halfDims + positionOffset; for (int j = i; j < i + 3; j++){ _verts[j].color = color; } _indices.reserve(_indices.size() + 6); //First line _indices.push_back(i); _indices.push_back(i + 1); //Second line _indices.push_back(i + 1); _indices.push_back(i + 2); //Third line _indices.push_back(i + 2); _indices.push_back(i); }
void Transformation::displacePivotTo(const Point& newPivot) { // Calculate the rotated corners Corners corners; transformBox(corners); // Rotate-back (-angle) the position of the rotated origin (corners[0]) // using the new pivot. PointT<double> newBoundsOrigin = rotatePoint(corners.leftTop(), PointT<double>(newPivot.x, newPivot.y), -m_angle); // Change the new pivot. m_pivot = newPivot; m_bounds = Rect(Point(newBoundsOrigin), m_bounds.getSize()); }
std::vector<float>& SpaceOrientation:: getNaoPositionInRobotFrame() { #ifdef __linux__ try { AL::ALMotionProxy motion(robotIp); bool useSensors = true; std::vector<float> sensorsPosition = motion.getRobotPosition(useSensors); // std::cout << "sensors: " << sensorsPosition << std::endl; cv::Point2d position(sensorsPosition[1], sensorsPosition[0]); position = rotatePoint(position, naoPositionOnStartInSpace[2]); // if (! naoPreviousPositionInSpace.size()) // { // naoPreviousPositionInSpace.push_back(0); // naoPreviousPositionInSpace.push_back(0); // naoPreviousPositionInSpace.push_back(0); // } // std::cout << "Position after rotation: " << // position << std::endl; naoPositionInSpace[0] = position.y - naoPositionOnStartInSpace[0]; naoPositionInSpace[1] = position.x - naoPositionOnStartInSpace[1]; // - naoPositionOnStartInSpace[1]; naoPositionInSpace[2] = sensorsPosition[2] - naoPositionOnStartInSpace[2]; // cv::Point2d naoPoint(naoPositionInSpace[1], // naoPositionInSpace[0]); } catch (const AL::ALError& e) { std::cerr << "Caught exception: " << e.what() << std::endl; exit(EXIT_FAILURE); } #else std::cout << "getNaoPositionInRobotFrame() not on Linux." << std::endl; #endif return naoPositionInSpace; }
IWORKPathPtr_t makeCalloutPath(const IWORKSize &size, const double radius, const double tailSize, const double tailX, const double tailY) { // user space canvas: [-1:1] x [-1:1] // TODO: draw correctly instead of just approximating (void) radius; (void) tailSize; (void) tailX; (void) tailY; deque<Point> points = rotatePoint(Point(-1, -1), 4); points.push_back(Point(-1, 0.5)); points.push_back(Point(-2, 0)); points.push_back(Point(-1, -0.5)); // create the path transform(points, scale(size.m_width, size.m_height) * scale(0.5, 0.5) * translate(1, 1)); const IWORKPathPtr_t path = makePolyLine(points); return path; }
void SpaceOrientation:: initializePosition() { #ifdef __linux__ try { AL::ALMotionProxy motion(robotIp); bool useSensors = true; naoPositionOnStartInSpace = motion.getRobotPosition(useSensors); // std::cout << "sensors: " << naoPositionOnStartInSpace << std::endl; cv::Point2d startPosition(naoPositionOnStartInSpace[1], naoPositionOnStartInSpace[0]); startPosition = rotatePoint(startPosition, naoPositionOnStartInSpace[2]); // std::cout << "rotation: " << rotatePoint(cv::Point2d(1,1), -M_PI/2); naoPositionOnStartInSpace[0] = startPosition.y; naoPositionOnStartInSpace[1] = startPosition.x; // std::cout << "Start position: " << startPosition << std::endl; naoPositionInSpace.push_back(0); naoPositionInSpace.push_back(0); naoPositionInSpace.push_back(0); naoPreviousPositionInSpace = naoPositionInSpace; } catch (const AL::ALError& e) { std::cerr << "Caught exception: " << e.what() << std::endl; exit(EXIT_FAILURE); } #else std::cout << "initializePosition() not on Linux." << std::endl; naoPositionInSpace.push_back(0); naoPositionInSpace.push_back(0); naoPositionInSpace.push_back(0); naoPreviousPositionInSpace = naoPositionInSpace; naoPositionOnStartInSpace = naoPositionInSpace; #endif }
template<> RotBox<3>& RotBox<3>::rotateCenter(const Quaternion& q) { rotatePoint(q, getCenter()); return *this; }
template<> RotBox<3>& RotBox<3>::rotateCorner(const Quaternion& q, int corner) { rotatePoint(q, getCorner(corner)); return *this; }
//Set up the scale Gui void Collider::setUpScaleGui() { mUniformScalePoint = boundingRect().center(); QRectF tmpRect = QRectF(mUniformScalePoint-QPointF(5,5),mUniformScalePoint+QPointF(5,5)); mScaleUniformPoly.clear(); mScaleUniformPoly.append(rotatePoint(tmpRect.topLeft(), mRotation)); mScaleUniformPoly.append(rotatePoint(tmpRect.topRight(), mRotation)); mScaleUniformPoly.append(rotatePoint(tmpRect.bottomRight(), mRotation)); mScaleUniformPoly.append(rotatePoint(tmpRect.bottomLeft(), mRotation)); mXScalePoint = mUniformScalePoint; mXScalePoint.setX(mUniformScalePoint.x() + 50); tmpRect = QRectF(mXScalePoint-QPointF(5,5),mXScalePoint+QPointF(5,5)); mScaleXPoly.clear(); mScaleXPoly.append(rotatePoint(tmpRect.topLeft(), mRotation)); mScaleXPoly.append(rotatePoint(tmpRect.topRight(), mRotation)); mScaleXPoly.append(rotatePoint(tmpRect.bottomRight(), mRotation)); mScaleXPoly.append(rotatePoint(tmpRect.bottomLeft(), mRotation)); mXScalePoint = rotatePoint(mXScalePoint, mRotation); mYScalePoint = mUniformScalePoint; mYScalePoint.setY(mUniformScalePoint.y() - 50); tmpRect = QRectF(mYScalePoint-QPointF(5,5),mYScalePoint+QPointF(5,5)); mScaleYPoly.clear(); mScaleYPoly.append(rotatePoint(tmpRect.topLeft(), mRotation)); mScaleYPoly.append(rotatePoint(tmpRect.topRight(), mRotation)); mScaleYPoly.append(rotatePoint(tmpRect.bottomRight(), mRotation)); mScaleYPoly.append(rotatePoint(tmpRect.bottomLeft(), mRotation)); mYScalePoint = rotatePoint(mYScalePoint, mRotation); mUniformScalePoint = rotatePoint(mUniformScalePoint, mRotation); mScaleXAxis = QVector2D(mXScalePoint - mUniformScalePoint); mScaleYAxis = QVector2D(mYScalePoint - mUniformScalePoint); }
template<> Segment<3>& Segment<3>::rotateCenter(const Quaternion& q) { rotatePoint(q, getCenter()); return *this; }
void world_getPushBack(float boundingBox[3], vect3_t velocity, vect3_t pushback) { float triangle[3][3] = {{0,0,0},{0,0,0},{0,0,0}}, matrix[16], force; int i, j, k, collided = 0, collisions = 0; vect_t **triPtr = triangleList, *tri; vect3_t triangleNormal; // Reset the pushback value pushback[0] = 0; pushback[1] = 0; pushback[2] = 0; // Setup the rotation matrix setupRotationMatrix(matrix); // setFlipMatrix(matrix); for (i = 0; i < numTriangles; i++, triPtr++) { // Copy triangle into correct format // and translate by camera position tri = *triPtr; for (j = 0; j < 3; j++) for (k = 0; k < 3; k++) triangle[j][k] = tri[k + 3*j] - camera.position[k]; // Apply camera rotation to each vertex for (k = 0; k < 3; k++) rotatePoint(triangle[k], matrix); // If if collides, get pushback vector if (doesCollide(boundingBox, triangle)) { collided = 1; if (math_absF(pushback[1]) < math_absF(triangleNormal[1])) pushback[1] = triangleNormal[1]; pushback[0] += triangleNormal[0]; pushback[2] += triangleNormal[2]; math_triangleNormal(&triangle[0][0], triangleNormal); VectorDot(triangleNormal, velocity, force); // printf("Collided with normal %6.2f %6.2f %6.2f\n", // triangleNormal[0], triangleNormal[1], triangleNormal[2]); if (force > 0) { // printf("Force = %6.2f\n", force); collisions++; VectorScale(triangleNormal, -1); // Scale the triangleNormal by the force // to get the amount pushed back // Set the maximum pushback amount for (j = 0; j < 3; j++) { if (math_absF(pushback[j]) < math_absF(triangleNormal[j])) pushback[j] = triangleNormal[j]; } } } } // if (collided) { // printf("Pushback = (%6.2f, %6.2f, %6.2f)\n", // pushback[0], pushback[1], pushback[2]); // } }