void initMouse(Mouse *input){
input->currentPos = makePoint(0,0);
input->deltaPos = makePoint(0,0);
int i;
for(i=0;i<NBBUTTONS;i++) input->isPressed[i] =0;
input->originPressedPos = makePoint(0,0);
}
TEST(GridUtils, CoordinateConversions)
{
// Set up info for a map at (2, -1) that is rotated 45 degrees, with resolution 0.1
nav_msgs::MapMetaData info;
info.resolution = 0.1f;
info.origin = makePose(2, -1, PI/4);
info.height = 50;
info.width = 80;
// Check conversions
EXPECT_EQ (804u, occupancy_grid_utils::cellIndex(info, occupancy_grid_utils::Cell(4, 10)));
EXPECT_EQ (occupancy_grid_utils::Cell(8, 1), occupancy_grid_utils::pointCell(info, makePoint(2.5, -0.35)));
EXPECT_EQ (88u, occupancy_grid_utils::pointIndex(info, makePoint(2.5, -0.35)));
EXPECT_EQ (occupancy_grid_utils::Cell(-8, 7), occupancy_grid_utils::pointCell(info, makePoint(1, -1)));
EXPECT_THROW (occupancy_grid_utils::pointIndex(info, makePoint(1, -1)), std::out_of_range);
EXPECT_THROW (occupancy_grid_utils::cellIndex(info, occupancy_grid_utils::Cell(100, 100)), std::out_of_range);
// Cell polygon
const double side = std::sqrt(2)/2;
geometry_msgs::Polygon expected;
expected.points.resize(4);
expected.points[0].x = static_cast<float>(2 + .1*side);
expected.points[0].y = static_cast<float>(-1 + .3*side);
expected.points[1].x = static_cast<float>(2);
expected.points[1].y = static_cast<float>(-1 + .4*side);
expected.points[2].x = static_cast<float>(2 + .1*side);
expected.points[2].y = static_cast<float>(-1 + .5*side);
expected.points[3].x = static_cast<float>(2 + .2*side);
expected.points[3].y = static_cast<float>(-1 + .4*side);
EXPECT_EQ(expected, cellPolygon(info, occupancy_grid_utils::Cell(2, 1)));
}
TEST(rosToEigen, pose) {
Eigen::Isometry3d pose1 = toEigen(makePose(makePoint(0, 1.5, 2), makeQuaternion(1, 0, 0, 0)));
Eigen::Isometry3d pose2 = toEigen(makePose(makePoint(-1.5, -2.6, -3.7), makeQuaternion(0, 0, 0, 1)));
Eigen::Vector3d const & p1 = pose1.translation();
Eigen::Vector3d const & p2 = pose2.translation();
Eigen::Quaterniond const & q1 = Eigen::Quaterniond{pose1.rotation()};
Eigen::Quaterniond const & q2 = Eigen::Quaterniond{pose2.rotation()};
ASSERT_NEAR(0.0, p1.x(), 1e-5);
ASSERT_NEAR(1.5, p1.y(), 1e-5);
ASSERT_NEAR(2.0, p1.z(), 1e-5);
ASSERT_NEAR(-1.5, p2.x(), 1e-5);
ASSERT_NEAR(-2.6, p2.y(), 1e-5);
ASSERT_NEAR(-3.7, p2.z(), 1e-5);
ASSERT_NEAR(1, q1.x(), 1e-5);
ASSERT_NEAR(0, q1.y(), 1e-5);
ASSERT_NEAR(0, q1.z(), 1e-5);
ASSERT_NEAR(0, q1.w(), 1e-5);
ASSERT_NEAR(0, q2.x(), 1e-5);
ASSERT_NEAR(0, q2.y(), 1e-5);
ASSERT_NEAR(0, q2.z(), 1e-5);
ASSERT_NEAR(1, q2.w(), 1e-5);
}
int main()
{
struct Drawing d;
// @@@ modify the values 400 and 150 to be the width/height
// @@@ that you want
initDrawing(&d, DRAWINGTYPE_COLOR, 400, 150, COLOR_WHITE);
// @@@ call the drawing Routines. The values 100,100, 50, 20
// @@@ below are just example values
// @@@ Draw each shape in at least two places in at least two
// @@@ different sizes
drawShape1(&d,makePoint(100,100),50,20);
drawShape1(&d,makePoint(150,100),70,25);
drawShape2(&d,makePoint(200,100),40,30);
drawShape2(&d,makePoint(250,50),30,40);
writeFile(&d, FILENAME);
return 0;
}
int main()
{
struct Drawing d;
struct Circle bottom;
struct Circle middle;
struct Circle top;
initDrawing(&d, DRAWINGTYPE_BW, 200, 150, BW_BLACK);
// draw a snowMan with height 90 pixels
// Bottom circle has radius 20, center at 100, 80
// Middle circle has radius 15, center at 100, 45
// Top circle has radius 10, center at 100, 20
initCircle(&bottom, makePoint(100.0,80.0), 20.0);
initCircle(&middle, makePoint(100.0,45.0), 15.0);
initCircle(&top, makePoint(100.0,20.0), 10.0);
fillCircle(&d,bottom,BW_WHITE);
fillCircle(&d,middle,BW_WHITE);
fillCircle(&d,top, BW_WHITE);
writeFile(&d, FILENAME);
return 0;
}
Point getPointWithNegativeValue(int ignore){
int x, y;
for(x = 0; x < mapSize.width; x++){
for(y = 0; y < mapSize.height; y++){
if(map[x][y] < 0 && map[x][y] != ignore)
return makePoint(x, y);
}
}
return makePoint(0, 0); //shut the compiler up.
}
Point startPoint(void){
int x, y;
for (y = 0; y < mapSize.height; y++) {
for (x = 0; x < mapSize.width; x++) {
if (map[x][y] == SPOTA) {
return makePoint(x, y);
}
}
}
return makePoint(0, 0);
}
TEST(rosToEigen, point) {
Eigen::Vector3d p1 = toEigen(makePoint(0, 1.5, 2));
Eigen::Vector3d p2 = toEigen(makePoint(-1.5, -2.6, -3.7));
ASSERT_NEAR(0.0, p1.x(), 1e-5);
ASSERT_NEAR(1.5, p1.y(), 1e-5);
ASSERT_NEAR(2.0, p1.z(), 1e-5);
ASSERT_NEAR(-1.5, p2.x(), 1e-5);
ASSERT_NEAR(-2.6, p2.y(), 1e-5);
ASSERT_NEAR(-3.7, p2.z(), 1e-5);
}
void drawLintasan(Lintasan l, Frame frame, int color) {
Point p1 = makePoint(l.position.xFrame - l.topWidth/2, l.position.yFrame + l.height);
Point p2 = makePoint(l.position.xFrame + l.topWidth/2, l.position.yFrame + l.height);
Point p3 = makePoint(l.position.xFrame + l.bottomWidth/2, l.position.yFrame);
Point p4 = makePoint(l.position.xFrame - l.bottomWidth/2, l.position.yFrame);
Line line1 = makeLine(&p1, &p4);
Line line2 = makeLine(&p2, &p3);
drawLineDDA(&line1, &frame, color);
drawLineDDA(&line2, &frame, color);
}
void unblack (Bit2_T bitmap, int cur_x, int cur_y) {
int w_pixels = Bit2_width (bitmap);
int h_pixels = Bit2_height (bitmap);
Seq_T point_queue = Seq_new(w_pixels*h_pixels);
assert(point_queue);
assert(0 <= cur_x && cur_x < w_pixels);
assert(0 <= cur_y && cur_y < h_pixels);
if (Bit2_get(bitmap, cur_x, cur_y) != 1) { // if pixel is white
assert(point_queue);
Seq_free(&point_queue);
return;
} else {
Seq_addhi(point_queue, (void*)makePoint(cur_x,cur_y));
while (Seq_length(point_queue) > 0) {
PointPair temp = (PointPair)Seq_remlo(point_queue);
assert(temp);
int i = temp->i;
int j = temp->j;
freePoint(temp);
if (Bit2_get(bitmap, i, j) == 1) { // if current is black pixel
Bit2_put(bitmap, i, j, 0); // set current to white
assert(0 <= i && i < w_pixels);
assert(0 <= j && j < h_pixels);
if (j != 0 && j != h_pixels-1) { // if not a top/bottom pixel
if (i+1 < w_pixels && Bit2_get(bitmap, i+1, j) == 1) { // if
Seq_addhi(point_queue, (void*)makePoint(i+1,j));
}
if (i > 0 && Bit2_get(bitmap, i-1, j) == 1) {
Seq_addhi(point_queue, (void*)makePoint(i-1,j));
}
}
if (i != 0 && i != w_pixels-1) {
if (j+1 < h_pixels && Bit2_get(bitmap, i, j+1) == 1) {
Seq_addhi(point_queue, (void*)makePoint(i,j+1));
}
if (j > 0 && Bit2_get(bitmap, i, j-1) == 1) {
Seq_addhi(point_queue, (void*)makePoint(i,j-1));
}
}
}
}
assert(point_queue);
Seq_free(&point_queue);
return;
}
}
void drawFilledPolygon(point* verticies, int numberOfVerticies, unsigned char shade) {
int x, y;
box myBox;
if(getSetting() == 0) { return; }
myBox = getBox(verticies, numberOfVerticies);
for(y = myBox.topL.y; y <= myBox.botR.y; y++){
for(x = myBox.topL.x; x <= myBox.botR.x; x++){
if(pointInPolygon(verticies, numberOfVerticies, makePoint(x, y))) {
drawPoint(makePoint(x, y), shade);
}
}
}
drawPolygon(verticies, numberOfVerticies, 0xF);
}
void drawRect(point topLeft, point botRight, unsigned char shade) {
point topRight; point botLeft;
if(topLeft.x > botRight.x){
intSwap(&topLeft.x, &botRight.x);
}if(topLeft.y > botRight.y){
intSwap(&topLeft.y, &botRight.y);
}
topRight = makePoint(botRight.x, topLeft.y);
botLeft = makePoint(topLeft.x, botRight.y);
drawLine(topLeft, topRight, shade);
drawLine(topRight, botRight, shade);
drawLine(botLeft, botRight, shade);
drawLine(topLeft, botLeft, shade);
}
int handleMouseEvent(Mouse *input, const SDL_Event *event){
switch(event->type)
{
case SDL_MOUSEMOTION:
input->deltaPos = makeVectorAB(makePoint(event->motion.x,event->motion.y),input->currentPos);
input->currentPos = makePoint(event->motion.x,event->motion.y);
break;
case SDL_MOUSEBUTTONUP:
switch (event->button.which) {
case SDL_BUTTON_LEFT:
input->isPressed[LEFTBUTTON] = 0;
break;
case SDL_BUTTON_RIGHT:
input->isPressed[RIGHTBUTTON] = 0;
break;
case SDL_BUTTON_WHEELDOWN:
input->isPressed[WHEELDOWN] = 0;
break;
case SDL_BUTTON_WHEELUP:
input->isPressed[WHEELUP] = 0;
break;
default:
break;
}
break;
case SDL_MOUSEBUTTONDOWN:
switch (event->button.which) {
case SDL_BUTTON_LEFT:
input->isPressed[LEFTBUTTON] = 1;
break;
case SDL_BUTTON_RIGHT:
input->isPressed[RIGHTBUTTON] = 1;
break;
case SDL_BUTTON_WHEELDOWN:
input->isPressed[WHEELDOWN] = 1;
break;
case SDL_BUTTON_WHEELUP:
input->isPressed[WHEELUP] = 1;
break;
default:
break;
}
break;
default:
break;
}
return 0;
}
void
pamd_text_box(int const height,
int const angle,
const char * const s,
int * const leftP,
int * const topP,
int * const rightP,
int * const bottomP) {
/*----------------------------------------------------------------------------
Calculate extents rectangle for a given piece of text. For most
applications where extents are needed, angle should be zero to obtain the
unrotated extents. If you need the extents box for post-rotation text,
however, you can set angle nonzero and it will be calculated correctly.
-----------------------------------------------------------------------------*/
extleft = 32767;
exttop = 32767;
extright = -32767;
extbottom = -32767;
pamd_text(NULL, 32767, 32767, 255, 255, makePoint(1000, 1000),
height, angle, s,
extentsDrawproc, NULL);
*leftP = extleft - 1000;
*topP = exttop - 1000;
*rightP = extright - 1000;
*bottomP = extbottom - 1000;
}
pair<double, double> solve(const int* input) {
vector<point> points;
for (int i = 0; i < 18; i+=3) {
points.push_back(makePoint(input[i+0], input[i+1], input[i+2]));
}
points.push_back(calculateTwoTetrahedraCenter(points[0],points[1],points[2],points[3],points[4]));
double minHeight = 100000.0;
double maxHeight = -1.0;
for (int i = 0; i < ROTATION_SIZE; i++) {
vector<point> rotated;
for (int j = 0; j < POINT_SIZE; j++) {
rotated.push_back(points[ROTATION[i][j]]);
}
if (!normalizePosition(rotated)) {
continue;
}
if (isStable(rotated)) {
minHeight = min(minHeight, rotated[5][2]);
maxHeight = max(maxHeight, rotated[5][2]);
}
}
return make_pair(minHeight, maxHeight);
}
frts::PointPtr frts::PointImpl::operator*(const Point& other) const
{
value cx = y * other.getZ() - z * other.getY();
value cy = z * other.getX() - x * other.getZ();
value cz = x * other.getY() - y * other.getX();
return makePoint(cx, cy, cz);
}
static void
drawSteepLine(pamd_drawproc drawProc,
const void * const clientdata,
tuple ** const tuples,
int const cols,
int const rows,
int const depth,
sample const maxval,
pamd_point const p0,
pamd_point const p1) {
/*----------------------------------------------------------------------------
Draw a line that is more vertical than horizontal.
Don't clip.
Assume the line has distinct start and end points (i.e. it's at least
two points).
-----------------------------------------------------------------------------*/
/* Loop over Y domain. */
long dx, scol;
int dy, col, row, prevcol;
if (p1.y > p0.y)
dy = 1;
else
dy = -1;
dx = (p1.x - p0.x) * DDA_SCALE / abs(p1.y - p0.y);
row = p0.y;
prevcol = col = p0.x;
scol = col * DDA_SCALE + DDA_SCALE / 2;
for ( ; ; ) {
if (linetype == PAMD_LINETYPE_NODIAGS && col != prevcol) {
drawPoint(drawProc, clientdata,
tuples, cols, rows, depth, maxval,
makePoint(prevcol, row));
prevcol = col;
}
drawPoint(drawProc, clientdata, tuples, cols, rows, depth, maxval,
makePoint(col, row));
if (row == p1.y)
break;
row += dy;
scol += dx;
col = scol / DDA_SCALE;
}
}
static void
drawShallowLine(pamd_drawproc drawProc,
const void * const clientdata,
tuple ** const tuples,
int const cols,
int const rows,
int const depth,
sample const maxval,
pamd_point const p0,
pamd_point const p1) {
/*----------------------------------------------------------------------------
Draw a line that is more horizontal than vertical.
Don't clip.
Assume the line has distinct start and end points (i.e. it's at least
two points).
-----------------------------------------------------------------------------*/
/* Loop over X domain. */
long dy, srow;
int dx, col, row, prevrow;
if (p1.x > p0.x)
dx = 1;
else
dx = -1;
dy = (p1.y - p0.y) * DDA_SCALE / abs(p1.x - p0.x);
prevrow = row = p0.y;
srow = row * DDA_SCALE + DDA_SCALE / 2;
col = p0.x;
for ( ; ; ) {
if (linetype == PAMD_LINETYPE_NODIAGS && row != prevrow) {
drawPoint(drawProc, clientdata,
tuples, cols, rows, depth, maxval,
makePoint(col, prevrow));
prevrow = row;
}
drawPoint(drawProc, clientdata, tuples, cols, rows, depth, maxval,
makePoint(col, row));
if (col == p1.x)
break;
srow += dy;
row = srow / DDA_SCALE;
col += dx;
}
}
void
pamd_text(tuple** const tuples,
int const cols,
int const rows,
int const depth,
sample const maxval,
pamd_point const pos,
int const height,
int const angle,
const char * const sArg,
pamd_drawproc drawProc,
const void * const clientdata) {
/*----------------------------------------------------------------------------
Draw the zero-terminated string 'sArg', with its baseline starting at point
'pos', inclined by 'angle' degrees to the X axis, with letters 'height'
tuples high (descenders will extend below the baseline). We pass the
supplied drawproc and clientdata to pamd_linep, which performs the actual
drawing.
There may be multiple lines of text. The baseline of the topmost line
starts at 'pos'.
-----------------------------------------------------------------------------*/
const struct ppmd_font * const fontP = ppmd_get_font();
long rotsin, rotcos;
pamd_point p;
const char * s;
pamd_validatePoint(pos);
p = makePoint(0, 0);
rotsin = isin(-angle);
rotcos = icos(-angle);
for (s = &sArg[0]; *s; ) {
unsigned char const ch = *s++;
if (ch >= fontP->header.firstCodePoint &&
ch < fontP->header.firstCodePoint + fontP->header.characterCount) {
const struct ppmd_glyph * const glyphP =
&fontP->glyphTable[ch - fontP->header.firstCodePoint];
unsigned int cursorAdvance;
pamd_validatePoint(p);
drawGlyph(glyphP, p, tuples, cols, rows, depth, maxval,
height, pos, rotcos, rotsin,
drawProc, clientdata, &cursorAdvance);
p.x += cursorAdvance;
} else if (ch == '\n') {
/* Move to the left edge of the next line down */
p.y += Scalef + Descend;
p.x = 0;
}
}
}
int main () {
point p1, p2;
double x, y;
printf("Enter coordinates of the first point: ");
fflush(stdout);
scanf("%lf %lf", &x, &y);
p1 = makePoint(x, y);
printf("Enter coordinates of the second point: ");
fflush(stdout);
scanf("%lf %lf", &x, &y);
p2 = makePoint(x, y);
printf("The distance between (%lf, %lf) and (%lf, %lf)\n is %lf.\n", p1.xCoord, p1.yCoord, p2.xCoord, p2.yCoord, distance(p1, p2));
return 0;
}
//Raster drawing
void drawSprite(unsigned char sprite[], point pos,
unsigned int width, unsigned int height) {
int i,j;
for(j = 0; j < height; j++) {
for(i = 0; i < width; i++) {
drawPoint(makePoint(pos.x+i, pos.y+j), sprite[i+j*width]);
}
}
}
Lintasan makeLintasan(Point pos, int tw, int bw, int h) {
Lintasan l;
l.position = makePoint(pos.xFrame, pos.yFrame);
l.topWidth = tw;
l.bottomWidth = bw;
l.height = h;
return l;
}
void killExplosions(void) {
int i;
//Set all Explosions to dead.
for(i = 0; i < MAX_EXPLOSIONS; i++) {
gExplosions[i].pos = makePoint(0,0);
gExplosions[i].status = DEAD;
gExplosions[i].lifetime = 0;
gExplosions[i].current = 0;
}
}
void killEnemies(void) {
int i;
//Set all UFOs to dead.
for(i = 0; i < MAX_UFOS; i++) {
gUFOs[i].pos = makePoint(0,0);
gUFOs[i].dx = 0;
gUFOs[i].dy = 0;
gUFOs[i].status = DEAD;
gUFOs[i].type = UFO_LARGE;
}
//Set all Satellites to dead.
for(i = 0; i < MAX_SATELLITES*3; i++) {
gSatellites[i].pos = makePoint(0,0);
gSatellites[i].dx = 0;
gSatellites[i].dy = 0;
gSatellites[i].status = DEAD;
gSatellites[i].type = SATELLITE;
}
}
static pamd_point
commandPoint(const struct ppmd_glyphCommand * const commandP) {
/*----------------------------------------------------------------------------
Return the point which is the argument of glyph drawing command
*commandP. The origin of the coordinate system for this point
is the center of the glyph cell and the scale is the scale of the
font, so (-10, -10) means the upper left corner of the glyph cell.
-----------------------------------------------------------------------------*/
return makePoint(twosCompByteValue(commandP->x),
twosCompByteValue(commandP->y));
}
static pamd_point
textPosFromFontPos(pamd_point const fontPos,
pamd_point const textBoxOrigin,
pamd_point const center,
pamd_point const glyphOrigin,
unsigned int const height,
long const rotcos,
long const rotsin) {
/*----------------------------------------------------------------------------
'fontPos' is a position within a glyph as told by the font definition.
It is relative to the center of the glyph, in units of font tuples
(1/21 of a glyph cell).
We return the position on the canvas of that point.
That takes into account where in the text box we are, where the text box
is on the canvas, the size of the characters, and the rotation of the
text box.
-----------------------------------------------------------------------------*/
pamd_point const ptl = vectorSum(center, fontPos);
/* Position relative to the top left of the standard glyph cell */
pamd_point const pl = vectorSum(glyphOrigin, ptl);
/* Position relative to the top left of the whole text box,
assuming the text box is horizontal and has font scale.
*/
pamd_point const ps = makePoint((pl.x * (int)height) / Scalef,
(pl.y * (int)height) / Scalef);
/* Same as above, but with the text box its actual size */
pamd_point const retval =
makePoint(textBoxOrigin.x +
(ps.x * rotcos - (ps.y-(int)height) * rotsin) / 65536,
textBoxOrigin.y +
(ps.x * rotsin + (ps.y-(int)height) * rotcos) / 65536);
pamd_validatePoint(retval);
return retval;
}
Point getCenter(Polygon p){
Point newp;
int centerx = 0;
int centery = 0;
int i;
for (i=0; i<p.nPoint; i++){
centerx += p.lp[i].x;
centery += p.lp[i].y;
}
newp = makePoint(centerx/3, centery/3);
return newp;
}
//Vector drawing
void drawLine(point a, point b, unsigned char shade) {
int dx = abs(b.x-a.x), sx = a.x<b.x ? 1 : -1;
int dy = abs(b.y-a.y), sy = a.y<b.y ? 1 : -1;
int err = (dx>dy ? dx : -dy)/2, e2;
for(;;) {
drawPoint(makePoint(a.x, a.y), shade);
if (a.x==b.x && a.y==b.y) { break; }
e2 = err;
if (e2 >-dx) { err -= dy; a.x += sx; }
if (e2 < dy) { err += dx; a.y += sy; }
}
}
GWindow makeWindow(int x, int y, int w, int h){
GWindow window;
window.offsetX = x;
window.offsetY = y;
window.width = w;
window.height = h;
int originX = (x+w+x)/2;
int originY = (y+h+y)/2;
Point P = makePoint(originX, originY);
window.origin = P;
return window;
}
void initCamera (int w, int h) {
viewpoint = makePoint(0.0,0.0,0.0);
pnear = 1.0;
fovx = M_PI/2.15;
/* window dimensions */
width = w;
height = h;
imageWidth = 2.0 * pnear * tan(fovx/2);
printf("Angle of view %.1f degrees\n", fovx/M_PI * 180);
printf("Virtual screen size -%.2f x %.2f\n",imageWidth/2.0,imageWidth/2.0);
}
