C++ p5函数代码示例

  void addWeighted(WpointVector &list,ConvexDecompInterface *callback)
  {

    Wpoint p1(mP1,mC1);
    Wpoint p2(mP2,mC2);
    Wpoint p3(mP3,mC3);

		Vector3d d1 = mNear1 - mP1;
		Vector3d d2 = mNear2 - mP2;
		Vector3d d3 = mNear3 - mP3;

		d1*=WSCALE;
		d2*=WSCALE;
		d3*=WSCALE;

		d1 = d1 + mP1;
		d2 = d2 + mP2;
 	  d3 = d3 + mP3;

    Wpoint p4(d1,mC1);
    Wpoint p5(d2,mC2);
    Wpoint p6(d3,mC3);

    list.push_back(p1);
    list.push_back(p2);
    list.push_back(p3);

    list.push_back(p4);
    list.push_back(p5);
    list.push_back(p6);

#if 0
		callback->ConvexDebugPoint(mP1.Ptr(),0.01f,0x00FF00);
		callback->ConvexDebugPoint(mP2.Ptr(),0.01f,0x00FF00);
		callback->ConvexDebugPoint(mP3.Ptr(),0.01f,0x00FF00);
		callback->ConvexDebugPoint(d1.Ptr(),0.01f,0xFF0000);
		callback->ConvexDebugPoint(d2.Ptr(),0.01f,0xFF0000);
		callback->ConvexDebugPoint(d3.Ptr(),0.01f,0xFF0000);

		callback->ConvexDebugTri(mP1.Ptr(), d1.Ptr(),  d1.Ptr(),0x00FF00);
		callback->ConvexDebugTri(mP2.Ptr(), d2.Ptr(),  d2.Ptr(),0x00FF00);
		callback->ConvexDebugTri(mP3.Ptr(), d3.Ptr(),  d3.Ptr(),0x00FF00);

		Vector3d np1 = mP1 + mNormal*0.05f;
		Vector3d np2 = mP2 + mNormal*0.05f;
		Vector3d np3 = mP3 + mNormal*0.05f;

		callback->ConvexDebugTri(mP1.Ptr(), np1.Ptr(), np1.Ptr(), 0xFF00FF );
		callback->ConvexDebugTri(mP2.Ptr(), np2.Ptr(), np2.Ptr(), 0xFF00FF );
		callback->ConvexDebugTri(mP3.Ptr(), np3.Ptr(), np3.Ptr(), 0xFF00FF );

		callback->ConvexDebugPoint( np1.Ptr(), 0.01F, 0XFF00FF );
		callback->ConvexDebugPoint( np2.Ptr(), 0.01F, 0XFF00FF );
		callback->ConvexDebugPoint( np3.Ptr(), 0.01F, 0XFF00FF );

#endif



  }

int main()
{
    FitParameter p0 (2,0.2,"0p2",0.2,0.2,0.2,0.2);
    FitParameter p1 (3,0.3,"0p3",0.3,0.3,0.3,0.3);

    std::cout << "Before"
            << "\n" << p0
            << "\n" << p1
            << std::endl;

    p0.swap(p1);

    std::cout << "After"
            << "\n" << p0
            << "\n" << p1
            << std::endl;


    Parameter p5 (5,0.5,"0p5",0.5);
    Parameter p6 (6,0.6,"0p6",0.6);

    std::cout << "Before"
            << "\n" << p5
            << "\n" << p6
            << std::endl;

    p5.swap(p6);

    std::cout << "After"
            << "\n" << p5
            << "\n" << p6
            << std::endl;

    std::cout << "////////////////////////" << std::endl;
    std::cout << "Parameters"  << std::endl;

    Parameters pars0;
    pars0.AddParameter(p0);
    pars0.AddParameter(p1);

    Parameters pars1;
    pars1.AddParameter(p5);
    pars1.AddParameter(p6);

    std::cout << "Before"
            << "\n" << pars0
            << "\n" << pars1
            << std::endl;

    pars0.swap(pars1);

    std::cout << "After"
            << "\n" << pars0
            << "\n" << pars1
            << std::endl;



    return 0;
}

int main()
{
  // Construct an arrangement of seven intersecting line segments.
  Point_2 p1(1, 1), p2(1, 4), p3(2, 2), p4(3, 7), p5(4, 4), p6(7, 1), p7(9, 3);
  Ex_arrangement  arr;
  insert(arr, Segment_2(p1, p6));
  insert(arr, Segment_2(p1, p4));  insert(arr, Segment_2(p2, p6));
  insert(arr, Segment_2(p3, p7));  insert(arr, Segment_2(p3, p5));
  insert(arr, Segment_2(p6, p7));  insert(arr, Segment_2(p4, p7));

  // Create a mapping of the arrangement faces to indices.
  Face_index_map  index_map(arr);

  // Perform breadth-first search from the unbounded face, using the event
  // visitor to associate each arrangement face with its discover time.
  unsigned int    time = 0;
  boost::breadth_first_search(Dual_arrangement(arr), arr.unbounded_face(),
                              boost::vertex_index_map(index_map).visitor
                              (boost::make_bfs_visitor
                               (stamp_times(Face_property_map(), time,
                                            boost::on_discover_vertex()))));

  // Print the discover time of each arrangement face.
  Ex_arrangement::Face_iterator  fit;
  for (fit = arr.faces_begin(); fit != arr.faces_end(); ++fit) {
    std::cout << "Discover time " << fit->data() << " for ";
    if (fit != arr.unbounded_face()) {
      std::cout << "face ";
      print_ccb<Ex_arrangement>(fit->outer_ccb());
    }
    else std::cout << "the unbounded face." << std::endl;
  }
  return 0;
}

void Bezier::split(double t, Bezier & left, Bezier & right) const
{
	// http://processingjs.nihongoresources.com/bezierinfo/sketchsource.php?sketch=CubicDeCasteljau

	// interpolate from 4 to 3 points
	QPointF p5((1-t)*m_endpoint0.x() + t*m_cp0.x(), (1-t)*m_endpoint0.y() + t*m_cp0.y());
	QPointF p6((1-t)*m_cp0.x() + t*m_cp1.x(), (1-t)*m_cp0.y() + t*m_cp1.y());
	QPointF p7((1-t)*m_cp1.x() + t*m_endpoint1.x(), (1-t)*m_cp1.y() + t*m_endpoint1.y());

	// interpolate from 3 to 2 points
	QPointF p8((1-t)*p5.x() + t*p6.x(), (1-t)*p5.y() + t*p6.y());
	QPointF p9((1-t)*p6.x() + t*p7.x(), (1-t)*p6.y() + t*p7.y());

	// interpolate from 2 points to 1 point
	QPointF p10((1-t)*p8.x() + t*p9.x(), (1-t)*p8.y() + t*p9.y());

	// we now have all the values we need to build the subcurves
	left.m_endpoint0 = m_endpoint0;
	left.m_cp0 = p5;
	left.m_cp1 = p8;
	right.m_endpoint0 = left.m_endpoint1 = p10;
	right.m_cp0 = p9;
	right.m_cp1 = p7;
	right.m_endpoint1 = m_endpoint1;
	left.m_isEmpty = right.m_isEmpty = false;
} 

BDic* find_nodeQueryPacket(char node[20], char target[20]) {


    BDic* res = neutralPacket();

    BString* a3a = new BString ("q");
    BString* a3b = new BString ("find_node");;
    std::pair<BString*, Bencodable *> p3(a3a, a3b);
    res->push_back(p3);

    BString* a4a = new BString ("a");
    BDic* a4b = new BDic;

    BString* a5a = new BString ("id");
    BString* a5b = new BString (node);
    std::pair<BString*, Bencodable *> p5(a5a, a5b);
    a4b->push_back(p5);

    BString* a6a = new BString ("target");
    BString* a6b = new BString (target);
    std::pair<BString*, Bencodable *> p6(a6a, a6b);
    a4b->push_back(p6);

    std::pair<BString*, Bencodable *> p4(a4a, a4b);
    res->push_back(p4);


    return res;

}

mininode_geometry_pyramid::mininode_geometry_pyramid(double sizex,double sizey,double sizez)
   : mininode_geometry(0,3,0)
   {
   miniv3d p1(-sizex/2,-sizey/2,0);
   miniv3d p2(sizex/2,-sizey/2,0);
   miniv3d p3(-sizex/2,sizey/2,0);
   miniv3d p4(sizex/2,sizey/2,0);
   miniv3d p5(0,0,sizez);

   // sides
   setnrm(p1,p2,p5);
   addvtx(p5);
   addvtx(p1);
   addvtx(p2);
   setnrm(p2,p4,p5);
   addvtx(p2);
   addvtx(p5);
   addvtx(p4);
   setnrm(p4,p3,p5);
   addvtx(p4);
   addvtx(p3);
   addvtx(p5);
   setnrm(p3,p1,p5);
   addvtx(p5);
   addvtx(p1);
   addvtx(p3);

   // bottom
   setnrm(miniv3d(0,0,-1));
   addvtx(p3);
   addvtx(p4);
   addvtx(p1);
   addvtx(p2);
   }

void measurement_overhead() {
    printf("Measurement, 0, 0\n");

    PM("","RDTSC, "O_STR);

    #define FOR_MEAS for (int k = 0; k < 100; ++k) { ; }
    double m; uint64_t med, se, min, max;
    measure(FOR_MEAS,m,med,se,min,max,10000);
    printf("Loop, "O_STR, m/100, med/100, se/100, min/100, max/100);

    printf("Function Calls, 0, 0\n");

    PM_COUNT(p0(), "p0, "O_STR,1000000);

    PM_COUNT(p1(1), "p1, "O_STR,1000000);

    PM_COUNT(p2(1,2), "p2, "O_STR,1000000);

    PM_COUNT(p3(1,2,3), "p3, "O_STR,1000000);

    PM_COUNT(p4(1,2,3,4), "p4, "O_STR,1000000);

    PM_COUNT(p5(1,2,3,4,5), "p5, "O_STR,1000000);

    PM_COUNT(p6(1,2,3,4,5,6), "p6, "O_STR,1000000);

    PM_COUNT(p7(1,2,3,4,5,6,7), "p7, "O_STR,1000000);
}

int main(void)
{
  printf("Entering 100 iterations of incrementing pose X and Y and decrementing Theta...");
  for (int i = 0; i < 100; i++)
    fn2(fn1());

  printf("\nTesting ArPose::operator+(const ArPose&) and ArPose::operator-(const ArPose&)...\n");
  ArPose p1(10, 10, 90);
  ArPose p2(10, 10, 45);
  ArPose p3(0, 0, 0);
  ArPose p4(-20, 0, 360);
  ArPose p5(-20, -20, -180);
  printf("(10,10,90) + (10,10,90) => ");
  (p1 + p1).log();
  printf("(10,10,90) - (10,10,90) => ");
  (p1 - p1).log();
  printf("(10,10,90) + (10,10,45) => ");
  (p1 + p2).log();
  printf("(10,10,90) + (0,0,0) => ");
  (p1 + p3).log();
  printf("(10,10,90) - (0,0,0) => ");
  (p1 - p3).log();
  printf("(0,0,0) + (-20,0,360) => ");
  (p3 + p4).log();
  printf("(0,0,0) - (-20,0,360) => ");
  (p3 - p4).log();
  printf("(10,10,90) + (-20,0,360) => ");
  (p1 + p4).log();
  printf("(-20,0,360) + (-20,0,360) => ");
  (p4 + p4).log();
  printf("(-20,-20,-180) - (10,10,45) => ");
  (p5 - p2).log();

}

void testSharedArrayPtr()
{
    Monkey::numConstructions = 0;
    Monkey::numDestructions = 0;

    {
        SharedArrayPtr<Monkey> p1(new Monkey[3]);

        PEGASUS_TEST_ASSERT(Monkey::numConstructions == 3);
        PEGASUS_TEST_ASSERT(Monkey::numDestructions == 0);

        SharedArrayPtr<Monkey> p5(new Monkey[2]);
        p1 = p5;

        PEGASUS_TEST_ASSERT(Monkey::numConstructions == 5);
        PEGASUS_TEST_ASSERT(Monkey::numDestructions == 3);

        PEGASUS_TEST_ASSERT(p1[0].index == p5[0].index);
        PEGASUS_TEST_ASSERT(p1[1].index == p5[1].index);
        PEGASUS_TEST_ASSERT(p1[0].index != p1[1].index);
    }

    PEGASUS_TEST_ASSERT(Monkey::numConstructions == 5);
    PEGASUS_TEST_ASSERT(Monkey::numDestructions == 5);
}

dgInt32 dgConvexHull4d::BuildNormalList (dgBigVector* const normalArray) const
{
	dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
	dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f));

	dgInt32 count = 0;
	dgInt32 subdivitions = 1;

	dgInt32 start = 0;
	TessellateTriangle  (subdivitions, p4, p0, p2, count, normalArray, start);
	start = 1;
	TessellateTriangle  (subdivitions, p5, p3, p1, count, normalArray, start);
	start = 2;
	TessellateTriangle  (subdivitions, p5, p1, p2, count, normalArray, start);
	start = 3;
	TessellateTriangle  (subdivitions, p4, p3, p0, count, normalArray, start);
	start = 4;
	TessellateTriangle  (subdivitions, p4, p2, p1, count, normalArray, start);
	start = 5;
	TessellateTriangle  (subdivitions, p5, p0, p3, count, normalArray, start);
	start = 6;
	TessellateTriangle  (subdivitions, p5, p2, p0, count, normalArray, start);
	start = 7;
	TessellateTriangle  (subdivitions, p4, p1, p3, count, normalArray, start);

	return count;
}

int main( int argc, char *argv[] )
{
    if( argc < 2 || argc > 2)
   	{
        printf("Expecting one argument. Please try again\n");
   	}
    else if(argc==2)
    {
        if(atoi(argv[1])==1)
        {
            p1();
        }
        else if(atoi(argv[1])==2)
        {
            p2();
        }
        else if(atoi(argv[1])==3)
        {
            p3();
        }
        else if(atoi(argv[1])==4)
        {
            p4();
        }
        else if(atoi(argv[1])==5)
        {
            p5();
        }
        else
        {
            printf("Incorrect argument supplied.\n");
        }
    }
}

    PrimitiveParts cubePrimitive(float width, float height, float depth)
    {
        float hx = width / 2;
        float hy = height / 2;
        float hz = depth / 2;

        // create the vertices
        Point3 p0(hx,hy,hz);
        Point3 p1(hx,hy,-hz);
        Point3 p2(-hx,hy,-hz);
        Point3 p3(-hx,hy,hz);
        Point3 p4(hx,-hy,hz);
        Point3 p5(hx,-hy,-hz);
        Point3 p6(-hx,-hy,-hz);
        Point3 p7(-hx,-hy,hz);

        QList<int> f0 = QList<int>() << 0 << 1 << 2 << 3;
        QList<int> f1 = QList<int>() << 4 << 5 << 1 << 0;
        QList<int> f2 = QList<int>() << 6 << 2 << 1 << 5;
        QList<int> f3 = QList<int>() << 7 << 3 << 2 << 6;
        QList<int> f4 = QList<int>() << 7 << 4 << 0 << 3;
        QList<int> f5 = QList<int>() << 4 << 7 << 6 << 5;

        PrimitiveParts parts;
        parts.points = QVector<Point3>() << p0 << p1 << p2 << p3 << p4 << p5 << p6 << p7;
        parts.faces = QVector<QList<int> >() << f0 << f1 << f2 << f3 << f4 << f5;
        return parts;
    }

std::vector<Eigen::Vector2i> PathPlanner::getNeighbours(Eigen::Vector2i p)
{
    Eigen::Vector2i p0(p(0) + 1, p(1));
    Eigen::Vector2i p1(p(0) - 1, p(1));
    Eigen::Vector2i p2(p(0), p(1) + 1);
    Eigen::Vector2i p3(p(0), p(1) - 1);

    Eigen::Vector2i p4(p(0) + 1, p(1) + 1);
    Eigen::Vector2i p5(p(0) - 1, p(1) - 1);
    Eigen::Vector2i p6(p(0) - 1, p(1) + 1);
    Eigen::Vector2i p7(p(0) + 1, p(1) - 1);

    std::vector<Eigen::Vector2i> points;

    if(validPoint(p0))
        points.push_back(p0);
    if(validPoint(p1))
        points.push_back(p1);
    if(validPoint(p2))
        points.push_back(p2);
    if(validPoint(p3))
        points.push_back(p3);

    //Disallow walking through diagonal cracks
    if(validPoint(p4) && (validPoint(p0) || validPoint(p2)))
        points.push_back(p4);
    if(validPoint(p5) && (validPoint(p1) || validPoint(p3)))
        points.push_back(p5);
    if(validPoint(p6) && (validPoint(p1) || validPoint(p2)))
        points.push_back(p6);
    if(validPoint(p7) && (validPoint(p0) || validPoint(p3)))
        points.push_back(p7);

    return points;
}

int main()
{
    vec v1(1, 1), v2(0, 2);
    cout << "vec1(1,1) cross vec2(0,2): " << cross(v1, v2) << endl << endl;
    node p0(0, 0), p1(0, 1), p2(1, 2), p3(2, 1), p4(2, 0), p5(1, 0), p6(1, 1);
    node s[7];
    s[0] = p4, s[1] = p3, s[2] = p2, s[3] = p1, s[4] = p0, s[5] = p5, s[6] = p6;
    segment l0(p0, p3), l1(p5, p6), l2(p6, p4), l3(p1, p2);
    test_segment(l0, l1);
    test_segment(l1, l2);
    test_segment(l0, l3);

    segment ll[4];
    ll[0] = l0, ll[1] = l1, ll[2] = l2, ll[3] = l3;
    for(int i = 0; i < 4; ++ i) {
        ll[i].s_lt.n_idx = i;
        ll[i].s_rt.n_idx = i;
        ll[i].s_lt.n_lt = 1;
        ll[i].s_rt.n_lt = 0;
    }
    cout << "sweeping:" << endl;
    for(int i = 0; i < 4; ++ i)
        ll[i].s_print();
    if(sweeping(ll, 4))
        cout << "yes" << endl;
    else
        cout << "no" << endl;

    return(0);
}

static void tst6() {
    params_ref      ps;
    reslimit        rlim;
    nlsat::solver s(rlim, ps);
    anum_manager & am     = s.am();
    nlsat::pmanager & pm  = s.pm();
    nlsat::assignment as(am);
    nlsat::explain& ex    = s.get_explain();
    nlsat::var x0, x1, x2, a, b, c, d;
    a  = s.mk_var(false);
    b  = s.mk_var(false);
    c  = s.mk_var(false);
    d  = s.mk_var(false);
    x0 = s.mk_var(false);
    x1 = s.mk_var(false);
    x2 = s.mk_var(false);

    polynomial_ref p1(pm), p2(pm), p3(pm), p4(pm), p5(pm);
    polynomial_ref _x0(pm), _x1(pm), _x2(pm);
    polynomial_ref _a(pm), _b(pm), _c(pm), _d(pm);
    _x0 = pm.mk_polynomial(x0);
    _x1 = pm.mk_polynomial(x1);
    _x2 = pm.mk_polynomial(x2);
    _a  = pm.mk_polynomial(a);
    _b  = pm.mk_polynomial(b);
    _c  = pm.mk_polynomial(c);
    _d  = pm.mk_polynomial(d);

    p1 = (_a*(_x0^2)) + _x2 + 2;
    p2 = (_b*_x1) - (2*_x2) - _x0 + 8;
    nlsat::scoped_literal_vector lits(s);
    lits.push_back(mk_gt(s, p1));
    lits.push_back(mk_gt(s, p2));
    lits.push_back(mk_gt(s, (_c*_x0) + _x2 + 1));
    lits.push_back(mk_gt(s, (_d*_x0) - _x1 + 5*_x2));

    scoped_anum zero(am), one(am), two(am);
    am.set(zero, 0);
    am.set(one,  1);
    am.set(two,  2);
    as.set(0, one);
    as.set(1, one);
    as.set(2, two);
    as.set(3, two);
    as.set(4, two);
    as.set(5, one);
    as.set(6, one);
    s.set_rvalues(as);


    project(s, ex, x0, 2, lits.c_ptr());
    project(s, ex, x1, 3, lits.c_ptr());
    project(s, ex, x2, 3, lits.c_ptr());
    project(s, ex, x2, 2, lits.c_ptr());
    project(s, ex, x2, 4, lits.c_ptr());
    project(s, ex, x2, 3, lits.c_ptr()+1);


}