vector<double> JointCylindrical::getViolation()
{
vector<double> out;
MatNxN p12 = getConstraintViolationP12();
MatNxN p22 = getConstraintViolationP22();
out.push_back(p12(0,0));
out.push_back(p12(1,0));
out.push_back(p22(0,0));
out.push_back(p22(1,0));
return out;
}
TEST(test9, tests)
{
cg::point_2 p11(0, 0);
cg::point_2 p12(10, 10);
cg::point_2 p21(10, 0);
cg::point_2 p22(0, 10);
cg::segment_2d s1(p11, p12);
cg::segment_2d s2(p21, p22);
ASSERT_EQ(segment_intersect(s1, s2), true);
}
TEST(test8, tests)
{
cg::point_2 p11(1, 1);
cg::point_2 p12(5, 1);
cg::point_2 p21(3, -1);
cg::point_2 p22(3, 0);
cg::segment_2d s1(p11, p12);
cg::segment_2d s2(p21, p22);
ASSERT_EQ(segment_intersect(s1, s2), false);
}
TEST(test6, tests)
{
cg::point_2 p11(1, 1);
cg::point_2 p12(5, 1);
cg::point_2 p21(3, 1);
cg::point_2 p22(3, 4);
cg::segment_2d s1(p11, p12);
cg::segment_2d s2(p21, p22);
ASSERT_EQ(segment_intersect(s1, s2), true);
}
TEST(test13, tests)
{
cg::point_2 p11(0, 0);
cg::point_2 p12(10, 10);
cg::point_2 p21(11, 11);
cg::point_2 p22(12, 12);
cg::segment_2d s1(p11, p12);
cg::segment_2d s2(p21, p22);
ASSERT_EQ(segment_intersect(s1, s2), false);
}
TEST(test7, tests)
{
cg::point_2 p11(20, 10);
cg::point_2 p12(10, 20);
cg::point_2 p13(20, 00);
cg::point_2 p21(0, 100);
cg::point_2 p22(40, 20);
cg::triangle_2d tr(p11, p12, p13);
cg::segment_2d ss(p21, p22);
ASSERT_EQ(triangle_segment_intersect(tr, ss), false);
}
TEST(test1, tests)
{
cg::point_2 p11(0, 0);
cg::point_2 p12(10, 0);
cg::point_2 p13(10, 10);
cg::point_2 p21(3, 3);
cg::point_2 p22(3, 100);
cg::triangle_2d tr(p11, p12, p13);
cg::segment_2d ss(p21, p22);
ASSERT_EQ(triangle_segment_intersect(tr, ss), true);
}
void CRectangle::getchargeL()
{
ptbegin.clear();
ptend.clear();
CVector v1=ptsEnd;
CVector vrx;vrx.getcoorx();
CVector vry;vry.getcoory();
CVector vrz;vrz.getcoorz();
CVector vro;vro.getcooro();
CVector v2=ptbvir.back();
CVector v3=v1-v2;
vrx=vrx*(v3.dot(vrx));
vry=vry*(v3.dot(vry));
CVector v3x=v2+vrx;
CVector v3y=v2+vry;//(v3.dot(vry));
CVector e=v3x+vry;//(v3.dot(vry));
CVector ve1(v1.x,v2.y);
CVector vb1(v2.x,v1.y);
CPoint p11(ptsEnd);
CPoint p22(ptsBegin);
CVector vp11(p11.x,p11.y);
ptbegin.push_back(v2);
ptbegin.push_back(v3x);//p1);
ptbegin.push_back(e);//ptevir.back());
ptbegin.push_back(v3y);//p3);
ptend.push_back(v3x);//p1);
ptend.push_back(e);//ptevir.back());
ptend.push_back(v3y);//p3);
ptend.push_back(v2);
// bez=new CBezier(this);
updatecore();
}
bica::ShapeList
EKFLocalization::getGrDebugAbs()
{
shapeListAbs.clear();
pthread_mutex_lock(&mutex);
{
Point2D p2d, p22;
p2d.x = (float)ekf.filter->x();
p2d.y = (float)ekf.filter->y();
p22.x = (float)ekf.filter->x() + 600.0 * cos(ekf.filter->t());
p22.y = (float)ekf.filter->y() + 600.0 * sin(ekf.filter->t());
bica::Point3DPtr src(new bica::Point3D);
bica::Point3DPtr dst(new bica::Point3D);
bica::ArrowPtr a(new bica::Arrow);
//cerr<<"---> ("<<(*it)->x()<<", "<<(*it)->y()<<", "<<(*it)->t()<<")"<<endl;
src->x = (float)ekf.filter->x();
src->y = (float)ekf.filter->y();
src->z = 600.0;
dst->x = (float)ekf.filter->x() + 600.0 * cos(ekf.filter->t());
dst->y = (float)ekf.filter->y() + 600.0 * sin(ekf.filter->t());
dst->z = 600.0;
a->src = src;
a->dst = dst;
a->width = 10.0;
a->color = bica::BLUE;
a->filled = true;
a->opacity = 255;
a->accKey = "c";
a->label = "KFE";
shapeListAbs.push_back(a);
bica::EllipsePtr e(new bica::Ellipse);
bica::Point3DPtr c(new bica::Point3D);
c->x = (float)ekf.filter->x();
c->y = (float)ekf.filter->y();
c->z = 550;
e->center = c;
GaussianDistribution2D distrib;
distrib.mean[0] = ekf.filter->x();
distrib.mean[1] = ekf.filter->y();
distrib.covariance[0][0] = ekf.filter->get_P()->e(0, 0);
distrib.covariance[0][1] = ekf.filter->get_P()->e(0, 1);
distrib.covariance[1][0] = ekf.filter->get_P()->e(1, 0);
distrib.covariance[1][1] = ekf.filter->get_P()->e(1, 1);
double va1, va2;
Vector2D_BH<double> ve1, ve2;
double x,y;
distrib.getEigenVectorsAndEigenValues(ve1, ve2, va1, va2);
double angle;
angle = atan2(ve1[1], ve1[0]);
if(ekf.filter->get_P()->e(0, 0)<ekf.filter->get_P()->e(1, 1))
angle = normalizePi(angle+pi_2);
e->width = (float)sqrt(va1);
e->length = (float)sqrt(va2);
e->angle = (float)angle;
e->color = bica::WHITE;
e->filled = true;
e->opacity = 125;
e->accKey = "c";
e->label = "KFEE";
shapeListAbs.push_back(e);
}
//cerr<<endl;
for(int i=0; i<mgrid->getRows(); i++)
for(int j=0; j<mgrid->getColumns(); j++)
{
bica::RectanglePtr a(new bica::Rectangle);
bica::Point3DPtr p2d(new bica::Point3D);
bica::Point3DPtr p22(new bica::Point3D);
p2d->x=(i*CELL_SIZE)-3000.0;
p2d->y=(j*CELL_SIZE)-2000.0;
p2d->z=500.0;
p22->x = ((i+1)*CELL_SIZE)-3000.0;
p22->y = ((j+1)*CELL_SIZE)-2000.0;
p22->z=500.0;
float media;
media = 1.0/((float)mgrid->getRows()*(float)mgrid->getColumns());
float value = (((mgrid->getProbIJ(i,j)/media)*255.0))/2.0;
float p = mgrid->getProbIJ(i,j);
if (p>0.01)
value = (p/0.02)*255.0;
else
value = 0.0;
//value = mgrid->getProbIJ(i,j);
if (value>255.0f)
value = 255.0f;
//cerr<<"("<<p2d->x<<", "<<p2d->y<<"," <<value<<") ";
a->p1 = p2d;
a->p2 = p22;
a->color = bica::RED;
a->filled = true;
a->opacity = value;
a->accKey = "c";
a->label = "KF";
shapeListAbs.push_back(a);
bica::Point3DPtr src(new bica::Point3D);
bica::Point3DPtr dst(new bica::Point3D);
bica::ArrowPtr r(new bica::Arrow);
float cx, cy;
cx = (((((float)i)*CELL_SIZE)-3000.0) + ((((float)(i+1))*CELL_SIZE)-3000.0))/2.0;
cy = (((((float)j)*CELL_SIZE)-2000.0) + ((((float)(j+1))*CELL_SIZE)-2000.0))/2.0;
src->x = cx;
src->y = cy;
src->z = 600.0;
dst->x = cx + (CELL_SIZE/2.0) * cos(mgrid->getThetaIJ(i, j));
dst->y = cy + (CELL_SIZE/2.0) * sin(mgrid->getThetaIJ(i, j));
dst->z = 600.0;
r->src = src;
r->dst = dst;
r->width = 10.0;
r->color = bica::BLUE;
r->filled = true;
r->opacity = 255;
r->accKey = "c";
r->label = "KFE";
shapeListAbs.push_back(r);
}
//cerr<<endl;
pthread_mutex_unlock(&mutex);
return shapeListAbs;
}
bool CRectangle::getCharge()
{
if (cancharge)
{
ptbegin.clear();
ptend.clear();
CVector v1=ptevir.back();
CVector vrx;vrx.getcoorx();
CVector vry;vry.getcoory();
CVector vrz;vrz.getcoorz();
CVector vro;vro.getcooro();
CVector v2=ptbvir.back();
CVector v3=v1-v2;
vrx=vrx*(v3.dot(vrx));
vry=vry*(v3.dot(vry));
CVector v3x=v2+vrx;
CVector v3y=v2+vry;//(v3.dot(vry));
CVector e=v3x+vry;//(v3.dot(vry));
CVector ve1(v1.x,v2.y);
CVector vb1(v2.x,v1.y);
CPoint p11(ptevir.back());
CPoint p22(ptbvir.back());
CVector vp11(p11.x,p11.y);
ptbegin.push_back(v2);
ptbegin.push_back(v3x);//p1);
ptbegin.push_back(e);//ptevir.back());
ptbegin.push_back(v3y);//p3);
ptend.push_back(v3x);//p1);
ptend.push_back(e);//ptevir.back());
ptend.push_back(v3y);//p3);
ptend.push_back(v2);
// updatecore();
box.empty();
box.add(v2);
box.add(v3x);
box.add(e);
box.add(v3y);
mesh.empty();
CVector uv=box.getParam(ptbegin[0]);
mesh.addVertex(new CVertex(ptbegin[0],uv.x,uv.y));
uv=box.getParam(ptbegin[1]);
mesh.addVertex(new CVertex(ptbegin[1],uv.x,uv.y));
new CEdge(mesh.Vertex(mesh.Vcount()-2),mesh.Vertex(mesh.Vcount()-1),mesh.Ecount());
uv=box.getParam(ptbegin[2]);
mesh.addVertex(new CVertex(ptbegin[2],uv.x,uv.y));
new CEdge(mesh.Vertex(mesh.Vcount()-2),mesh.Vertex(mesh.Vcount()-1),mesh.Ecount());
uv=box.getParam(ptbegin[3]);
mesh.addVertex(new CVertex(ptbegin[3],uv.x,uv.y));
new CEdge(mesh.Vertex(mesh.Vcount()-2),mesh.Vertex(mesh.Vcount()-1),mesh.Ecount());
new CEdge(mesh.Vertex(mesh.Vcount()-1),mesh.Vertex(mesh.Vcount()-4),mesh.Ecount());
// bez=new CBezier(this);
return cancharge;
}
else
{
return false;
}
}
TEST(FloatRectTest, SquaredDistanceToTest)
{
//
// O--x
// |
// y
//
// FloatRect.x() FloatRect.maxX()
// | |
// 1 | 2 | 3
// ======+==========+====== --FloatRect.y()
// 4 | 5(in) | 6
// ======+==========+====== --FloatRect.maxY()
// 7 | 8 | 9
//
FloatRect r1(100, 100, 250, 150);
// `1` case
FloatPoint p1(80, 80);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p1), 800.f);
FloatPoint p2(-10, -10);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p2), 24200.f);
FloatPoint p3(80, -10);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p3), 12500.f);
// `2` case
FloatPoint p4(110, 80);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p4), 400.f);
FloatPoint p5(150, 0);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p5), 10000.f);
FloatPoint p6(180, -10);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p6), 12100.f);
// `3` case
FloatPoint p7(400, 80);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p7), 2900.f);
FloatPoint p8(360, -10);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p8), 12200.f);
// `4` case
FloatPoint p9(80, 110);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p9), 400.f);
FloatPoint p10(-10, 180);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p10), 12100.f);
// `5`(& In) case
FloatPoint p11(100, 100);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p11), 0.f);
FloatPoint p12(150, 100);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p12), 0.f);
FloatPoint p13(350, 100);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p13), 0.f);
FloatPoint p14(350, 150);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p14), 0.f);
FloatPoint p15(350, 250);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p15), 0.f);
FloatPoint p16(150, 250);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p16), 0.f);
FloatPoint p17(100, 250);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p17), 0.f);
FloatPoint p18(100, 150);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p18), 0.f);
FloatPoint p19(150, 150);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p19), 0.f);
// `6` case
FloatPoint p20(380, 150);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p20), 900.f);
// `7` case
FloatPoint p21(80, 280);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p21), 1300.f);
FloatPoint p22(-10, 300);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p22), 14600.f);
// `8` case
FloatPoint p23(180, 300);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p23), 2500.f);
// `9` case
FloatPoint p24(450, 450);
EXPECT_PRED_FORMAT2(GeometryTest::AssertAlmostEqual, r1.squaredDistanceTo(p24), 50000.f);
}
cv::Mat TestProjection::test(double userX, double userY, double userZ,
const char* filename) {
//Coordinates of the projection in the real world
/*cv::Point3f p11(-480, 735, -420);
cv::Point3f p12(0, 935, 0);
cv::Point3f p13(0, 220, 0);
cv::Point3f p14(-480, 240, -420);
Plane3d proj1(p11, p12, p13, p14);
cv::Point3f p21(0, 935, 0);
cv::Point3f p22(480, 735, -420);
cv::Point3f p23(480, 240, -420);
cv::Point3f p24(0, 220, 0);
Plane3d proj2(p21, p22, p23, p24);*/
cv::Point3f p11(-590, 725, -350);
cv::Point3f p12(0, 955, 0);
cv::Point3f p13(0, 200, 0);
cv::Point3f p14(-590, 227, -350);
Plane3d proj1(p11, p12, p13, p14);
cv::Point3f p21(0, 955, 0);
cv::Point3f p22(567, 755, -350);
cv::Point3f p23(567, 227, -350);
cv::Point3f p24(0, 200, 0);
Plane3d proj2(p21, p22, p23, p24);
std::vector<Plane3d> planes;
planes.push_back(proj1);
planes.push_back(proj2);
Projection proj(planes);
// proj.print();
//Create the user with the obtained projection coordinates
User u(proj);
//Update his position
u.updatePosition(userX, userY, userZ);
// u.print();
//Create the distorted-corrected plane pairs, using the projections
//on the user's view plane
//Plane 1
//****************************************************************************************************
Plane2d p1 = u.getProjectedPlanes().at(0).to2d();
Plane2d p2(cv::Point2f(0, 0), cv::Point2f(480, 0), cv::Point2f(480, 540), cv::Point2f(0, 540));
// Plane2d p2(cv::Point2f(0, 0), cv::Point2f(230, 0), cv::Point2f(230, 520), cv::Point2f(0, 520));
// Plane2d p2(cv::Point2f(0, 0), cv::Point2f(270, 0), cv::Point2f(270, 405), cv::Point2f(0, 405));
//****************************************************************************************************
//Invert the plane y coordinates
Plane2d inv1 = p1.yInverted();
//Move it so that it's closer to the target plane
cv::Vec2f dist = pjs::distance(inv1, p2);
Plane2d pp1(cv::Point2f(inv1.getPoint(0).x - dist[0], inv1.getPoint(0).y - dist[1]),
cv::Point2f(inv1.getPoint(1).x - dist[0], inv1.getPoint(1).y - dist[1]),
cv::Point2f(inv1.getPoint(2).x - dist[0], inv1.getPoint(2).y - dist[1]),
cv::Point2f(inv1.getPoint(3).x - dist[0], inv1.getPoint(3).y - dist[1]));
//Plane 2
//****************************************************************************************************
Plane2d p3 = u.getProjectedPlanes().at(1).to2d();
Plane2d p4(cv::Point2f(0, 0), cv::Point2f(480, 0), cv::Point2f(480, 540), cv::Point2f(0, 540));
// Plane2d p4(cv::Point2f(0, 0), cv::Point2f(230, 0), cv::Point2f(230, 520), cv::Point2f(0, 520));
// Plane2d p4(cv::Point2f(0, 0), cv::Point2f(270, 0), cv::Point2f(270, 405), cv::Point2f(0, 405));
//****************************************************************************************************
//Invert the plane y coordinates
Plane2d inv2 = p3.yInverted();
//Move it so that it's closer to the target plane
dist = pjs::distance(inv2, p4);
Plane2d pp3(cv::Point2f(inv2.getPoint(0).x - dist[0], inv2.getPoint(0).y - dist[1]),
cv::Point2f(inv2.getPoint(1).x - dist[0], inv2.getPoint(1).y - dist[1]),
cv::Point2f(inv2.getPoint(2).x - dist[0], inv2.getPoint(2).y - dist[1]),
cv::Point2f(inv2.getPoint(3).x - dist[0], inv2.getPoint(3).y - dist[1]));
//***********************
//Load the target image
//***********************
cv::Mat img = cv::imread(filename, CV_LOAD_IMAGE_COLOR);
if (!img.data) {
std::cout << " --(!) Error reading image" << std::endl;
throw std::exception();
}
//Helper object
Utils utils;
//Divide the image in two
// std::vector<cv::Mat> images = utils.divideImageInTwo(img);
//Build the surfaces with their reference planes and their corresponding
//image
Surface s1(pp1, p2);
Surface s2(pp3, p4);
std::vector<Surface*> surfaces;
surfaces.push_back(&s1);
surfaces.push_back(&s2);
int originX;
int padding;
int screenWidth = 1280;
int screenHeight = 800;
//TODO recursive position correction
int width1 = s1.getWidth();
int width2 = s2.getWidth();
int diffW = width1 - width2;
if (diffW < 0) {
originX = screenWidth / 2 - width1;
padding = 0;
} else {
originX = 0 + screenWidth / 2 - width1;
padding = 0;
}
//1st position correction
cv::Point2f origin(originX, 0);
s1.correctBBPosition(origin);
cv::Point2f s1ur = s1.getUpperRightCorner();
s2.correctPosition(s1ur);
cv::Point2f upperLeft = s2.getUpperLeftCorner();
cv::Point2f upperRight = s2.getUpperRightCorner();
double topY;
if (upperLeft.y < upperRight.y) {
topY = upperLeft.y;
} else {
topY = upperRight.y;
}
cv::Size size = utils.getFinalSize(surfaces);
int diffH = screenHeight - size.height;
//2nd position correction if necessary (if second plane is still outside)
if (!topY < 0) {
topY = 0;
}
cv::Point2f newOrigin(originX, -topY + diffH / 2);
s1.correctBBPosition(newOrigin);
s1ur = s1.getUpperRightCorner();
s2.correctPosition(s1ur);
// cv::Size size = utils.getFinalSize(surfaces);
size.width += padding;
size.width = std::max(screenWidth, size.width);
size.height = screenHeight;
cv::Size sizeS1(size.width / 2, size.height);
s1.setSize(sizeS1);
s2.setSize(size);
std::vector<cv::Mat> images = utils.divideImageInTwo(img);
s1.setImage(images.at(0));
s2.setImage(images.at(1));
s1.applyHomography();
s2.applyHomography();
// s1.addTransparency();
// s2.addTransparency();
cv::Mat finalImage = utils.getImageFromSurfaces(surfaces);
surfaces.clear();
return finalImage;
}
BezTreeNode::BezTreeNode(int _lv, float us, float ue, float vs, float ve,
BezTree* _tree, BezTreeNode* p, int* _idx)
: lv(_lv), u_s(us), u_e(ue), v_s(vs), v_e(ve)
{
tree = _tree;
parent = p;
child[0] = child[1] = child[2] = child[3] = NULL;
memcpy(idx, _idx, sizeof(int) * 9);
//const double Height = static_cast<double>(GeoCorrection::gridY) - 1;
// compute bezier control points
Vector2f p00(tree->transformedPoints[idx[0]].at<double>(0, 0), tree->transformedPoints[idx[0]].at<double>(1, 0)),
p02(tree->transformedPoints[idx[2]].at<double>(0, 0), tree->transformedPoints[idx[2]].at<double>(1, 0)),
p01 = estimateControlPoint(p00, p02, cv::Point2f(tree->transformedPoints[idx[1]].at<double>(0, 0), tree->transformedPoints[idx[1]].at<double>(1, 0)), 0.5),
p20(tree->transformedPoints[idx[6]].at<double>(0, 0), tree->transformedPoints[idx[6]].at<double>(1, 0)),
p22(tree->transformedPoints[idx[8]].at<double>(0, 0), tree->transformedPoints[idx[8]].at<double>(1, 0)),
p21 = estimateControlPoint(p20, p22, cv::Point2f(tree->transformedPoints[idx[7]].at<double>(0, 0), tree->transformedPoints[idx[7]].at<double>(1, 0)), 0.5),
p10 = estimateControlPoint(p00, p20, cv::Point2f(tree->transformedPoints[idx[3]].at<double>(0, 0), tree->transformedPoints[idx[3]].at<double>(1, 0)), 0.5),
p12 = estimateControlPoint(p02, p22, cv::Point2f(tree->transformedPoints[idx[5]].at<double>(0, 0), tree->transformedPoints[idx[5]].at<double>(1, 0)), 0.5),
p11 = estimateControlPoint(p10, p12, cv::Point2f(tree->transformedPoints[idx[4]].at<double>(0, 0), tree->transformedPoints[idx[4]].at<double>(1, 0)), 0.5);
Vector2f glProjPoint0(tree->projPoints[idx[0]].x, tree->projPoints[idx[0]].y),
glProjPoint1(tree->projPoints[idx[1]].x, tree->projPoints[idx[1]].y),
glProjPoint2(tree->projPoints[idx[2]].x, tree->projPoints[idx[2]].y),
glProjPoint3(tree->projPoints[idx[3]].x, tree->projPoints[idx[3]].y),
glProjPoint4(tree->projPoints[idx[4]].x, tree->projPoints[idx[4]].y),
glProjPoint5(tree->projPoints[idx[5]].x, tree->projPoints[idx[5]].y),
glProjPoint6(tree->projPoints[idx[6]].x, tree->projPoints[idx[6]].y),
glProjPoint7(tree->projPoints[idx[7]].x, tree->projPoints[idx[7]].y),
glProjPoint8(tree->projPoints[idx[8]].x, tree->projPoints[idx[8]].y);
if (p == nullptr)
{
// actual bezier control points, need to differntiate delta
/*surface = new QuadBezierPatch2f(2.0f*glProjPoint0 - p00,
2.0f*glProjPoint1 - p01,
2.0f*glProjPoint2 - p02,
2.0f*glProjPoint3 - p10,
2.0f*glProjPoint4 - p11,
2.0f*glProjPoint5 - p12,
2.0f*glProjPoint6 - p20,
2.0f*glProjPoint7 - p21,
2.0f*glProjPoint8 - p22
);*/
surface = new QuadBezierPatch2f(glProjPoint0 ,
2.0f*glProjPoint1 - p01,
glProjPoint2,
2.0f*glProjPoint3 - p10,
2.0f*glProjPoint4 - p11,
2.0f*glProjPoint5 - p12,
glProjPoint6,
2.0f*glProjPoint7 - p21,
glProjPoint8);
}
else
{
std::cout << "subdivision above 0th level" << std::endl;
size_t row = sqrt(p->bezPatch.size());
surface = new QuadBezierPatch2f(p->bezPatch[idx[0]],
2.0*glProjPoint1 - p01,
p->bezPatch[idx[2]],
2.0*glProjPoint3 - p10,
2.0*glProjPoint4 - p11,
2.0*glProjPoint5 - p12,
p->bezPatch[idx[6]],
2.0*glProjPoint7 - p21,
p->bezPatch[idx[8]]
);
}
// generate vertices and mesh indices
surface->interpolate(bezPatch, bezPatchIdx, BernsVal, 0, BernsVal.size());
}
int main()
{
Point p1(-253.357, -123.36);
Point p2(-190.03, 216.606);
Point p3(-343.349, 286.6);
Point p4(141.604, 279.934);
Point p5(276.591, -46.7012);
Point p6(251.593, -263.347);
Point p7(-3.38184, -343.339);
Point p8(-380.012, -173.355);
Point p9(-98.3726, 39.957);
Point p10(133.271, 124.949);
Point p11(289.923, 301.598);
Point p12(421.577, 23.292);
Point p13(79.9434, -93.3633);
Point p14(-40.0449, 366.592);
Point p15(311.587, 374.924);
Point p16(431.576, 214.94);
Point p17(426.576, -131.693);
Point p18(-265.023, -285.011);
Point p19(369.915, 89.9521);
Point p20(368.249, -15.0376);
Point p21(484.904, 18.2925);
Point p22(-411.675, 283.267);
Point p23(-250.024, 124.949);
Point p24(-80.041, -78.3647);
Point p25(-360.014, 31.6245);
Point p26(-305.019, 356.593);
// built Delaunay triangulation
PS.insert(p1); PS.insert(p2); PS.insert(p3); PS.insert(p4);
PS.insert(p5); PS.insert(p6); PS.insert(p7); PS.insert(p8);
PS.insert(p9); PS.insert(p10); PS.insert(p11); PS.insert(p12);
PS.insert(p13); PS.insert(p14); PS.insert(p15); PS.insert(p16);
PS.insert(p17); PS.insert(p18); PS.insert(p19); PS.insert(p20);
PS.insert(p21); PS.insert(p22); PS.insert(p23); PS.insert(p24);
PS.insert(p25); PS.insert(p26);
std::list<Vertex_handle> LV;
bool correct = true;
// circle emptiness check
Circle cs1(Point(-23.3799, 108.284), 1124.78);
check_empty checker(cs1);
CGAL::range_search(PS,cs1,std::back_inserter(LV),checker,true);
if (checker.get_result()) {
std::cout << "circle not empty !\n";
std::cout << "this is an error !\n"; correct=false;
}
else std::cout << "circle was empty !\n";
Circle cs2(Point(-255.024, -100.029), 23551);
check_empty checker2(cs2);
CGAL::range_search(PS,cs2,std::back_inserter(LV),checker2,true);
if (checker2.get_result()) std::cout << "circle not empty !\n";
else {
std::cout << "circle was empty !\n";
std::cout << "this is an error !\n"; correct=false;
}
// triangle check
Triangle t1(Point(-21.7134, -123.36), Point(84.9429, 74.9536), Point(209.931, -161.69));
Triangle t2(Point(-61.7095, 164.945), Point(-88.3735, 101.618), Point(49.9463, 101.618));
check_empty_triangle tchecker1(t1);
CGAL::range_search(PS,t1.vertex(0),t1.vertex(1),t1.vertex(2),std::back_inserter(LV),tchecker1,true);
if (tchecker1.get_result()) std::cout << "triangle not empty !\n";
else {
std::cout << "triangle was empty !\n";
std::cout << "this is an error !\n"; correct=false;
}
check_empty_triangle tchecker2(t2);
CGAL::range_search(PS,t2.vertex(0),t2.vertex(1),t2.vertex(2),std::back_inserter(LV),tchecker2,true);
if (tchecker2.get_result()) {
std::cout << "triangle not empty !\n";
std::cout << "this is an error !\n"; correct=false;
}
else std::cout << "triangle was empty !\n";
// rectangle check
Rectangle_2 r1(-290.021, -175.022, -125.037, -35.0356);
Rectangle_2 r2(-48.3774, 136.614, -23.3799, 251.603);
check_empty_rectangle rchecker1(r1);
CGAL::range_search(PS,r1.vertex(0),r1.vertex(1),r1.vertex(2),r1.vertex(3),std::back_inserter(LV),rchecker1,true);
if (rchecker1.get_result()) std::cout << "rectangle not empty !\n";
else {
std::cout << "rectangle was empty !\n";
std::cout << "this is an error !\n"; correct=false;
}
check_empty_rectangle rchecker2(r2);
CGAL::range_search(PS,r2.vertex(0),r2.vertex(1),r2.vertex(2),r2.vertex(3),std::back_inserter(LV),rchecker2,true);
if (rchecker2.get_result()) {
std::cout << "rectangle not empty !\n";
std::cout << "this is an error !\n"; correct=false;
}
else std::cout << "rectangle was empty !\n";
if (correct) return 0;
return 1;
}
void CContainers::prepareMemBuffers()
{
memout=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p("!data",memout);
memmap.insert(p);
memout_words=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p2("!!!words",memout_words);
memmap.insert(p2);
memout_letters=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p3("!!letters",memout_letters);
memmap.insert(p3);
memout_num=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p4("!num",memout_num);
memmap.insert(p4);
memout_year=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p5("!year",memout_year);
memmap.insert(p5);
memout_date=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p6("!date",memout_date);
memmap.insert(p6);
memout_words2=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p7("!!!words2",memout_words2);
memmap.insert(p7);
memout_words3=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p8("!!!words3",memout_words3);
memmap.insert(p8);
memout_words4=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p9("!!!words4",memout_words4);
memmap.insert(p9);
memout_pages=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p10("!pages",memout_pages);
memmap.insert(p10);
memout_num2=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p11("!num2",memout_num2);
memmap.insert(p11);
memout_num3=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p12("!num3",memout_num3);
memmap.insert(p12);
memout_num4=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p13("!num4",memout_num4);
memmap.insert(p13);
memout_remain=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p14("!remain",memout_remain);
memmap.insert(p14);
memout_date2=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p15("!date2",memout_date2);
memmap.insert(p15);
memout_date3=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p16("!date3",memout_date3);
memmap.insert(p16);
memout_num2b=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p17("!num2b",memout_num2b);
memmap.insert(p17);
memout_num3b=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p18("!num3b",memout_num3b);
memmap.insert(p18);
memout_num4b=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p19("!num4b",memout_num4b);
memmap.insert(p19);
memout_numb=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p20("!numb",memout_numb);
memmap.insert(p20);
memout_num2c=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p21("!num2c",memout_num2c);
memmap.insert(p21);
memout_num3c=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p22("!num3c",memout_num3c);
memmap.insert(p22);
memout_num4c=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p23("!num4c",memout_num4c);
memmap.insert(p23);
memout_numc=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p24("!numc",memout_numc);
memmap.insert(p24);
memout_time=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p25("!time",memout_time);
memmap.insert(p25);
memout_remain2=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p26("!remain2",memout_remain2);
memmap.insert(p26);
memout_ip=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p27("!ip",memout_ip);
memmap.insert(p27);
memout_hm=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p28("!hm",memout_hm);
memmap.insert(p28);
memout_hms=new CMemoryBuffer();
std::pair<std::string,CMemoryBuffer*> p29("!hms",memout_hms);
memmap.insert(p29);
}
int main(void){
TriangleCache::TriangleCache TC(50,2);
sm::Point p11(10,10);
sm::Point p12(20,20);
sm::Point p13(30,30);
sm::Point p21(40,40);
sm::Point p22(50,50);
sm::Point p23(60,60);
Triangle t1(&p11, &p12, &p13);
Triangle t2(&p21, &p22, &p23);
GNode gn1(&t1);
GNode gn2(&t2);
GNode gn3(&t2);
GNode gn4(&t2);
GNode gn5(&t1);
GNode gn6(&t1);
const GNode ** path1 = (const GNode **)calloc(3, sizeof(GNode *));
path1[0] = &gn3;
path1[1] = &gn4;
path1[2] = 0;
const GNode ** path = (const GNode **)calloc(3, sizeof(GNode *));
path[0] = &gn1;
path[1] = &gn2;
path[2] = 0;
const GNode ** path2 = (const GNode **)calloc(4, sizeof(GNode *));
path2[0] = &gn5;
path2[1] = &gn3;
path2[2] = &gn6;
path2[3] = 0;
bool f = false;
debugGREEN("TEST FIND WHEN THERE IS NOTHING\n");
const GNode **r = TC.getPath( &gn1, &gn2, f);
ASSERT(!r);
debugRED("OK... \n");
debugGREEN("TEST ADD AND SEARCH ONE ELEMENT\n");
TC.addPath( &gn1, &gn2, path);
const GNode **ret = TC.getPath( &gn1, &gn2, f);
ASSERT(ret);
ASSERT(f);
ASSERT(&gn1 == ret[0]);
ASSERT(&gn2 == ret[1]);
debugRED("OK... \n");
debugGREEN("TEST SEARCH THE SAME ELEMENT BUT IN REVERSE ORDER\n");
ASSERT(TC.getPath( &gn2, &gn1, f));
ASSERT(!f);
debugRED("OK... \n");
debugGREEN("TEST ADD 500000 MORE ELEMENTS gn2-gn1 and look for gn1-gn2 and gn2-gn1\n");
for (int i = 0; i < 50000; i++){
TC.addPath( &gn2, &gn1, path);
}
ASSERT(TC.getPath( &gn1, &gn2, f));
ASSERT(TC.getPath( &gn2, &gn1, f));
debugRED("OK... \n");
debugGREEN("TEST ADD path, then path1, then path2, and then find path 1\n");
TC.addPath( &gn1, &gn2, path);
TC.addPath( &gn3, &gn4, path1);
TC.addPath( &gn5, &gn6, path2);
ASSERT(!TC.getPath( &gn1, &gn2, f));
ASSERT(!TC.getPath( &gn2, &gn1, f));
ASSERT(TC.getPath( &gn4, &gn3, f));
ASSERT(TC.getPath( &gn3, &gn4, f));
ASSERT(TC.getPath( &gn5, &gn6, f));
ASSERT(TC.getPath( &gn6, &gn5, f));
debugRED("OK... \n");
debugGREEN("TEST CLEAR CACHE\n");
TC.clear();
ASSERT(!TC.getPath( &gn1, &gn2, f));
ASSERT(!TC.getPath( &gn2, &gn1, f));
ASSERT(!TC.getPath( &gn4, &gn3, f));
ASSERT(!TC.getPath( &gn3, &gn4, f));
ASSERT(!TC.getPath( &gn5, &gn6, f));
ASSERT(!TC.getPath( &gn6, &gn5, f));
debugRED("OK... \n");
debugGREEN("TEST REFRESHING\n");
TC.addPath( &gn2, &gn1, path);
TC.addPath( &gn3, &gn4, path1);
TC.addPath( &gn2, &gn1, path);
TC.addPath( &gn5, &gn6, path2);
ASSERT(!TC.getPath( &gn4, &gn3, f));
ASSERT(TC.getPath( &gn1, &gn2, f));
ASSERT(TC.getPath( &gn5, &gn6, f));
debugRED("OK... \n");
debugBLUE("END TESTS (ALL OK)\n");
free(path);
free(path1);
free(path2);
return 0;
}
