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;
}
void dgCollisionSphere::TesselateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* const ouput) const
{
if (level) {
dgAssert (dgAbs (p0.DotProduct(p0).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbs (p1.DotProduct(p1).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbs (p2.DotProduct(p2).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgVector p01 (p0 + p1);
dgVector p12 (p1 + p2);
dgVector p20 (p2 + p0);
//p01 = p01 * p01.InvMagSqrt();
//p12 = p12 * p12.InvMagSqrt();
//p20 = p20 * p20.InvMagSqrt();
p01 = p01.Normalize();
p12 = p12.Normalize();
p20 = p20.Normalize();
dgAssert (dgAbs (p01.DotProduct(p01).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbs (p12.DotProduct(p12).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbs (p20.DotProduct(p20).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
TesselateTriangle (level - 1, p0, p01, p20, count, ouput);
TesselateTriangle (level - 1, p1, p12, p01, count, ouput);
TesselateTriangle (level - 1, p2, p20, p12, count, ouput);
TesselateTriangle (level - 1, p01, p12, p20, count, ouput);
} else {
ouput[count ++] = p0;
ouput[count ++] = p1;
ouput[count ++] = p2;
}
}
void dgCollisionTaperedCapsule::TesselateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* ouput) const
{
if (level) {
dgAssert (dgAbsf (p0 % p0 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p1 % p1 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p2 % p2 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgVector p01 (p0 + p1);
dgVector p12 (p1 + p2);
dgVector p20 (p2 + p0);
p01 = p01.Scale3 (dgRsqrt(p01 % p01));
p12 = p12.Scale3 (dgRsqrt(p12 % p12));
p20 = p20.Scale3 (dgRsqrt(p20 % p20));
dgAssert (dgAbsf (p01 % p01 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p12 % p12 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p20 % p20 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
TesselateTriangle (level - 1, p0, p01, p20, count, ouput);
TesselateTriangle (level - 1, p1, p12, p01, count, ouput);
TesselateTriangle (level - 1, p2, p20, p12, count, ouput);
TesselateTriangle (level - 1, p01, p12, p20, count, ouput);
} else {
ouput[count + 0] = p0.Scale3 (m_radio0);
ouput[count + 1] = p1.Scale3 (m_radio0);
ouput[count + 2] = p2.Scale3 (m_radio0);
count += 3;
}
}
void dgCollisionSphere::TesselateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* ouput) const
{
if (level) {
dgAssert (dgAbsf (p0 % p0 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p1 % p1 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p2 % p2 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgVector p01 (p0 + p1);
dgVector p12 (p1 + p2);
dgVector p20 (p2 + p0);
p01 = p01.CompProduct4(p01.InvMagSqrt());
p12 = p12.CompProduct4(p12.InvMagSqrt());
p20 = p20.CompProduct4(p20.InvMagSqrt());
dgAssert (dgAbsf (p01 % p01 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p12 % p12 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p20 % p20 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
TesselateTriangle (level - 1, p0, p01, p20, count, ouput);
TesselateTriangle (level - 1, p1, p12, p01, count, ouput);
TesselateTriangle (level - 1, p2, p20, p12, count, ouput);
TesselateTriangle (level - 1, p01, p12, p20, count, ouput);
} else {
ouput[count ++] = p0;
ouput[count ++] = p1;
ouput[count ++] = p2;
}
}
void dgConvexHull4d::TessellateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgBigVector* const ouput, dgInt32& start) const
{
if (level) {
dgAssert (dgAbsf (p0 % p0 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p1 % p1 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p2 % p2 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgVector p01 (p0 + p1);
dgVector p12 (p1 + p2);
dgVector p20 (p2 + p0);
p01 = p01.Scale3 (dgRsqrt(p01 % p01));
p12 = p12.Scale3 (dgRsqrt(p12 % p12));
p20 = p20.Scale3 (dgRsqrt(p20 % p20));
dgAssert (dgAbsf (p01 % p01 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p12 % p12 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
dgAssert (dgAbsf (p20 % p20 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
TessellateTriangle (level - 1, p0, p01, p20, count, ouput, start);
TessellateTriangle (level - 1, p1, p12, p01, count, ouput, start);
TessellateTriangle (level - 1, p2, p20, p12, count, ouput, start);
TessellateTriangle (level - 1, p01, p12, p20, count, ouput, start);
} else {
dgBigPlane n (p0, p1, p2);
n = n.Scale (dgFloat64(1.0f) / sqrt (n % n));
n.m_w = dgFloat64(0.0f);
ouput[start] = n;
start += 8;
count ++;
}
}
TEST(test8, tests)
{
cg::point_2 p11(10, 10);
cg::point_2 p12(0, 0);
cg::point_2 p13(20, 5);
cg::point_2 p2(6, 5);
cg::triangle_2d tr(p11, p12, p13);
ASSERT_EQ(point_inside_triangle(tr, p2), true);
}
TEST(test5, tests)
{
cg::point_2 p11(0, 0);
cg::point_2 p12(10, 0);
cg::point_2 p13(10, 10);
cg::point_2 p2(-10, -10);
cg::triangle_2d tr(p11, p12, p13);
ASSERT_EQ(point_inside_triangle(tr, p2), false);
}
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(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(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(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);
}
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);
}
static void decasteljau(double tol, std::vector<sortedPoint> &discrete, int pos,
const SPoint3 &p0, const SPoint3 &p1, const SPoint3 &p2,
double t0, double t2)
{
if(sqDistPointSegment(p1, p0, p2) < tol * tol)
return;
SPoint3 p01((p0 + p1) * 0.5);
SPoint3 p12((p1 + p2) * 0.5);
SPoint3 p012((p01 + p12) * 0.5);
double t012 = 0.5 * (t0 + t2);
int newpos = sortedPointInsert(p012, t012, discrete, pos);
decasteljau(tol, discrete, pos, p0, p01, p012, t0, t012);
decasteljau(tol, discrete, newpos, p012, p12, p2, t012, t2);
}
Vector getFrameNormal(const door_msgs::Door& door)
{
// normal on frame
Vector p12(door.frame_p1.x-door.frame_p2.x, door.frame_p1.y-door.frame_p2.y, door.frame_p1.z-door.frame_p2.z);
p12.Normalize();
Vector z_axis(0,0,1);
Vector normal = p12 * z_axis;
// make normal point in direction we travel through door
Vector dir(door.travel_dir.x, door.travel_dir.y, door.travel_dir.z);
if (dot(dir, normal) < 0)
normal = normal * -1.0;
return normal;
}
int main(int argc, char *argv[])
{
QGuiApplication app(argc, argv);
QQmlApplicationEngine engine;
Player p1("Emma", 4);
Player p2("Emma", 4);
Player p3("Emma", 4);
Player p4("Emma", 4);
Player p5("Emma", 4);
Player p6("Emma", 4);
Player p7("Emma", 4);
Player p8("Emma", 4);
Player p9("Emma", 4);
Player p10("Emma", 4);
Player p11("Emma", 4);
Player p12("Emma", 4);
Match m(3);
engine.rootContext()->setContextProperty("player1", &p1);
engine.rootContext()->setContextProperty("player2", &p2);
engine.rootContext()->setContextProperty("player3", &p3);
engine.rootContext()->setContextProperty("player4", &p4);
engine.rootContext()->setContextProperty("player5", &p5);
engine.rootContext()->setContextProperty("player6", &p6);
engine.rootContext()->setContextProperty("player7", &p7);
engine.rootContext()->setContextProperty("player8", &p8);
engine.rootContext()->setContextProperty("player9", &p9);
engine.rootContext()->setContextProperty("player10", &p10);
engine.rootContext()->setContextProperty("player11", &p11);
engine.rootContext()->setContextProperty("player12", &p12);
engine.rootContext()->setContextProperty("match", &m);
QCoreApplication::setApplicationName("Lab2");
QCoreApplication::setOrganizationName("EMMA");
QCoreApplication::setOrganizationDomain(".fourThompson");
engine.load(QUrl(QStringLiteral("qrc:/main.qml")));
return app.exec();
}
short orient3d_triangle (const GenericPointT& p1,
const GenericPointT& p2,
const GenericPointT& p3,
const GenericPointT& ptn )
{
// possible bounding box check
// [RH] TODO
//
typedef GenericPointT Vector_t;
Vector_t p12(p2 - p1);
Vector_t p13(p3 - p1);
Vector_t useNormal = p12.ex(p13);
useNormal.normalize();
short ori1, ori2;
// Test line p1 - p2
GenericPointT temppoint (p1[0] + useNormal[0],
p1[1] + useNormal[1],
p1[2] + useNormal[2]);
SwkMatrix3x3<GenericPointT, double> swk_matrix1 (p1, p2, temppoint, ptn);
SwkMatrix3x3<GenericPointT, double> swk_matrix2 (p1, p2, temppoint, p3);
ori1 = swk_matrix1.orient3D_short();
ori2 = swk_matrix2.orient3D_short();
//std::cout << "p12: ori1: " << ori1 << std::endl;
//std::cout << "p12: ori2: " << ori2 << std::endl;
if (ori1 == RH_ON || ori2 == RH_ON)
return RH_ON;
if (ori1 != ori2)
return (RH_OUT);
// Test line p1 - p3
SwkMatrix3x3<GenericPointT, double> swk_matrix3 (p1, p3, temppoint, ptn);
SwkMatrix3x3<GenericPointT, double> swk_matrix4 (p1, p3, temppoint, p2);
ori1 = swk_matrix3.orient3D_short();
ori2 = swk_matrix4.orient3D_short();
//std::cout << "p13: ori1: " << ori1 << std::endl;
//std::cout << "p13: ori2: " << ori2 << std::endl;
if (ori1 == RH_ON || ori2 == RH_ON)
return RH_ON;
if (ori1 != ori2)
return (RH_OUT);
// Test line p2 - p3
GenericPointT temppoint2 (p2[0] + useNormal[0],
p2[1] + useNormal[1],
p2[2] + useNormal[2]);
SwkMatrix3x3<GenericPointT, double> swk_matrix5 (p2, p3, temppoint2, ptn);
SwkMatrix3x3<GenericPointT, double> swk_matrix6 (p2, p3, temppoint2, p1);
ori1 = swk_matrix5.orient3D_short();
ori2 = swk_matrix6.orient3D_short();
//std::cout << "p23: ori1: " << ori1 << std::endl;
//std::cout << "p23: ori2: " << ori2 << std::endl;
if (ori1 == RH_ON || ori2 == RH_ON)
return RH_ON;
if (ori1 != ori2)
return (RH_OUT);
return (RH_IN);
}
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);
}
int main(int argc, char **argv)
{
plan_tests(92);
// test constructor
GeoPoint p1(Angle::Degrees(345.32), Angle::Degrees(-6.332));
ok1(p1.IsValid());
ok1(equals(p1, -6.332, 345.32));
// test normalize()
p1.Normalize();
ok1(p1.IsValid());
ok1(equals(p1, -6.332, -14.68));
// test parametric()
GeoPoint p2(Angle::Degrees(2), Angle::Degrees(1));
GeoPoint p3 = p1.Parametric(p2, 5);
ok1(p2.IsValid());
ok1(p3.IsValid());
ok1(equals(p3, -1.332, -4.68));
// test interpolate
GeoPoint p4 = p1.Interpolate(p3, 0.5);
ok1(p4.IsValid());
ok1(equals(p4, -3.832, -9.68));
GeoPoint p5 = p1.Interpolate(p3, 0.25);
ok1(p5.IsValid());
ok1(equals(p5, -5.082, -12.18));
// test *
GeoPoint p6 = p2 * 3.5;
ok1(p6.IsValid());
ok1(equals(p6, 3.5, 7));
// test +
p6 = p6 + p2;
ok1(p6.IsValid());
ok1(equals(p6, 4.5, 9));
// test +=
p6 += p2;
ok1(p6.IsValid());
ok1(equals(p6, 5.5, 11));
// test -
p6 = p6 - p2;
ok1(p6.IsValid());
ok1(equals(p6, 4.5, 9));
// for large and short distance testing
GeoPoint p11(Angle::Degrees(0.00001), Angle::Degrees(0.00001));
GeoPoint p12(Angle::Degrees(179), Angle::Degrees(0));
p11 += p1;
p12 += p1;
ok1(p11.IsValid());
ok1(equals(p11, -6.33199, -14.67999));
ok1(p12.IsValid());
ok1(equals(p12, -6.332, 164.32));
// test sort()
ok1(!p1.Sort(p3));
ok1(p3.Sort(p1));
ok1(!p1.Sort(p4));
ok1(p4.Sort(p1));
ok1(!p1.Sort(p5));
ok1(p5.Sort(p1));
ok1(!p4.Sort(p3));
ok1(p3.Sort(p4));
ok1(!p5.Sort(p3));
ok1(p3.Sort(p5));
ok1(!p5.Sort(p4));
ok1(p4.Sort(p5));
// test distance()
//
// note: distance between p1 and p4 and between p3 and p4 is not
// the same due to linear interpolation instead of real geographic
// intermediate point calculation
ok1(equals(p2.Distance(p6), 869146.334126));
ok1(equals(p6.Distance(p2), 869146.334126));
ok1(equals(p1.Distance(p5), 309506.275043));
ok1(equals(p1.Distance(p4), 619486.719361));
ok1(equals(p1.Distance(p3), 1240403.22926));
ok1(equals(p3.Distance(p4), 620924.169000));
ok1(equals(p1.Distance(p11), 1.561761));
ok1(equals(p1.Distance(p12), 18599361.600));
ok1(equals(p2.DistanceS(p6), 869326.653160));
ok1(equals(p6.DistanceS(p2), 869326.653160));
ok1(equals(p1.DistanceS(p5), 309562.219016));
ok1(equals(p1.DistanceS(p4), 619603.149273));
ok1(equals(p1.DistanceS(p3), 1240649.267606));
ok1(equals(p3.DistanceS(p4), 621053.760625));
ok1(equals(p1.DistanceS(p11), 1.568588));
ok1(equals(p1.DistanceS(p12), 18602548.701));
// test bearing()
//
// note: the bearings p1 -> p5, p5 -> p4 and so on are not the same due to
// linear interpolation instead of real geographic intermediate point
// calculation
ok1(equals(p2.Bearing(p6), 63.425773));
ok1(equals(p6.Bearing(p2), 243.762198));
ok1(equals(p1.Bearing(p5), 63.601900));
ok1(equals(p1.Bearing(p4), 63.735395));
ok1(equals(p1.Bearing(p3), 63.937616));
ok1(equals(p5.Bearing(p4), 63.619712));
ok1(equals(p5.Bearing(p3), 63.799336));
ok1(equals(p4.Bearing(p3), 63.694155));
ok1(equals(p5.Bearing(p6), 66.126880));
ok1(equals(p2.Bearing(p3), 250.886912));
ok1(equals(p2.BearingS(p6), 63.272424));
ok1(equals(p6.BearingS(p2), 243.608847));
ok1(equals(p1.BearingS(p5), 63.449343));
ok1(equals(p1.BearingS(p4), 63.582620));
ok1(equals(p1.BearingS(p3), 63.784526));
ok1(equals(p5.BearingS(p4), 63.466726));
ok1(equals(p5.BearingS(p3), 63.646072));
ok1(equals(p4.BearingS(p3), 63.540756));
ok1(equals(p5.BearingS(p6), 65.982854));
ok1(equals(p2.BearingS(p3), 250.786774));
// test distance_bearing()
// note: should be the same output as bearing() and distance()
GeoVector v = p2.DistanceBearing(p6);
ok1(equals(v.distance, 869146.334126));
ok1(equals(v.bearing, 63.425773));
v = p2.DistanceBearingS(p6);
ok1(equals(v.distance, 869326.653160));
ok1(equals(v.bearing, 63.272424));
// test intermediate_point()
GeoPoint p7(Angle::Zero(), Angle::Zero());
ok1(p7.IsValid());
GeoPoint p8 = p7.IntermediatePoint(p2, 100000);
ok1(p8.IsValid());
ok1(equals(p8, 0.402361, 0.804516));
ok1(equals(p8.Distance(p7), 100000));
GeoPoint p9 = p7.IntermediatePoint(p2, 100000000);
ok1(p9.IsValid());
ok1(equals(p9, p2));
// test projected_distance()
ok1(equals(p8.ProjectedDistance(p7, p2), 100000));
ok1(equals(p4.ProjectedDistance(p1, p3), 619494.517917));
ok1(equals((p2 * 2).ProjectedDistance(p2, p6), 248511.833322));
// Tests moved here from test_fixed.cpp
GeoPoint l1(Angle::Zero(), Angle::Zero());
ok1(l1.IsValid());
GeoPoint l2(Angle::Degrees(-0.3), Angle::Degrees(1.0));
ok1(l2.IsValid());
GeoPoint l3(Angle::Degrees(0.00001), Angle::Zero());
ok1(l3.IsValid());
GeoPoint l4(Angle::Degrees(10), Angle::Zero());
ok1(l4.IsValid());
l4.SetInvalid();
ok1(!l4.IsValid());
bool find_lat_lon_okay = true;
for (Angle bearing = Angle::Zero(); bearing < Angle::FullCircle();
bearing += Angle::Degrees(5)) {
GeoPoint p_test = FindLatitudeLongitude(p1, bearing, 50000);
find_lat_lon_okay = equals(p_test.Distance(p1), 50000) && find_lat_lon_okay;
}
ok1(find_lat_lon_okay);
v = l1.DistanceBearing(l2);
// 116090 @ 343
v = l1.DistanceBearing(l3);
ok(v.distance > 0 && v.distance < 2, "earth distance short", 0);
GeoPoint p10(GeoPoint::Invalid());
ok1(!p10.IsValid());
return exit_status();
}
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;
}
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);
}
void tst_QHashFunctions::qhash()
{
{
QBitArray a1;
QBitArray a2;
QVERIFY(qHash(a1) == 0);
a1.resize(1);
a1.setBit(0, true);
a2.resize(1);
a2.setBit(0, false);
uint h1 = qHash(a1);
uint h2 = qHash(a2);
QVERIFY(h1 != h2);
a2.setBit(0, true);
QVERIFY(h1 == qHash(a2));
a1.fill(true, 8);
a1.resize(7);
h1 = qHash(a1);
a2.fill(true, 7);
h2 = qHash(a2);
QVERIFY(h1 == h2);
a2.setBit(0, false);
uint h3 = qHash(a2);
QVERIFY(h2 != h3);
a2.setBit(0, true);
QVERIFY(h2 == qHash(a2));
a2.setBit(6, false);
uint h4 = qHash(a2);
QVERIFY(h2 != h4);
a2.setBit(6, true);
QVERIFY(h2 == qHash(a2));
QVERIFY(h3 != h4);
}
{
QPair<int, int> p12(1, 2);
QPair<int, int> p21(2, 1);
QVERIFY(qHash(p12) == qHash(p12));
QVERIFY(qHash(p21) == qHash(p21));
QVERIFY(qHash(p12) != qHash(p21));
QPair<int, int> pA(0x12345678, 0x12345678);
QPair<int, int> pB(0x12345675, 0x12345675);
QVERIFY(qHash(pA) != qHash(pB));
}
}
int main(int argc, const char * argv[]) {
// insert code here...
double coeff[13];//array of coefficients for taylor polynomial of cos(x)
bool neg=false;//if coefficient is supposed to be negative, then true
for (int i=0.0; i<13; i++) {//assigns coefficients to array
if(i%2==0)
{if(!neg)
{
coeff[i]=(double)(1.0/factorial(i));
neg=true;
}
else
{ coeff[i]=(double)(-1.0/factorial(i));
neg=false;
}
}
else{
coeff[i]=0;
}
}
Matrix z=Linspace(-3, 3, 601);//creates matrix with 601 evenly div numbers between -3 and 3
Matrix p4coeff(1,5,coeff);//matrix with first 4 taylor coefficients
Matrix p4(1,601);//matrix with result for 4th degree polynomial evaluated at z
for (int i=0;i<601;i++)
{
p4(i)=nest(p4coeff,z(i));
}
p4.Write("p4.txt");//wriete results to text file
//same as above for 8th degree polynomial
Matrix p8coeff(1,9,coeff);
Matrix p8(1,601);
for (int i=0;i<601;i++)
{
p8(i)=nest(p8coeff,z(i));
}
p8.Write("p8.txt");
//same as above for 12th degree polynomial
Matrix p12coeff(1,13,coeff);
Matrix p12(1,601);
for (int i=0;i<601;i++)
{
p12(i)=nest(p12coeff,z(i));
}
p12.Write("p12.txt");
// all points are evaluated at cos(x)
Matrix f(1,601);
for (int i=0;i<601;i++)
{
f(i)=cos(z(i));
}
f.Write("f.txt");
//calculate relative error for the three taylor polynomials
Matrix err4(1,601);
for (int i=0;i<601;i++)
{
err4(i)=cos(z(i))-p4(i);
}
err4.Abs();
err4.Write("err4.txt");
Matrix err8(1,601);
for (int i=0;i<601;i++)
{
err8(i)=cos(z(i))-p8(i);
}
err8.Abs();
err8.Write("err8.txt");
Matrix err12(1,601);
for (int i=0;i<601;i++)
{
err12(i)=cos(z(i))-p12(i);
}
err12.Abs();
err12.Write("err12.txt");
z.Write("z.txt");
}
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;
}
//------------------------------------------------------------------------------
TEST_F(Test_Engine_Object, GetSetHelper)
{
TestObjectC t1;
// <Test> 値型
{
// set
PropertyInfo::setPropertyValue(&t1, TestObjectC::V1Id, 100);
ASSERT_EQ(100, t1.V1);
// get
Variant v1 = PropertyInfo::getPropertyValue(&t1, TestObjectC::V1Id);
ASSERT_EQ(100, Variant::cast<int>(v1));
// set (Direct)
PropertyInfo::setPropertyValueDirect<int>(&t1, TestObjectC::V1Id, 200);
ASSERT_EQ(200, t1.V1);
// get (Direct)
Variant v2 = PropertyInfo::getPropertyValueDirect<int>(&t1, TestObjectC::V1Id);
ASSERT_EQ(200, Variant::cast<int>(v2));
// setter/getter
t1.SetV1(300);
ASSERT_EQ(300, t1.GetV1());
}
// <Test> ポインタ型
{
RefTest1 rt1;
RefTest1 rt2;
// set
PropertyInfo::setPropertyValue(&t1, TestObjectC::V2Id, &rt1);
ASSERT_EQ(&rt1, t1.V2);
// get
Variant v1 = PropertyInfo::getPropertyValue(&t1, TestObjectC::V2Id);
ASSERT_EQ(&rt1, Variant::cast<RefTest1*>(v1));
// set (Direct)
PropertyInfo::setPropertyValueDirect<RefTest1*>(&t1, TestObjectC::V2Id, &rt2);
ASSERT_EQ(&rt2, t1.V2);
// get (Direct)
Variant v2 = PropertyInfo::getPropertyValueDirect<RefTest1*>(&t1, TestObjectC::V2Id);
ASSERT_EQ(&rt2, Variant::cast<RefTest1*>(v2));
// setter/getter
RefTest1 rt3;
t1.SetV2(&rt3);
ASSERT_EQ(&rt3, t1.GetV2());
// setter/getter (nullptr)
t1.SetV2(nullptr);
ASSERT_EQ(nullptr, t1.GetV2());
}
// <Test> 構造体
{
// set
Point pt1(1, 2);
PropertyInfo::setPropertyValue(&t1, TestObjectC::V3Id, pt1);
ASSERT_EQ(1, t1.V3.get().x);
ASSERT_EQ(2, t1.V3.get().y);
// get
Variant v = PropertyInfo::getPropertyValue(&t1, TestObjectC::V3Id);
ASSERT_EQ(1, Variant::cast<Point>(v).x);
ASSERT_EQ(2, Variant::cast<Point>(v).y);
// set (Direct)
Point pt2(10, 20);
PropertyInfo::setPropertyValueDirect<Point>(&t1, TestObjectC::V3Id, pt2);
ASSERT_EQ(10, t1.V3.get().x);
ASSERT_EQ(20, t1.V3.get().y);
// get (Direct)
Variant v2 = PropertyInfo::getPropertyValueDirect<Point>(&t1, TestObjectC::V3Id);
ASSERT_EQ(10, Variant::cast<Point>(v2).x);
ASSERT_EQ(20, Variant::cast<Point>(v2).y);
// setter/getter
t1.SetV3(Point(3, 4));
ASSERT_EQ(3, t1.GetV3().x);
ASSERT_EQ(4, t1.GetV3().y);
}
// <Test> Ref
{
// set
Ref<RefTest2> p1(LN_NEW RefTest2(), false);
PropertyInfo::setPropertyValue(&t1, TestObjectC::V4Id, p1);
ASSERT_EQ(p1, t1.V4);
// get
Variant v = PropertyInfo::getPropertyValue(&t1, TestObjectC::V4Id);
ASSERT_EQ(p1.get(), Variant::cast<Ref<RefTest2>>(v));
// set (Direct)
Ref<RefTest2> p12(LN_NEW RefTest2(), false);
PropertyInfo::setPropertyValueDirect<Ref<RefTest2>>(&t1, TestObjectC::V4Id, p12);
ASSERT_EQ(p12, t1.V4);
// get (Direct)
Variant v2 = PropertyInfo::getPropertyValueDirect<Ref<RefTest2>>(&t1, TestObjectC::V4Id);
ASSERT_EQ(p12, Variant::cast<Ref<RefTest2>>(v2));
// setter/getter
Ref<RefTest2> p2(LN_NEW RefTest2(), false);
t1.SetV4(p2);
ASSERT_EQ(p2.get(), t1.GetV4());
// setter/getter (nullptr)
t1.SetV4(nullptr);
ASSERT_EQ(nullptr, t1.GetV4());
}
}
void BuildTreeDoubleYShape(Node *node[], Tree &tree)
{
const KDL::Vector unitx(1,0,0);
const KDL::Vector unity(0,1,0);
const KDL::Vector unitz(0,0,1);
const KDL::Vector unit1(sqrt(14.0)/8.0, 1.0/8.0, 7.0/8.0);
const KDL::Vector zero = KDL::Vector::Zero();
KDL::Vector p0(0.0f, -1.5f, 0.0f);
KDL::Vector p1(0.0f, -1.0f, 0.0f);
KDL::Vector p2(0.0f, -0.5f, 0.0f);
KDL::Vector p3(0.5f*Root2Inv, -0.5+0.5*Root2Inv, 0.0f);
KDL::Vector p4(0.5f*Root2Inv+0.5f*HalfRoot3, -0.5+0.5*Root2Inv+0.5f*0.5, 0.0f);
KDL::Vector p5(0.5f*Root2Inv+1.0f*HalfRoot3, -0.5+0.5*Root2Inv+1.0f*0.5, 0.0f);
KDL::Vector p6(0.5f*Root2Inv+1.5f*HalfRoot3, -0.5+0.5*Root2Inv+1.5f*0.5, 0.0f);
KDL::Vector p7(0.5f*Root2Inv+0.5f*HalfRoot3, -0.5+0.5*Root2Inv+0.5f*HalfRoot3, 0.0f);
KDL::Vector p8(0.5f*Root2Inv+1.0f*HalfRoot3, -0.5+0.5*Root2Inv+1.0f*HalfRoot3, 0.0f);
KDL::Vector p9(0.5f*Root2Inv+1.5f*HalfRoot3, -0.5+0.5*Root2Inv+1.5f*HalfRoot3, 0.0f);
KDL::Vector p10(-0.5f*Root2Inv, -0.5+0.5*Root2Inv, 0.0f);
KDL::Vector p11(-0.5f*Root2Inv-0.5f*HalfRoot3, -0.5+0.5*Root2Inv+0.5f*HalfRoot3, 0.0f);
KDL::Vector p12(-0.5f*Root2Inv-1.0f*HalfRoot3, -0.5+0.5*Root2Inv+1.0f*HalfRoot3, 0.0f);
KDL::Vector p13(-0.5f*Root2Inv-1.5f*HalfRoot3, -0.5+0.5*Root2Inv+1.5f*HalfRoot3, 0.0f);
KDL::Vector p14(-0.5f*Root2Inv-0.5f*HalfRoot3, -0.5+0.5*Root2Inv+0.5f*0.5, 0.0f);
KDL::Vector p15(-0.5f*Root2Inv-1.0f*HalfRoot3, -0.5+0.5*Root2Inv+1.0f*0.5, 0.0f);
KDL::Vector p16(-0.5f*Root2Inv-1.5f*HalfRoot3, -0.5+0.5*Root2Inv+1.5f*0.5, 0.0f);
node[0] = new Node(p0, unit1, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertRoot(node[0]);
node[1] = new Node(p1, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[0], node[1]);
node[2] = new Node(p1, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[1], node[2]);
node[3] = new Node(p2, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[2], node[3]);
node[4] = new Node(p2, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertRightSibling(node[3], node[4]);
node[5] = new Node(p3, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[3], node[5]);
node[6] = new Node(p3, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertRightSibling(node[5], node[6]);
node[7] = new Node(p3, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[5], node[7]);
node[8] = new Node(p4, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[7], node[8]);
node[9] = new Node(p5, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[8], node[9]);
node[10] = new Node(p5, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[9], node[10]);
node[11] = new Node(p6, zero, 0.08, EFFECTOR);
tree.InsertLeftChild(node[10], node[11]);
node[12] = new Node(p3, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[6], node[12]);
node[13] = new Node(p7, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[12], node[13]);
node[14] = new Node(p8, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[13], node[14]);
node[15] = new Node(p8, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[14], node[15]);
node[16] = new Node(p9, zero, 0.08, EFFECTOR);
tree.InsertLeftChild(node[15], node[16]);
node[17] = new Node(p10, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[4], node[17]);
node[18] = new Node(p10, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[17], node[18]);
node[19] = new Node(p10, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertRightSibling(node[17], node[19]);
node[20] = new Node(p11, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[18], node[20]);
node[21] = new Node(p12, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[20], node[21]);
node[22] = new Node(p12, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[21], node[22]);
node[23] = new Node(p13, zero, 0.08, EFFECTOR);
tree.InsertLeftChild(node[22], node[23]);
node[24] = new Node(p10, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[19], node[24]);
node[25] = new Node(p14, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[24], node[25]);
node[26] = new Node(p15, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[25], node[26]);
node[27] = new Node(p15, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
tree.InsertLeftChild(node[26], node[27]);
node[28] = new Node(p16, zero, 0.08, EFFECTOR);
tree.InsertLeftChild(node[27], node[28]);
}
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;
}
GeometryTools::IntersectionStatus inPlaneLineIntersect(
double x1, double y1,
double x2, double y2,
double x3, double y3,
double x4, double y4,
double l1NormalizedTolerance, double l2NormalizedTolerance,
double *x, double *y, double* fractionAlongLine1, double* fractionAlongLine2)
{
double mua, mub;
double denom, numera, numerb;
denom = (y4-y3) * (x2-x1) - (x4-x3) * (y2-y1);
numera = (x4-x3) * (y1-y3) - (y4-y3) * (x1-x3);
numerb = (x2-x1) * (y1-y3) - (y2-y1) * (x1-x3);
double EPS = 1e-40;
// Are the line coincident?
if (fabs(numera) < EPS && fabs(numerb) < EPS && fabs(denom) < EPS)
{
#if 0
*x = 0;
*y = 0;
*fractionAlongLine1 = 0;
*fractionAlongLine2 = 0;
return GeometryTools::LINES_OVERLAP;
#else
cvf::Vec3d p12(x2-x1, y2-y1, 0);
cvf::Vec3d p13(x3-x1, y3-y1, 0);
cvf::Vec3d p34(x4-x3, y4-y3, 0);
double length12 = p12.length();
double length34 = p34.length();
// Check if the p1 p2 line is a point
if (length12 < EPS )
{
cvf::Vec3d p34(x4-x3, y4-y3, 0);
*x = x1;
*y = y1;
*fractionAlongLine1 = 1;
*fractionAlongLine2 = p13.length()/p34.length();
return GeometryTools::LINES_OVERLAP;
}
cvf::Vec3d p14(x4-x1, y4-y1, 0);
cvf::Vec3d p32(x2-x3, y2-y3, 0);
cvf::Vec3d e12 = p12.getNormalized();
double normDist13 = e12*p13 / length12;
double normDist14 = e12*p14 / length12;
// Check if both points on the p3 p4 line is outside line p1 p2.
if( (normDist13 < 0 - l1NormalizedTolerance && normDist14 < 0-l1NormalizedTolerance )|| (normDist13 > 1 +l1NormalizedTolerance && normDist14 > 1+l1NormalizedTolerance ) )
{
*x = 0;
*y = 0;
*fractionAlongLine1 = 0;
*fractionAlongLine2 = 0;
return GeometryTools::NO_INTERSECTION;
}
double normDist32 = e12*p32 / length34;
//double normDist31 = -e12*p13 / length34;
// Set up fractions along lines to the edge2 vertex actually touching edge 1.
/// if two, select the one furthest from the start
bool pt3IsInside = false;
bool pt4IsInside = false;
if ((0.0 - l1NormalizedTolerance) <= normDist13 && normDist13 <= (1.0 +l1NormalizedTolerance) ) pt3IsInside = true;
if ((0.0 - l1NormalizedTolerance) <= normDist14 && normDist14 <= (1.0 +l1NormalizedTolerance) ) pt4IsInside = true;
if (pt3IsInside && !pt4IsInside)
{
*fractionAlongLine1 = normDist13;
*fractionAlongLine2 = 0.0;
*x = x3;
*y = y3;
}
else if (pt4IsInside && !pt3IsInside)
{
*fractionAlongLine1 = normDist14;
*fractionAlongLine2 = 1.0;
*x = x4;
*y = y4;
}
else if (pt3IsInside && pt4IsInside)
{
// Return edge 2 vertex furthest along edge 1
if (normDist13 <= normDist14)
{
*fractionAlongLine1 = normDist14 ;
*fractionAlongLine2 = 1.0;
*x = x4;
*y = y4;
}
else
{
*fractionAlongLine1 = normDist13;
*fractionAlongLine2 = 0.0;
*x = x3;
*y = y3;
}
}
else // both outside on each side
{
// Return End of edge 1
*fractionAlongLine1 = 1.0;
*fractionAlongLine2 = normDist32;
*x = x2;
*y = y2;
}
return GeometryTools::LINES_OVERLAP;
#endif
}
/* Are the line parallel */
if (fabs(denom) < EPS) {
*x = 0;
*y = 0;
*fractionAlongLine1 = 0;
*fractionAlongLine2 = 0;
return GeometryTools::NO_INTERSECTION;
}
/* Is the intersection along the the segments */
mua = numera / denom;
mub = numerb / denom;
*x = x1 + mua * (x2 - x1);
*y = y1 + mua * (y2 - y1);
*fractionAlongLine1 = mua;
*fractionAlongLine2 = mub;
if (mua < 0 - l1NormalizedTolerance || 1 + l1NormalizedTolerance < mua || mub < 0 - l2NormalizedTolerance || 1 + l2NormalizedTolerance < mub)
{
return GeometryTools::LINES_INTERSECT_OUTSIDE;
}
else if (fabs(mua) < l1NormalizedTolerance || fabs(1-mua) < l1NormalizedTolerance ||
fabs(mub) < l2NormalizedTolerance || fabs(1-mub) < l2NormalizedTolerance )
{
if (fabs(mua) < l1NormalizedTolerance) *fractionAlongLine1 = 0;
if (fabs(1-mua) < l1NormalizedTolerance) *fractionAlongLine1 = 1;
if (fabs(mub) < l2NormalizedTolerance) *fractionAlongLine2 = 0;
if (fabs(1-mub) < l2NormalizedTolerance) *fractionAlongLine2 = 1;
return GeometryTools::LINES_TOUCH;
}
else
{
return GeometryTools::LINES_CROSSES;
}
}
void test_RT()
{
typedef RT Cls;
// _test_cls_regular_3( Cls() );
typedef traits::Bare_point Point;
typedef traits::Weighted_point Weighted_point;
typedef typename Cls::Vertex_handle Vertex_handle;
typedef typename Cls::Cell_handle Cell_handle;
typedef typename Cls::Facet Facet;
typedef typename Cls::Edge Edge;
typedef std::list<Weighted_point> list_point;
typedef typename Cls::Finite_cells_iterator Finite_cells_iterator;
// temporary version
int n, m;
int count = 0;
// For dimension 0, we need to check that the point of highest weight is the
// one that finally ends up in the vertex.
std::cout << " test dimension 0 " << std::endl;
Cls T0;
T0.insert(Weighted_point( Point (0,0,0), 0) );
T0.insert(Weighted_point( Point (0,0,0), 1) );
T0.insert(Weighted_point( Point (0,0,0), -1) );
assert(T0.dimension() == 0);
assert(T0.number_of_vertices() == 1);
assert(T0.finite_vertices_begin()->point().weight() == 1);
std::cout << " test dimension 1 " << std::endl;
Cls T1;
std::cout << " number of inserted points : " ;
Weighted_point p[5];
for ( m=0; m<5; m++) {
if ( (m%2)== 0 )
p[m] = Weighted_point( Point( 2*m,0,0 ), 2 );
else
p[m] = Weighted_point( Point( -2*m+1,0,0 ), 2 );
T1.insert( p[m] );
count++;
if (count <10)
std::cout << count << '\b' ;
else
if (count < 100)
std::cout << count << '\b' << '\b' ;
else
std::cout << count << '\b' << '\b' << '\b' ;
std::cout.flush();
}
assert( T1.is_valid() );
std::cout << std::endl << " number of vertices : "
<< T1.number_of_vertices() << std::endl;
std::cout << " number of inserted points : " ;
Weighted_point q[5];
for ( m=0; m<5; m++) {
if ( (m%2)== 0 )
q[m] = Weighted_point( Point( 2*m+1,0,0 ), 5 );
else
q[m] = Weighted_point( Point( -2*m+1,0,0 ), 5 );
T1.insert( q[m] );
count++;
if (count <10)
std::cout << count << '\b' ;
else
if (count < 100)
std::cout << count << '\b' << '\b' ;
else
std::cout << count << '\b' << '\b' << '\b' ;
std::cout.flush();
}
assert( T1.is_valid() );
std::cout << std::endl << " number of vertices : "
<< T1.number_of_vertices() << std::endl;
std::cout << " number of inserted points : " ;
Weighted_point r[10];
for ( m=0; m<10; m++) {
if ( (m%2)== 0 )
r[m] = Weighted_point( Point( m,0,0 ), 1 );
else
r[m] = Weighted_point( Point( -m,0,0 ), 1 );
T1.insert( r[m] );
count++;
if (count <10)
std::cout << count << '\b' ;
else
if (count < 100)
std::cout << count << '\b' << '\b' ;
else
std::cout << count << '\b' << '\b' << '\b' ;
std::cout.flush();
}
assert( T1.is_valid() );
std::cout << std::endl << " number of vertices : "
<< T1.number_of_vertices() << std::endl;
assert( T1.dimension()==1 );
// The following is distilled from a bug report by Wulue Zhao
// ([email protected]), a student of Tamal Dey.
Point pt0(0,0,0);
Point pt1( 1,0,0), pt2(2,0,0), pt3(3,0,0);
Point pt4(-1,0,0), pt5(-2,0,0), pt6(-3,0,0);
Weighted_point wp0(pt0,10.0);
Weighted_point wp1(pt1,0.0), wp2(pt2,0.0), wp3(pt3,0.0);
Weighted_point wp4(pt4,0.0), wp5(pt5,0.0), wp6(pt6,0.0);
Cls T11;
T11.insert(wp0);
T11.insert(wp1);
T11.insert(wp2);
T11.insert(wp3);
T11.insert(wp4);
T11.insert(wp5);
T11.insert(wp6);
assert(T11.is_valid());
// And another distilled bug report from the same guy.
{
Point p1(-0.07, 0.04, 0.04);
Point p2(0.09, 0.04, 0.04);
Point p3(0.09, -0.05, 0.04);
Point p4(0.05, -0.05, 0.04);
Point p5(0.05, 0.0, 0.04);
Point p6(-0.07, 0.0, 0.04);
Point p7(-0.07, 0.04, -0.04);
Point p8(0.09, 0.04, -0.04);
Point p9(0.09, -0.05, -0.04);
Point p10(0.05, -0.05, -0.04);
Point p11(0.05, 0.0, -0.04);
Point p12(-0.07, 0.0, -0.04);
Weighted_point wp1(p1,0);
Weighted_point wp2(p2,0);
Weighted_point wp3(p3,0);
Weighted_point wp4(p4,0);
Weighted_point wp5(p5,0);
Weighted_point wp6(p6,0);
Weighted_point wp7(p7,0);
Weighted_point wp8(p8,0);
Weighted_point wp9(p9,0);
Weighted_point wp10(p10,0);
Weighted_point wp11(p11,0);
Weighted_point wp12(p12,0);
Weighted_point wp13(p3,0.3); // wp13 has the same coordinates with wp3
Cls T111;
T111.insert(wp1);
T111.insert(wp2);
T111.insert(wp3);
T111.insert(wp13); // it doesnot work inserting wp13 here
T111.insert(wp4);
T111.insert(wp5);
T111.insert(wp6);
T111.insert(wp7);
T111.insert(wp8);
T111.insert(wp9);
T111.insert(wp10);
T111.insert(wp11);
T111.insert(wp12);
assert(T111.is_valid());
}
std::cout << " test dimension 2 " << std::endl;
std::cout << " number of inserted points : " ;
Cls T2;
count = 0 ;
int px=1, py=1;
int qx=-1, qy=2;
Weighted_point s[400];
for (m=0; m<10; m++)
for (n=0; n<10; n++) {
s[m+20*n] = Weighted_point( Point(m*px+n*qx, m*py+n*qy, 0), 1 );
T2.insert( s[m+20*n] );
count++;
if (count <10)
std::cout << count << '\b' ;
else
if (count < 100)
std::cout << count << '\b' << '\b' ;
else
std::cout << count << '\b' << '\b' << '\b' ;
std::cout.flush();
}
for (m=10; m<20; m++)
for (n=0; n<10; n++) {
s[m+20*n] = Weighted_point( Point(m*px+n*qx, m*py+n*qy, 0), -1 );
T2.insert( s[m+20*n] );
count++;
if (count <10)
std::cout << count << '\b' ;
else
if (count < 100)
std::cout << count << '\b' << '\b' ;
else
std::cout << count << '\b' << '\b' << '\b' ;
std::cout.flush();
}
for (m=0; m<10; m++)
for (n=10; n<20; n++) {
s[m+20*n] = Weighted_point( Point(m*px+n*qx, m*py+n*qy, 0), -2 );
T2.insert( s[m+20*n] );
count++;
if (count <10)
std::cout << count << '\b' ;
else
if (count < 100)
std::cout << count << '\b' << '\b' ;
else
std::cout << count << '\b' << '\b' << '\b' ;
std::cout.flush();
}
for (m=10; m<20; m++)
for (n=10; n<20; n++) {
s[m+20*n] = Weighted_point( Point(m*px+n*qx, m*py+n*qy, 0), 5 );
T2.insert( s[m+20*n] );
count++;
if (count <10)
std::cout << count << '\b' ;
else
if (count < 100)
std::cout << count << '\b' << '\b' ;
else
std::cout << count << '\b' << '\b' << '\b' ;
std::cout.flush();
}
std::cout << std::endl << " number of vertices : "
<< T2.number_of_vertices() << std::endl;
assert( T2.dimension()==2 );
assert( T2.is_valid() );
// dimension 3
std::cout << " test dimension 3" << std::endl;
Cls T;
list_point lp;
int a, b, d;
for (a=0;a!=10;a++)
// for (b=0;b!=10;b++)
for (b=0;b!=5;b++)
// for (d=0;d!=10;d++)
for (d=0;d!=5;d++)
lp.push_back(Weighted_point( Point(a*b-d*a + (a-b)*10 +a ,
a-b+d +5*b,
a*a-d*d+b),
a*b-a*d) );
list_point::iterator it;
count = 0 ;
std::cout << " number of inserted points : " ;
for (it=lp.begin(); it!=lp.end(); ++it){
count++;
T.insert(*it);
if (count <10)
std::cout << count << '\b' ;
else
if (count < 100)
std::cout << count << '\b' << '\b' ;
else
if (count < 1000)
std::cout << count << '\b' << '\b' << '\b' ;
else
std::cout << count << std::endl;
std::cout.flush();
}
std::cout << std::endl;
std::cout << " number of vertices : "
<< T.number_of_vertices() << std::endl;
assert(T.is_valid());
assert(T.dimension()==3);
T.clear();
std::cout << " test iterator range insert" << std::endl;
T.insert (lp.begin(), lp.end());
std::cout << " number of vertices : "
<< T.number_of_vertices() << std::endl;
assert(T.is_valid());
assert(T.dimension()==3);
//test nearest_power_vertex
std::cout << " test nearest_power_vertex " << std::endl;
Point pp1(0.0, 0.0, 0.0);
Point pp2(1.0, 0.0, 0.0);
Point pp3(0.0, 1.0, 0.0);
Point pp4(0.0, 0.0, 1.0);
Point pp5(1.0, 1.0, 0.0);
Point pp6(0.0, 1.0, 1.0);
Point pp7(1.0, 0.0, 1.0);
Point pp8(1.0, 1.0, 1.0);
Weighted_point wpp1(pp1, 1.0);
Weighted_point wpp2(pp2, 2.0);
Weighted_point wpp3(pp3, 1.0);
Weighted_point wpp4(pp4, 4.0);
Weighted_point wpp5(pp5, 1.0);
Weighted_point wpp6(pp6, 1.0);
Weighted_point wpp7(pp7, 1.0);
Weighted_point wpp8(pp8, 8.0);
Cls T3;
T3.insert(wpp1);
Vertex_handle v2 = T3.insert(wpp2);
assert( T3.nearest_power_vertex(Point(0.5,0.5,0.5)) == v2);
T3.insert(wpp3);
Vertex_handle v4 = T3.insert(wpp4);
assert( T3.nearest_power_vertex(Point(0.5,0.5,0.5)) == v4);
T3.insert(wpp5);
T3.insert(wpp6);
T3.insert(wpp7);
// Avoid inserting the same point twice, now that hidden points are handled,
// insert (existing_point) returns Vertex_handle().
// T3.insert(wpp8);
Vertex_handle v8 = T3.insert(wpp8);
Point query(0.5,0.5,0.5);
assert(T3.nearest_power_vertex(query) == v8);
assert(T3.nearest_power_vertex(Weighted_point(query,1.0)) == v8 );
assert(T3.nearest_power_vertex_in_cell(query ,v8->cell()) == v8);
// test dual
std::cout << " test dual member functions" << std::endl;
Finite_cells_iterator fcit = T3.finite_cells_begin();
for( ; fcit != T3.finite_cells_end(); ++fcit) {
Point cc = T3.dual(fcit);
Vertex_handle ncc = T3.nearest_power_vertex(cc);
assert(fcit->has_vertex(ncc));
}
// test Gabriel
std::cout << " test is_Gabriel " << std::endl;
Point q0(0.,0.,0.);
Point q1(2.,0.,0.);
Point q2(0.,2.,0.);
Point q3(0.,0.,2.);
Weighted_point wq0(q0,0.);
Weighted_point wq1(q1,0.);
Weighted_point wq2(q2,0.);
Weighted_point wq3(q3,0.);
Weighted_point wq01(q0,2.);
Cls T4;
Vertex_handle v0 = T4.insert(wq0);
Vertex_handle v1 = T4.insert(wq1);
v2 = T4.insert(wq2);
Vertex_handle v3 = T4.insert(wq3);
Cell_handle c;
int i,j,k,l;
assert(T4.is_facet(v0,v1,v2,c,j,k,l));
i = 6 - (j+k+l);
Facet f = std::make_pair(c,i);
assert(T4.is_Gabriel(c,i));
assert(T4.is_Gabriel(f));
assert(T4.is_facet(v1,v2,v3,c,j,k,l));
i = 6 - (j+k+l);
assert(!T4.is_Gabriel(c,i));
assert(T4.is_edge(v0,v1,c,i,j));
assert(T4.is_Gabriel(c,i,j));
Edge e = make_triple(c,i,j);
assert(T4.is_Gabriel(e));
assert(T4.is_edge(v2,v3,c,i,j));
assert(T4.is_Gabriel(c,i,j));
Vertex_handle v01 = T4.insert(wq01);
(void) v01; // kill warning
assert(T4.is_edge(v2,v3,c,i,j));
assert(!T4.is_Gabriel(c,i,j));
Weighted_point wwq0(q0,0.);
Weighted_point wwq1(q1,0.);
Weighted_point wwq2(q2,0.);
Weighted_point wwq3(q3,5.);
Cls T5;
v0 = T5.insert(wwq0);
v1 = T5.insert(wwq1);
v2 = T5.insert(wwq2);
v3 = T5.insert(wwq3);
assert(T5.nearest_power_vertex(v3->point().point()) == v3);
assert(T5.nearest_power_vertex(v0->point().point()) == v3);
assert(T5.is_Gabriel(v3));
assert(!T5.is_Gabriel(v0));
}
