void runTest()
{
BufferedLineSegmentIntersector uut;
{
LineSegment ls1(0, 0, 1, 1);
LineSegment ls2(0, 0, 1, 1);
LineSegment result;
HOOT_STR_EQUALS(true, uut.intersect(ls1, 1, ls2, result));
HOOT_STR_EQUALS("LINESEGMENT(0 0,1 1)",
result);
}
{
LineSegment ls1(0, 1, 1, 0);
LineSegment ls2(0, 0, 1, 1);
LineSegment result;
HOOT_STR_EQUALS(true, uut.intersect(ls1, .14142135623730950488016887242097, ls2, result));
HOOT_STR_EQUALS("LINESEGMENT(0.4 0.4,0.6 0.6)",
result);
}
{
LineSegment ls1(2, 0, 3, 2);
LineSegment ls2(2, 0, 4, 4);
LineSegment result;
HOOT_STR_EQUALS(true, uut.intersect(ls1, .1, ls2, result));
HOOT_STR_EQUALS("LINESEGMENT(2 0,3.04472 2.08944)", result);
}
}
void
GeometricHelperTest::LineSegmentIntersectionTest() {
{
LineSegment ls1(Vertex(0, 1), Vertex(1, 1));
LineSegment ls2(Vertex(0, 0), Vertex(1, 2));
// segments intersect
boost::optional<Vertex> intersection_point_opt = GeometricHelper::intersect(ls1, ls2);
CPPUNIT_ASSERT_MESSAGE("Intersection point expected", bool(intersection_point_opt));
Vertex intersection_point = *intersection_point_opt;
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("Intersection point error", 0.5, intersection_point.x(), 1E-10);
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("Intersection point error", 1.0, intersection_point.y(), 1E-10);
}
{
LineSegment ls1(Vertex(0, 1), Vertex(1, 1));
LineSegment ls2(Vertex(0, 0), Vertex(2, 1));
// segments do not intersect
boost::optional<Vertex> intersection_point_opt = GeometricHelper::intersect(ls1, ls2);
CPPUNIT_ASSERT_MESSAGE("No intersection point expected", !bool(intersection_point_opt));
}
{
LineSegment ls1(Vertex(0, 1), Vertex(1, 1));
LineSegment ls2(Vertex(0.5, 1), Vertex(2, 1));
// segments overlap
boost::optional<Vertex> intersection_point_opt = GeometricHelper::intersect(ls1, ls2);
CPPUNIT_ASSERT_MESSAGE("No intersection point expected", !bool(intersection_point_opt));
}
{
LineSegment ls1(Vertex(0.5, 1), Vertex(1, 1));
LineSegment ls2(Vertex(0, 1), Vertex(2, 1));
// segment 1 contained in segment 2
boost::optional<Vertex> intersection_point_opt = GeometricHelper::intersect(ls1, ls2);
CPPUNIT_ASSERT_MESSAGE("No intersection point expected", !bool(intersection_point_opt));
}
{
LineSegment ls1(Vertex(0, 1), Vertex(1, 1));
LineSegment ls2(Vertex(1.5, 1), Vertex(2, 1));
// segments parallel
boost::optional<Vertex> intersection_point_opt = GeometricHelper::intersect(ls1, ls2);
CPPUNIT_ASSERT_MESSAGE("No intersection point expected", !bool(intersection_point_opt));
}
}
// sanity test the URDF and enum
TEST(LockedRobotState, URDF_sanity)
{
moveit::core::RobotModelPtr model = getModel();
robot_interaction::LockedRobotState ls1(model);
EXPECT_EQ(ls1.getState()->getVariableNames()[JOINT_A], "joint_a");
}
void
ls(const char *path, const char *prefix)
{
int r;
struct Stat st;
if ((r = stat(path, &st,0)) < 0)
panic("stat %s: %e", path, r);
if ((st.st_isdir == FTYPE_DIR) && !flag['d'])
lsdir(path, prefix);
else
ls1(0, st.st_isdir, st.st_size, path,st.rwx,st.fuid,st.fgid);
}
void
ls(char *path, char *prefix)
{
int r;
struct Stat st;
if ((r=stat(path, &st)) < 0)
panic("stat %s: %e", path, r);
if (st.st_isdir && !flag['d'])
lsdir(path, prefix);
else
ls1(0, st.st_isdir, st.st_size, path);
}
// Test constructors and robot model loading
TEST(LockedRobotState, load)
{
moveit::core::RobotModelPtr model = getModel();
robot_interaction::LockedRobotState ls1(model);
moveit::core::RobotState state2(model);
state2.setToDefaultValues();
robot_interaction::LockedRobotState ls2(state2);
robot_interaction::LockedRobotStatePtr ls4(
new robot_interaction::LockedRobotState(model));
}
void runRandomTest()
{
BufferedLineSegmentIntersector uut;
double scale = 5;
double uutTime = 0.0;
double geosTime = 0.0;
// the geos intersection is an approximation so we need a fairly large epsilon
double epsilon = 1e-2;
size_t count = 1000;
for (size_t i = 0; i < count; i++)
{
double r = Tgs::Random::instance()->generateUniform() * 3.0;
LineSegment ls1(Tgs::Random::instance()->generateUniform() * scale,
Tgs::Random::instance()->generateUniform() * scale,
Tgs::Random::instance()->generateUniform() * scale,
Tgs::Random::instance()->generateUniform() * scale);
LineSegment ls2(Tgs::Random::instance()->generateUniform() * scale,
Tgs::Random::instance()->generateUniform() * scale,
Tgs::Random::instance()->generateUniform() * scale,
Tgs::Random::instance()->generateUniform() * scale);
LineSegment lsGeos;
double start = Tgs::Time::getTime();
bool resultGeos = geosIntersect(ls1, r, ls2, lsGeos);
double end = Tgs::Time::getTime();
geosTime += end - start;
LineSegment lsUut;
start = Tgs::Time::getTime();
bool resultUut = uut.intersect(ls1, r, ls2, lsUut);
end = Tgs::Time::getTime();
uutTime += end - start;
// if the line is very close to the buffer then we can't expect consistent results.
bool touchy = fabs(ls1.distance(ls2) - r) <= epsilon;
CPPUNIT_ASSERT_EQUAL(resultGeos || touchy, resultUut || touchy);
if (resultGeos && !touchy)
{
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, lsGeos.p0.distance(lsUut.p0), epsilon);
CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0, lsGeos.p1.distance(lsUut.p1), epsilon);
}
}
LOG_INFO("GEOS Mean Time: " << (1000.0 * geosTime) / (double)count << "ms");
LOG_INFO("UUT Mean Time: " << (1000.0 * uutTime) / (double)count << "ms");
LOG_INFO("Ratio: " << geosTime / uutTime);
}
void
lsdir(const char *path, const char *prefix)
{
int fd, n;
struct File f;
if ((fd = open(path, O_RDONLY)) < 0)
panic("open %s: %e", path, fd);
while ((n = readn(fd, &f, sizeof f)) == sizeof f)
if (f.f_name[0])
ls1(prefix, f.f_type, f.f_size, f.f_name,f.rwx,f.fuid,f.fgid);
if (n > 0)
panic("short read in directory %s", path);
if (n < 0)
panic("error reading directory %s: %e", path, n);
}
static scanDirFunc ls2(PCStr(file),LsArg *lsa)
{ int li,osize,nsize;
ls1(file,lsa);
if( lsa->l_buf[0] ){
if( lsa->l_nfill == lsa->l_nsize ){
osize = lsa->l_nsize;
nsize = (lsa->l_nsize += 2048) * sizeof(Elem);
if( osize == 0 )
lsa->l_lines=(Elem*)malloc(nsize);
else lsa->l_lines=(Elem*)realloc(lsa->l_lines,nsize);
}
li = lsa->l_nfill++;
lsa->l_lines[li].s_ikey = lsa->l_ikey;
lsa->l_lines[li].s_skey = stralloc(file);
lsa->l_lines[li].s_line = stralloc(lsa->l_buf);
}
return 0;
}
TEST(LockedRobotState, robotStateChanged)
{
moveit::core::RobotModelPtr model = getModel();
Super1 ls1(model);
EXPECT_EQ(ls1.cnt_, 0);
robot_state::RobotState cp1(*ls1.getState());
cp1.setVariablePosition(JOINT_A, 0.00001);
cp1.setVariablePosition(JOINT_C, 0.00002);
cp1.setVariablePosition(JOINT_F, 0.00003);
ls1.setState(cp1);
EXPECT_EQ(ls1.cnt_, 1);
ls1.modifyState(modify1);
EXPECT_EQ(ls1.cnt_, 2);
ls1.modifyState(modify1);
EXPECT_EQ(ls1.cnt_, 3);
}
void MyBrush::draw() {
// Set up camera for drawing
setup2DDrawing( Color(0,0,0), screenWidth, screenHeight );
glPixelStorei(GL_PACK_ALIGNMENT, 1);
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
// Draw a border around the actual image
glColor3f(1.0f, 1.0f, 1.0f);
glBegin(GL_LINE_LOOP);
glVertex2i( 0, 0 );
glVertex2i( imageWidth+1, 0 );
glVertex2i( imageWidth+1, imageHeight+1 );
glVertex2i( 0, imageHeight+1 );
glEnd();
glRasterPos2i(0, 0);
// Copy data into window
//for ( int iX = 0; iX < 100; iX++ )
//putPixel( iX, iX, Color(1,0,0) );
glDrawPixels(imageWidth, imageHeight, GL_RGB, GL_UNSIGNED_BYTE, &pixelData[0]);
// These 5 lines draw a white line across your canvas
// Remove this and replace it with intelligent OpenGL preview code
/*
glLineWidth( 10);
glBegin( GL_LINES );
glVertex2i( 100, 100 );
glVertex2i( 200, 200 );
glEnd();
*/
// Add in your OpenGL pre-view code here
// display draw in progress (mouse is down)
ToolType type = brushUI->getToolType();
if (!isMouseDown) {
switch(type) {
case TOOL_BRUSH:{
int xCenter = mouseDrag[0];
int yCenter = mouseDrag[1];
int radius = brushUI->getRadius();
int segments = radius * 4;
glBegin(GL_LINE_LOOP);
for (int i = 0; i < segments; i++)
{
float angle = 2.0f * 3.1415926f * float(i) / float(segments);
float x = radius * cosf(angle);
float y = radius * sinf(angle);
glVertex2f(x + xCenter, y + yCenter);
}
glEnd();
}break;
}
}
else{
switch (type) {
case TOOL_LINE:{
const int radius = brushUI->getRadius();
int x0 = mouseDown[0];
int y0 = mouseDown[1];
int x1 = mouseDrag[0];
int y1 = mouseDrag[1];
LineSegment ls(x0, y0, x1, y1);
std::vector<LineSegment> lineList;
//find four corners of thick line
if (ls.v0.horizontal){
LineSegment ls0(x0, (int)(y0 - floor(radius / (float)2)), x1, (int)(y1 - floor(radius / (float)2)));
LineSegment ls1(x0, (int)(y0 + ceil(radius / (float)2)), x1, (int)(y1 + ceil(radius / (float)2)));
lineList.push_back(ls0);
lineList.push_back(ls1);
}
else if (ls.v0.type == VERTICAL){
LineSegment ls0((int)(x0 - floor(radius / (float)2)), y0, (int)(x1 + ceil(radius / (float)2)), y0);
LineSegment ls1((int)(x0 - floor(radius / (float)2)), y1, (int)(x1 + ceil(radius / (float)2)), y1);
lineList.push_back(ls0);
lineList.push_back(ls1);
}
else{
int rise = y1 - y0;
int run = x1 - x0;
float normFactor = (float)sqrt(rise*rise + run*run);
float dy = run / normFactor;
float dx = rise / normFactor;
float halfRadius = radius / (float)2;
int fR = (int)floor(halfRadius);
int cR = (int)ceil(halfRadius);
LineSegment ls0(x0 - myroundf(dx*fR), y0 + myroundf(dy*fR), x1 - myroundf(dx*fR), y1 + myroundf(dy*fR));
LineSegment ls1(x1 + myroundf(dx*cR), y1 - myroundf(dy*cR), x0 + myroundf(dx*cR), y0 - myroundf(dy*cR));
lineList.push_back(ls0);
lineList.push_back(ls1);
}
glBegin(GL_LINE_LOOP);
glVertex2f((GLfloat)lineList[0].v0.x, (GLfloat)lineList[0].v0.y);
glVertex2f((GLfloat)lineList[0].v1.x, (GLfloat)lineList[0].v1.y);
glVertex2f((GLfloat)lineList[1].v0.x, (GLfloat)lineList[1].v0.y);
glVertex2f((GLfloat)lineList[1].v1.x, (GLfloat)lineList[1].v1.y);
glEnd();
}break;
case TOOL_CIRCLE:{
int xCenter = mouseDown[0];
int yCenter = mouseDown[1];
int thickness = brushUI->getRadius();
int xExtent = mouseDrag[0];
int yExtent = mouseDrag[1];
int radius = myroundf((float)sqrt((xExtent - xCenter)*(xExtent - xCenter) + (yExtent - yCenter)*(yExtent - yCenter)));
int innerRadius = radius - (int)floor(thickness / (float)2);
int outerRadius = radius + (int)ceil(thickness / (float)2);
int segments = innerRadius * 4;
glBegin(GL_LINE_LOOP);
for (int i = 0; i < segments; i++)
{
float angle = 2.0f * 3.1415926f * float(i) / float(segments);
float x = innerRadius * cosf(angle);
float y = innerRadius * sinf(angle);
glVertex2f(x + xCenter, y + yCenter);
}
glEnd();
segments = outerRadius * 4;
glBegin(GL_LINE_LOOP);
for (int i = 0; i < segments; i++)
{
float angle = 2.0f * 3.1415926f * float(i) / float(segments);
float x = outerRadius * cosf(angle);
float y = outerRadius * sinf(angle);
glVertex2f((GLfloat)(x + xCenter), (GLfloat)(y + yCenter));
}
glEnd();
}break;
case TOOL_POLYGON:{
glBegin(GL_LINE_LOOP);
for (size_t i = 0; i < polygon.size(); ++i){
glVertex2f((GLfloat)polygon[i][0], (GLfloat)polygon[i][1]);
}
glVertex2f((GLfloat)mouseDrag[0], (GLfloat)mouseDrag[1]);
glEnd();
}break;
}
}
endDrawing();
}
int ext2(void) {
printf("Hello World, this is the Ext2 FS\n");
// Hardcode test fs into memory so that we can test read
// Set up the superblock
struct ext2_super_block *sb;
sb = (struct ext2_super_block*) (VIRT_MEM_LOCATION +
EXT2_SUPERBLOCK_LOCATION);
sb->s_inodes_count = 50;
sb->s_blocks_count = 8192;
sb->s_r_blocks_count = 6;
sb->s_free_blocks_count = 8186;
sb->s_free_inodes_count = 49;
sb->s_first_data_block = 1;
sb->s_log_block_size = 0;
sb->s_log_frag_size = 0;
sb->s_blocks_per_group = 8192;
sb->s_frags_per_group = 8192;
sb->s_inodes_per_group = 50;
sb->s_magic = EXT2_MAGIC;
sb->s_state = EXT2_VALID_FS;
sb->s_errors = EXT2_ERRORS_CONTINUE;
sb->s_creator_os = EXT2_OS_XINU;
sb->s_first_ino = 2;
sb->s_inode_size = sizeof( struct ext2_inode );
sb->s_block_group_nr = 0;
char name[16] = "FAKE RAM FS :D";
memcpy(sb->s_volume_name,name,16);
// Set up the group descriptors table
struct ext2_group_desc *gpd;
// DUMB POINTER ARITHMATIC
gpd = (struct ext2_group_desc *) (sb + 1);
gpd->bg_block_bitmap = 2;
gpd->bg_inode_bitmap = 3;
gpd->bg_inode_table = 4;
gpd->bg_free_blocks_count = 44;
gpd->bg_free_inodes_count = 19;
gpd->bg_used_dirs_count = 1;
// Set up the block bitmap
uint8 *blBitmap;
blBitmap = (uint8 *) (sb + 2);
blBitmap[0] = 0x3F; // super block
int i;
for (i = 6; i < sb->s_blocks_count; i++)
blBitmap[i] = 0;
// Set up the inode bitmap
uint8 *iBitmap;
iBitmap = (uint8 *) (sb + 3);
iBitmap[0] = 0x1; // .
for (i = 1; i < sb->s_inodes_count; i++)
iBitmap[i] = 0;
// Set up the inode table
struct ext2_inode *iTbl;
iTbl = (struct ext2_inode *) (sb + 4);
// Set up . inode
iTbl->i_mode = EXT2_S_IFDIR;
iTbl->i_size = sizeof(struct ext2_dir_entry_2);
iTbl->i_links_count = 0;
iTbl->i_blocks = 1;
iTbl->i_flags = EXT2_NODUMP_FL;
iTbl->i_block[0] = 5;
// Set up . entry for the home directory
struct ext2_dir_entry_2 *blk5;
blk5 = (struct ext2_dir_entry_2 *) (sb + 5);
blk5->inode = 1;
blk5->next_dirent = 0;
blk5->name_len = 1;
blk5->filetype = 2;
char homeName[255] = ".";
memcpy(blk5->name, homeName, 255);
_fs_ext2_init();
touch1( xinu_fs, "./", "test" );
char bufferL[9] = "Go long!";
uint32 bytes_written;
ext2_write_status stat = ext2_write_file_by_path( xinu_fs, "./test", bufferL,
&bytes_written, 0, 8 );
touch1( xinu_fs, "./", "yo");
stat = ext2_write_file_by_path( xinu_fs, "./yo", bufferL,
&bytes_written, 0, 8 );
touch1( xinu_fs, "./", "whoah" );
stat = ext2_write_file_by_path( xinu_fs, "./whoah", bufferL,
&bytes_written, 0, 8 );
touch1( xinu_fs, "./", "iasjdf" );
stat = ext2_write_file_by_path( xinu_fs, "./iasjdf", bufferL,
&bytes_written, 0, 8 );
touch1( xinu_fs, "./", "f" );
stat = ext2_write_file_by_path( xinu_fs, "./f", bufferL,
&bytes_written, 0, 8 );
ls1( xinu_fs, "./" );
printf("removing yo\n");
rm1( xinu_fs, "./", "yo" );
ls1( xinu_fs, "./" );
printf("touching asdf\n");
touch1( xinu_fs, "./", "asdf" );
stat = ext2_write_file_by_path( xinu_fs, "./asdf", bufferL,
&bytes_written, 0, 8 );
ls1( xinu_fs, "./" );
printf("mking dir\n");
mkdir1( xinu_fs, "./", "dir" );
ls1( xinu_fs, "./" );
printf("touching yo\n");
touch1( xinu_fs, "./dir/", "yo" );
ls1( xinu_fs, "./dir/");
copy1( xinu_fs, "./", "whoah", "./", "hello" );
ls1( xinu_fs, "./" );
cat1( xinu_fs, "./", "hello" );
copy1( xinu_fs, "./", "hello", "./dir/", "hello" );
printf("HERE\n");
ls1( xinu_fs, "./dir/" );
cat1( xinu_fs, "./dir/", "hello" );
printf("removeing\n");
mv1( xinu_fs, "./dir/", "yo", "./", "a" );
ls1( xinu_fs, "./" );
cat1( xinu_fs, "./", "a" );
#if 0
// Test the read/write functions
printf("Testing hardcoded data\n");
print_superblock( xinu_fs->sb );
struct ext2_inode *i1 = ext2_get_inode(xinu_fs, 1);
print_inode( i1, 1, xinu_fs );
struct ext2_dir_entry_2 *home = ext2_get_first_dirent(xinu_fs, i1 );
print_dirent( home );
uint32 inode_num = ext2_inode_alloc( xinu_fs );
struct ext2_inode *i2 = ext2_get_inode( xinu_fs, inode_num+1 );
i2->i_mode = EXT2_S_IFREG;
i2->i_size = 0;
printf("Allocated new inode\n");
print_inode( i2, inode_num+1, xinu_fs );
struct ext2_dir_entry_2 *dirent = ext2_dirent_alloc( xinu_fs, i1 );
dirent->inode = 2;
dirent->next_dirent = 0;
dirent->name_len = 4;
dirent->filetype = EXT2_FT_REG_FILE;
char testName[255] = "test";
memcpy(dirent->name, testName, 255);
printf("Allocated new dir_entry_2 test\n");
print_dirent( dirent );
char path[8] = "./test";
char buffer[14] = "Writing! Yay!";
char bufferL[9] = "Go long!";
uint32 bytes_written;
ext2_write_status stat = ext2_write_file_by_path( xinu_fs, path, buffer,
&bytes_written, 0, 13 );
printf("bytes_written = %d stat = %d\n", bytes_written, stat);
char buffer2[12*1024];
// stat = ext2_write_file_by_path( xinu_fs, path, buffer2,
// &bytes_written, 13, (12*1024)-1 );
printf("bytes_written = %d stat = %d\n", bytes_written, stat);
// stat = ext2_write_file_by_path( xinu_fs, path, bufferL,
// &bytes_written, (12*1024)+12, 8 );
printf("bytes_written = %d stat = %d\n", bytes_written, stat);
int read = 0;
char readBuf[30];
read = ext2_read_dirent( xinu_fs, dirent, readBuf, 0, 29);
printf("Read %d bytes readBuf = %s\n", read, readBuf);
// read = ext2_read_dirent( xinu_fs, dirent, readBuf, (12*1024)+12, 10);
// printf("Read %d bytes readBuf = %s\n", read, readBuf);
#endif
return 0;
}
double MaximalSubline::ThresholdMatchCriteria::match(int index1, int index2) const
{
Coordinate c11 = _map->getNode(_w1->getNodeId(index1))->toCoordinate();
Coordinate c12 = _map->getNode(_w1->getNodeId(index1 + 1))->toCoordinate();
Coordinate c21 = _map->getNode(_w2->getNodeId(index2))->toCoordinate();
Coordinate c22 = _map->getNode(_w2->getNodeId(index2 + 1))->toCoordinate();
LineSegment ls1(c11, c12);
LineSegment ls2(c21, c22);
Radians heading1, heading2;
// if the line segment is zero length
if (ls1.p0 == ls1.p1)
{
// calculate heading based on location in way. See #4765 for the drama associated with this fix.
// This should find way headings appropriately even if there are one or more duplicate nodes in
// the way. This is done by calculating the way location before and after this node location.
// There may be some edge conditions when this will fail, but that seems very unlikely.
heading1 = WayHeading::calculateHeading(WayLocation(_map, _w1, index1, 0));
}
else
{
// little faster than above and better defined.
heading1 = ls1.angle();
}
// if the line segment is zero length
if (ls2.p0 == ls2.p1)
{
// calculate heading based on location in way
heading2 = WayHeading::calculateHeading(WayLocation(_map, _w2, index2, 0));
}
else
{
// little faster than above and better defined.
heading2 = ls2.angle();
}
Radians angleDiff = WayHeading::deltaMagnitude(heading1, heading2);
if (angleDiff > _maxAngleDiff)
{
return 0.0;
}
double maxD;
// Treat this as a variation of Frechet's Distance. This means that we're looking for the longest
// subline that is within _maxDistance of the other linestring.
// find the subline that is within _maxDistance. Ultimately we'll need a proper implementation of
// Frechet's distance, but that'll have to wait till later. See #3213
matchingSubline(ls1, ls2);
// previously I found the maximalNearest subline, but that causes issues when there is a large
// offset between the two datasets.
//maximalNearestSubline(ls1, ls2);
maxD = max(ls1.p0.distance(ls2.p0), ls1.p1.distance(ls2.p1));
double mns = min(ls1.getLength(), ls2.getLength());
const double epsilon = 1e-6;
if (maxD > _maxDistance + epsilon)
{
return 0.0;
}
return 1.0 * mns;
}
void
GeometricHelperTest::LineSegmentRayIntersectionTest() {
{
LineSegment ls1(Vertex(0, 1), Vertex(1, 1));
Ray ray(Vertex(0, 0), Vertex(1, 2));
// segment/ray intersect
boost::optional<Vertex> intersection_point_opt = GeometricHelper::intersect(ls1, ray);
CPPUNIT_ASSERT_MESSAGE("Intersection point expected", bool(intersection_point_opt));
Vertex intersection_point = *intersection_point_opt;
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("Intersection point error", 0.5, intersection_point.x(), 1E-10);
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("Intersection point error", 1.0, intersection_point.y(), 1E-10);
}
{
LineSegment ls1(Vertex(0, 3), Vertex(1, 3));
Ray ray(Vertex(0, 0), Vertex(0.25, 1));
// segment/ray intersect
boost::optional<Vertex> intersection_point_opt = GeometricHelper::intersect(ls1, ray);
CPPUNIT_ASSERT_MESSAGE("Intersection point expected", bool(intersection_point_opt));
Vertex intersection_point = *intersection_point_opt;
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("Intersection point error", 0.75, intersection_point.x(), 1E-10);
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE("Intersection point error", 3.0, intersection_point.y(), 1E-10);
}
{
LineSegment ls1(Vertex(0, 1), Vertex(1, 1));
Ray ray(Vertex(0.25, 2), Vertex(1, 4));
// segment/ray do not intersect
boost::optional<Vertex> intersection_point_opt = GeometricHelper::intersect(ls1, ray);
CPPUNIT_ASSERT_MESSAGE("No intersection point expected", !bool(intersection_point_opt));
}
{
LineSegment ls(Vertex(0, 1), Vertex(1, 1));
Ray ray(Vertex(0, 0), Vertex(2, 1));
// segments do not intersect
boost::optional<Vertex> intersection_point_opt = GeometricHelper::intersect(ls, ray);
CPPUNIT_ASSERT_MESSAGE("No intersection point expected", !bool(intersection_point_opt));
}
{
LineSegment ls(Vertex(0, 1), Vertex(1, 1));
Ray ray(Vertex(0.5, 1), Vertex(2, 1));
// segment/ray overlap
boost::optional<Vertex> intersection_point_opt = GeometricHelper::intersect(ls, ray);
CPPUNIT_ASSERT_MESSAGE("No intersection point expected", !bool(intersection_point_opt));
}
{
LineSegment ls(Vertex(0.5, 1), Vertex(1, 1));
Ray ray(Vertex(0, 1), Vertex(2, 1));
// segment contained in ray
boost::optional<Vertex> intersection_point_opt = GeometricHelper::intersect(ls, ray);
CPPUNIT_ASSERT_MESSAGE("No intersection point expected", !bool(intersection_point_opt));
}
{
LineSegment ls(Vertex(0, 1), Vertex(1, 1));
Ray ray(Vertex(1.5, 1), Vertex(2, 1));
// segment and ray parallel
boost::optional<Vertex> intersection_point_opt = GeometricHelper::intersect(ls, ray);
CPPUNIT_ASSERT_MESSAGE("No intersection point expected", !bool(intersection_point_opt));
}
}
int main(int argc, char** argv)
{
HeapInitialize();
for (int i=0; i<argc; i++)
{
if (strcmp(argv[i], "-verbose")==0 || strcmp(argv[i], "--verbose")==0)
gbVerifyVerbose = true;
}
// Tests
{
Verify_Step("ls1(`Test`)");
CEStr ls1(L"Test");
Verify0((ls1.ms_Val && 0==wcscmp(ls1.ms_Val,L"Test")),"ls1==`Test`");
Verify_Step("ls12 = ls1.Detach()");
/* Store ptr for Verify test result */ LPCWSTR pszPtr = ls1.ms_Val;
CEStr ls2 = ls1.Detach();
Verify2((ls2.ms_Val && !ls1.ms_Val && ls2.ms_Val==pszPtr),"ls2.ms_Val{x%p}==pszPtr{x%p}",ls2.ms_Val,pszPtr);
}
{
Verify_Step("ls3 = `Test3`");
CEStr ls3 = L"Test3";
Verify0((ls3.ms_Val && 0==wcscmp(ls3.ms_Val,L"Test3")),"ls3==`Test3`");
Verify_Step("ls4 = (LPCWSTR)ls3.ms_Val");
CEStr ls4 = static_cast<LPCWSTR>(ls3.ms_Val);
Verify2((ls4.ms_Val && ls4.ms_Val != ls3.ms_Val),"ls4.ms_Val{x%p}!=ls3.ms_Val{x%p}",ls4.ms_Val,ls3.ms_Val);
Verify_Step("ls5 = lstrdup(ls3)");
CEStr ls5 = lstrdup(ls3);
Verify0((ls5.ms_Val && 0==wcscmp(ls5.ms_Val,L"Test3")),"ls5==`Test3`");
Verify_Step("ls6(lstrdup(ls3))");
CEStr ls6(lstrdup(ls3));
Verify0((ls6.ms_Val && 0==wcscmp(ls6.ms_Val,L"Test3")),"ls6==`Test3`");
}
{
Verify_Step("NextArg and Switch comparison");
LPCWSTR pszCmd = L"conemu.exe /c/dir -run -inside=0x800 /cmdlist \"-inside=\\eCD /d %1\" -bad|switch ";
CEStr ls;
Verify0((0==NextArg(&pszCmd,ls)),"NextArg conemu.exe");
Verify0((!ls.IsPossibleSwitch()),"!IsPossibleSwitch()");
Verify0((0==NextArg(&pszCmd,ls)),"NextArg /c/dir");
Verify0((!ls.IsPossibleSwitch()),"!IsPossibleSwitch()");
Verify0((0==NextArg(&pszCmd,ls)),"NextArg -run");
Verify0((ls.OneOfSwitches(L"/cmd",L"/run")),"OneOfSwitches(/cmd,/run)");
Verify0((!ls.OneOfSwitches(L"/cmd",L"/cmdlist")),"!OneOfSwitches(/cmd,/cmdlist)");
Verify0((ls.IsSwitch(L"-run")),"IsSwitch(-run)");
Verify0((0==NextArg(&pszCmd,ls)),"NextArg -inside=0x800");
Verify0((ls.IsSwitch(L"-inside=")),"IsSwitch(-inside=)");
Verify0((ls.OneOfSwitches(L"-inside",L"-inside=")),"OneOfSwitches(-inside,-inside=)");
Verify0((!ls.IsSwitch(L"-inside")),"!IsSwitch(-inside)");
Verify0((0==NextArg(&pszCmd,ls)),"NextArg /cmdlist");
Verify0((ls.IsSwitch(L"-cmdlist")),"IsSwitch(-cmdlist)");
Verify0((0==NextArg(&pszCmd,ls)),"NextArg \"-inside=\\eCD /d %1\"");
Verify0((ls.IsSwitch(L"-inside:")),"IsSwitch(-inside=)");
Verify0((0==NextArg(&pszCmd,ls)),"NextArg -bad|switch");
Verify0((ls.Compare(L"-bad|switch")==0),"Compare(-bad|switch)");
Verify0((!ls.IsPossibleSwitch()),"!IsPossibleSwitch");
}
{
Verify_Step("-new_console parser tests");
LPCWSTR pszTest = L"-new_console:a \\\"-new_console:c\\\" `-new_console:d:C:\\` -cur_console:b";
LPCWSTR pszCmp = L"\\\"-new_console:c\\\" `-new_console:d:C:\\`";
RConStartArgs arg; arg.pszSpecialCmd = lstrdup(pszTest);
arg.ProcessNewConArg();
int iCmp = lstrcmp(arg.pszSpecialCmd, pszCmp);
Verify0((iCmp==0),"arg.pszSpecialCmd==\\\"-new_console:c\\\" `-new_console:d:C:\\`");
Verify_Step("RConStartArgs::RunArgTests()");
RConStartArgs::RunArgTests();
Verify0(!gbVerifyFailed,"RConStartArgs tests passed");
}
{
Verify_Step("msprintf tests");
wchar_t szBuffer[200];
msprintf(szBuffer, countof(szBuffer), L"%u %03u %03u %i %x %02X %02X %04x %08X",
123, 98, 4567, -234, 0x12AB, 0x0A, 0xABC, 0x01A0, 0x0765ABCD);
const wchar_t szStd[] = L"123 098 4567 -234 12ab 0A ABC 01a0 0765ABCD";
int iCmp = lstrcmp(szBuffer, szStd);
WVerify2((iCmp==0),L"`%s` (msprintf[W])\n `%s` (standard)", szBuffer, szStd);
char szBufA[200];
msprintf(szBufA, countof(szBufA), "%u %i %x %02X %02X %04x %08X",
123, -234, 0x12AB, 0x0A, 0xABC, 0x01A0, 0x0765ABCD);
const char szStdA[] = "123 -234 12ab 0A ABC 01a0 0765ABCD";
iCmp = lstrcmpA(szBufA, szStdA);
Verify2((iCmp==0),"`%s` (msprintf[A])\n `%s` (standard)", szBufA, szStdA);
}
if (gbVerifyFailed)
Verify_MsgFail("Some tests failed!");
else
Verify_MsgOk("All done");
return gbVerifyFailed ? 99 : 0;
}
// Run several threads and ensure they modify the state consistantly
// ncheck - # of checkThreadFunc threads to run
// nset - # of setThreadFunc threads to run
// nmod - # of modifyThreadFunc threads to run
static void runThreads(int ncheck, int nset, int nmod)
{
MyInfo info;
moveit::core::RobotModelPtr model = getModel();
robot_interaction::LockedRobotState ls1(model);
int num = ncheck + nset + nmod;
typedef int *int_ptr;
typedef boost::thread * thread_ptr;
int *cnt = new int[num];
int_ptr *counters = new int_ptr[num+1];
thread_ptr *threads = new thread_ptr[num];
int p = 0;
double val = 0.1;
// These threads check the validity of the RobotState
for (int i = 0 ; i < ncheck ; ++i)
{
cnt[p] = 0;
counters[p] = &cnt[p];
threads[p] = new boost::thread(&MyInfo::checkThreadFunc,
&info,
&ls1,
&cnt[p]);
val += 0.1;
p++;
}
// These threads set the RobotState to new values
for (int i = 0 ; i < nset ; ++i)
{
cnt[p] = 0;
counters[p] = &cnt[p];
threads[p] = new boost::thread(&MyInfo::setThreadFunc,
&info,
&ls1,
&cnt[p],
val);
val += 0.1;
p++;
}
// These threads modify the RobotState in place
for (int i = 0 ; i < nmod ; ++i)
{
cnt[p] = 0;
counters[p] = &cnt[p];
threads[p] = new boost::thread(&MyInfo::modifyThreadFunc,
&info,
&ls1,
&cnt[p],
val);
val += 0.1;
p++;
}
ASSERT_EQ(p, num);
counters[p] = NULL;
// this thread waits for all the other threads to make progress, then stops
// everything.
boost::thread wthread(&MyInfo::waitThreadFunc,
&info,
&ls1,
counters,
1000);
// wait for all threads to finish
for (int i = 0 ; i < p ; ++i)
{
threads[i]->join();
wthread.join();
}
// clean up
for (int i = 0 ; i < p ; ++i)
{
delete threads[i];
}
delete[] cnt;
delete[] counters;
delete[] threads;
}
int main(int argc, char** argv)
{
HeapInitialize();
for (int i=0; i<argc; i++)
{
if (strcmp(argv[i], "-verbose")==0 || strcmp(argv[i], "--verbose")==0)
gbVerifyVerbose = true;
}
// Tests
{
Verify_Step("ls1(`Test`)");
CEStr ls1(L"Test");
Verify0((ls1.ms_Val && 0==wcscmp(ls1.ms_Val,L"Test")),"ls1==`Test`");
Verify_Step("ls12 = ls1.Detach()");
/* Store ptr for Verify test result */ LPCWSTR pszPtr = ls1.ms_Val;
CEStr ls2 = ls1.Detach();
Verify2((ls2.ms_Val && !ls1.ms_Val && ls2.ms_Val==pszPtr),"ls2.ms_Val{x%p}==pszPtr{x%p}",ls2.ms_Val,pszPtr);
}
{
Verify_Step("ls3 = `Test3`");
CEStr ls3 = L"Test3";
Verify0((ls3.ms_Val && 0==wcscmp(ls3.ms_Val,L"Test3")),"ls3==`Test3`");
Verify_Step("ls4 = (LPCWSTR)ls3.ms_Val");
CEStr ls4 = static_cast<LPCWSTR>(ls3.ms_Val);
Verify2((ls4.ms_Val && ls4.ms_Val != ls3.ms_Val),"ls4.ms_Val{x%p}!=ls3.ms_Val{x%p}",ls4.ms_Val,ls3.ms_Val);
Verify_Step("ls5 = lstrdup(ls3)");
CEStr ls5 = lstrdup(ls3);
Verify0((ls5.ms_Val && 0==wcscmp(ls5.ms_Val,L"Test3")),"ls5==`Test3`");
Verify_Step("ls6(lstrdup(ls3))");
CEStr ls6(lstrdup(ls3));
Verify0((ls6.ms_Val && 0==wcscmp(ls6.ms_Val,L"Test3")),"ls6==`Test3`");
}
{
Verify_Step("-new_console parser tests");
LPCWSTR pszTest = L"-new_console:a \\\"-new_console:c\\\" `-new_console:d:C:\\` -cur_console:b";
LPCWSTR pszCmp = L"\\\"-new_console:c\\\" `-new_console:d:C:\\`";
RConStartArgs arg; arg.pszSpecialCmd = lstrdup(pszTest);
arg.ProcessNewConArg();
int iCmp = lstrcmp(arg.pszSpecialCmd, pszCmp);
Verify0((iCmp==0),"arg.pszSpecialCmd==\\\"-new_console:c\\\" `-new_console:d:C:\\`");
Verify_Step("RConStartArgs::RunArgTests()");
RConStartArgs::RunArgTests();
Verify0(!gbVerifyFailed,"RConStartArgs tests passed");
}
{
Verify_Step("msprintf tests");
wchar_t szBuffer[200];
msprintf(szBuffer, countof(szBuffer), L"%u %03u %03u %i %x %02X %02X %04x %08X",
123, 98, 4567, -234, 0x12AB, 0x0A, 0xABC, 0x01A0, 0x0765ABCD);
const wchar_t szStd[] = L"123 098 4567 -234 12ab 0A ABC 01a0 0765ABCD";
int iCmp = lstrcmp(szBuffer, szStd);
WVerify2((iCmp==0),L"`%s` (msprintf[W])\n `%s` (standard)", szBuffer, szStd);
char szBufA[200];
msprintf(szBufA, countof(szBufA), "%u %i %x %02X %02X %04x %08X",
123, -234, 0x12AB, 0x0A, 0xABC, 0x01A0, 0x0765ABCD);
const char szStdA[] = "123 -234 12ab 0A ABC 01a0 0765ABCD";
iCmp = lstrcmpA(szBufA, szStdA);
Verify2((iCmp==0),"`%s` (msprintf[A])\n `%s` (standard)", szBufA, szStdA);
}
if (gbVerifyFailed)
Verify_MsgFail("Some tests failed!");
else
Verify_MsgOk("All done");
return gbVerifyFailed ? 99 : 0;
}
void dir2ls(PCStr(dirpath),FileStat *stp,PCStr(opt),xPVStr(fmt),FILE *fp)
{ const char *op;
const char *dp;
int onow;
int flags;
char sortspec;
LsArg lsa;
CStr(fmt_s,256);
CStr(fmt_a,256);
CStr(line,2048);
int f_l,f_s;
CStr(fmt_b,256);
CStr(spath,1024);
const char *vbase = 0;
CStr(vbaseb,1024);
CStr(xpath,1024);
if( pathnorm("sort_ls",dirpath,AVStr(xpath)) ){
dirpath = xpath;
}
if( opt == NULL )
opt = "";
bzero(&lsa,sizeof(lsa));
lsa.l_tfmt = getenv("LSTFMT");
flags = 0;
for( op = opt; *op; op++ ){
switch( *op ){
case 'a':
lsa.l_all = 1;
break;
case 'l':
flags |= O_FORM_L;
break;
case 's':
flags |= O_FORM_S;
break;
case 'L':
lsa.l_reflink = 1;
break;
case 'V':
lsa.l_virtual = 1;
break;
case 'd':
flags |= O_PUTSELF;
break;
case 't':
flags |= O_TIMESORT;
break;
case 'z':
flags |= O_SIZESORT;
break;
case 'u':
flags |= O_BYATIME;
break;
case '*':
flags |= O_REXP;
break;
case '/':
flags |= O_VBASE;
vbase = op + 1;
while( op[1] ) *op++; /* skip remaining */
if( strpbrk(vbase,"*?[") ){
/* vbase is "const" */
strcpy(vbaseb,vbase);
vbase = vbaseb;
if( dp = strrchr(vbase,'/') )
((char*)dp)[1] = 0;
else Xstrcpy(QVStr((char*)vbase,vbaseb),"");
}
break;
}
}
if( (DoFileRexp || (flags & O_REXP)) && strpbrk(dirpath,"*?[") ){
strcpy(spath,dirpath);
dirpath = spath;
if( dp = strrchr(dirpath,'/') ){
lsa.l_maskrexp = frex_create(dp + 1);
truncVStr(dp);
}else{
lsa.l_maskrexp = frex_create(dirpath);
dirpath = ".";
}
}
if( fmt == NULL ){
cpyQStr(fmt,fmt_b);
setVStrEnd(fmt,0);
if( flags & O_FORM_S )
strcat(fmt,"%4K ");
if( flags & O_FORM_L )
strcat(fmt,"%T%M %2L %-8O %8S %D ");
/*
strcat(fmt,"%T%M%3L %-8O %8S %D ");
*/
if( flags & O_VBASE )
strcat(fmt,"%V");
else strcat(fmt,"%N");
}
if( flags & O_TIMESORT ){
if( flags & O_BYATIME )
sortspec = 'a';
else sortspec = 'm';
sprintf(fmt_s,"%%%c%s",sortspec,fmt);
fmt = fmt_s;
}
if( flags & O_SIZESORT ){
sprintf(fmt_s,"%%%c%s",'z',fmt);
fmt = fmt_s;
}
if( flags & O_BYATIME ){
strcpy(fmt_a,fmt);
fmt = fmt_a;
if( dp = strstr(fmt,"%D") )
((char*)dp)[1] = 'U';
}
lsa.l_dir = dirpath;
lsa.l_vbase = (char*)vbase;
lsa.l_stp = stp;
lsa.l_fmt = (char*)fmt;
lsa.l_out = fp;
setQStr(lsa.l_buf,line,sizeof(line));
line[0] = 0;
onow = TIME_NOW;
TIME_NOW = time(0);
setpwent();
if( !(flags & O_PUTSELF) && fileIsdir(dirpath) )
sort_ls(dirpath,&lsa);
else{
lsa.l_dir = "";
lsa.l_all = 1;
ls1(dirpath,&lsa);
fprintf(fp,"%s\r\n",lsa.l_buf);
}
endpwent();
TIME_NOW = onow;
if( lsa.l_maskrexp )
frex_free(lsa.l_maskrexp);
return;
}
