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