approximate_min_ellipsoid_d_debug.h

来自「很多二维 三维几何计算算法 C++ 类库」· C头文件 代码 · 共 728 行 · 第 1/2 页

	  << "    2 index 0 ge { neg } { } ifelse" << endl
	  << "    % stack: a b c sqrt(disc)" << endl
	  << "    % compute first solution (sqrt(disc)-b) / (2*a):" << endl
	  << "    dup 3 index sub 4 index 2.0 mul div" << endl
	  << "    5 1 roll" << endl
	  << "    % stack: x1 a b c sqrt(disc)" << endl
	  << "    % compute second solution 2*c / (sqrt(disc)-b):" << endl
	  << "    3 2 roll sub exch 2.0 mul exch div" << endl
	  << "    exch pop" << endl
	  << "} def" << endl
	  << "%" << endl
	  << "% Usage: u v normalize u' v'" << endl
	  << "% Takes the vector [u,v] and normalizes it to length 1." << endl
	  << "/normalize {" << endl
	  << "    dup dup mul" << endl
	  << "    % stack: u v v^2" << endl
	  << "    2 index dup mul add" << endl
	  << "    % stack: u v v^2+u^2" << endl
	  << "    sqrt 1.0 exch div dup 4 1 roll mul 3 1 roll mul exch" << endl
	  << "} def" << endl
	  << "%" << endl
	  << "% Usage: a b h cellipse" << endl
	  << "% Draws the ellipse x^T M x <= 1 with M = [[a,h],[h,b]]." << endl
	  << "%" << endl
	  << "/cellipse {" << endl
	  << "% compute Eigen decomposition of M:" << endl
	  << "%" << endl
	  << "% stack: a b h" << endl
	  << "dup 0.0 eq {" << endl
	  << "	% ellipse is a sphere:" << endl
	  << "	2 index sqrt" << endl
	  << "	2 index sqrt" << endl
	  << "	[ 1.0 0.0 0.0 1.0 0.0 0.0 ]" << endl
	  << "}" << endl
	  << "{" << endl
	  << "	1.0" << endl
	  << "	4 copy pop pop add neg" << endl
	  << "	5 copy pop pop dup mul neg 3 1 roll mul add" << endl
	  << "	find-roots" << endl
	  << "	% stack: a b h x1 x2" << endl
	  << "	%" << endl
	  << "	% build first Eigenvector:" << endl
	  << "	2 index 2 index 6 index sub normalize" << endl
	  << "	% build second Eigenvector:" << endl
	  << "	% stack: a b h x1 x2 ev1x ev1y" << endl
	  << "	4 index 3 index 8 index sub normalize" << endl
	  << "	% build matrix containing Eigenvectors:" << endl
	  << "	% stack: a b h x1 x2 ev1x ev1y ev2x ev2y" << endl
	  << "	6 array" << endl
	  << "	dup 0 6 index put" << endl
	  << "	dup 1 4 index put" << endl
	  << "	dup 2 5 index put" << endl
	  << "	dup 3 3 index put" << endl
	  << "	dup 4 0 put" << endl
	  << "	dup 5 0 put" << endl
	  << "	5 1 roll pop pop pop pop" << endl
	  << "} ifelse" << endl
	  << "    % stack:  a b h x1 x2 T" << endl
	  << "    %" << endl
	  << "    % We now draw an circle scaled by x1 (along the " << endl
	  << "    % x-axis) and x2 (along the y-axis):" << endl
	  << "    matrix currentmatrix    %  remember CTM" << endl
	  << "    % stack:  a b h x1 x2 T old-ctm" << endl
	  << "    exch matrix invertmatrix concat" << endl
	  << "    2 index sqrt 1.0 exch div 2 index sqrt 1.0" << endl
	  << "    exch div scale" << endl
	  << "    newpath" << endl
	  << "    0 0 1 0 360 arc closepath" << endl
	  << "    setmatrix               %  restore CTM" << endl
	  << "    pop pop pop pop pop" << endl
	  << "} def" << endl
	  << "%" << endl
	  << "% Usage: a b d u v mu ellipse" << endl
	  << "% Finds the path (i.e., border) of the ellipse" << endl
	  << "%" << endl
	  << "%   E = { x | x^T M x + x^T m + mu <= 0 }," << endl
	  << "%" << endl
	  << "% where" << endl
	  << "%" << endl
	  << "%       [ a  b ]        [ u ]" << endl
	  << "%   M = [      ],   m = [   ]" << endl
	  << "%       [ b  d ]        [ v ]" << endl
	  << "%" << endl
	  << "% and det(M) > 0." << endl
	  << "/ellipse {" << endl
	  << "  /emu exch def" << endl
	  << "  /ev exch def" << endl
	  << "  /eu exch def" << endl
	  << "  /ed exch def" << endl
	  << "  /eb exch def" << endl
	  << "  /ea exch def" << endl
	  << "  /ematrix [ ea eb eb ed 0 0 ] matrix invertmatrix def" << endl
	  << "  % compute z = M^{-1} m:" << endl
	  << "  eu ev ematrix transform" << endl
	  << "  /ezy exch def" << endl
	  << "  /ezx exch def" << endl
	  << "  % translate to center:" << endl
	  << "  matrix currentmatrix    %  remember CTM" << endl
	  << "  ezx neg 2 div ezy neg 2 div translate" << endl
	  << "  % compute matrix of (now centrally symmetric) ellipse:" << endl
	  << "  /efactor ezx eu mul ezy ev mul add 4.0 div emu sub def" << endl
	  << "  ea efactor div ed efactor div eb efactor div cellipse" << endl
	  << "  setmatrix               % restore CTM" << endl
	  << "} def" << endl
	  << "/setcol { 193 255 div 152 255 div 159 255 div" << endl
	  << "          setrgbcolor" << endl
	  << "} def" << endl
	  << "%" << endl
	  << "%" << endl
	  << "%%EndProlog" << endl
	  << "%%Page: 1 1" << endl;
	  
	// write content:
	f << "gsave" << endl
	  << body.str()
	  << "grestore" << endl
	  << "showpage" << endl
	  << "%%Trailer" << endl;
	
	// write footer:
	f.close();
      }
      
    public: // modifiers:
      // Sets the stroke mode for the next objects.
      void set_stroke_mode(Stroke_mode m)
      {
	bm = m;
      }
      
      // Sets the labelmode for the next objects.
      void set_label_mode(Label_mode m)
      {
	lm = m;
      }
      
      // Sets the label for the next object.  (This only makes sense if
      // the label mode for the next object will be different from None.)
      void set_label(const std::string& l)
      {
	next_label = l;
      }
      
      // Sets the angle for the next object drawn in mode Angle or
      // Random_angle.  (So you can temporarily overwrite the random
      // angle.)
      void set_angle(double angle)
      {
	next_angle = angle;
      }
      
      // Enable or disable automatic adjusting of the bounding box.
      void enlarge_bounding_box(bool active)
      {
	adjust_bb = active;
      }
      
      // Renders the line from (x,y) to (u,v) in the current stroke
      // and label mode.
      //
      // Implementation note: stroke and label mode are not yet implemented.
      void write_line(double x, double y, double u, double v,
		      bool colored = false)
      {
	// set color:
	if (colored)
	  body << "gsave setcol" << std::endl;
	
	// output ball in appropriate mode:
	adjust_bounding_box(x,y);
	adjust_bounding_box(u,v);
	body << "newpath\n"
	     << x << ' ' << y << " moveto\n"
	     << u << ' ' << v << " lineto\n"
	     << "stroke\n";

	// restore color:
	if (colored)
	  body << "grestore" << std::endl;
      }
      
      // Renders the circle with center (x,y) and radius r in
      // the current stroke and label mode.  All coordinates must be of
      // type double.
      void write_circle(double x, double y, double r, bool colored = false)
      {
	// set color:
	if (colored)
	  body << "gsave setcol" << std::endl;
	
	// output ball in appropriate mode:
	adjust_bounding_box(x-r,y-r);
	adjust_bounding_box(x+r,y+r);
	body << x << ' ' << y << ' ' << r << ' ';
	const char *mode[] = { "ball", "bball", "dball" };
	body << mode[static_cast<int>(bm)] << std::endl;
	
	// output label:
	if (lm != None) {
	  const double dist = 5.0;
	  const double lx = x + (r+dist/zoom)*std::cos(next_angle);
	  const double ly = y + (r+dist/zoom)*std::sin(next_angle);
	  adjust_bounding_box(lx,ly);
	  body << x << ' ' << y << ' ' << r << " (" << next_label << ") "
	       << dist << ' ' << next_angle << " drawl" << std::endl;
	}
	
	// restore color:
	if (colored)
	  body << "grestore" << std::endl;

	// generate next angle (if needed):
	if (lm == Random_angle)
	  next_angle = static_cast<double>(std::rand());
	
	// update counter and label:
	next_label = default_label(++count);
      }
      
      // Renders the centrally symmetric ellipse x^T M x <= 1 with M =
      // [[a,h],[h,b]] in the current stroke and label mode.
      void write_cs_ellipse(double a,double b,double h)
      {
	using std::endl;

	// Adjust the bounding box: For this, we use the fact that the
	// ellipse has semi-axes of lengths (1/sqrt(c),1/sqrt(d)) where
	//
        //   (c,d) = find_roots(1.0,-(a+b),a b - h^2)
	//
	// (See Sven's thesis, code on page 87.)  So it it enclosed in
	// some (rotated) rectangle of side lengths 2*c,2*d, centered
	// at the origin.  Consequently, the circle of radius
	// r=sqrt(c^2+d^2) encloses the ellipse, and we use this to
	// find the bounding box:
	const std::pair<double,double> semi = find_roots(1.0,-(a+b),a*b-h*h);
	const double r = std::sqrt(1/semi.first+1/semi.second);
	adjust_bounding_box(-r,-r);
	adjust_bounding_box(r,r);

	// draw the ellipse:
	body << "gsave" << endl 
	     << "  " << a << ' ' << b << ' ' << h << " cellipse" << endl;
	if (bm == Solid_filled)
	  body << "  gsave 0.6 setgray eofill grestore" << endl;
	if (bm == Dashed)
	  body << "  [dashlen] 0 setdash" << endl;
	body << "  stroke" << endl << "grestore" << endl;
	
	// output label:
	// Todo: Not implemented yet.
	if (false && lm != None) {
	  const double distance = 5.0;
	  body << a << ' ' << b << ' ' << distance << ' ' << next_angle
	       << " cellipse-pt moveto (" << next_label << ") show" << endl;
	}
	
	// generate next angle (if needed):
	if (lm == Random_angle)
	  next_angle = static_cast<double>(std::rand());
	
	// update counter and label:
	next_label = default_label(++count);
      }

      // Renders the ellipse
      // 
      //    E = { x | x^T M x + x^T m + mu <= 0 }
      //
      // where
      //
      //       [ a  h ]        [ u ]
      //   M = [      ],   m = [   ]
      //       [ h  b ]        [ v ]
      //
      // and det(M) > 0 in the current stroke and label mode.
      void write_ellipse(double a,double b,double h,
			 double u,double v,double mu)
      {
	using std::endl;
	
	// adjust bounding box (see file boundingbox.mw and my file note):
	const double tmp1 = a*b-h*h;
	const double tmp2 = ((u*(u*b-2.0*v*h)+v*v*a)/tmp1-4.0*mu)/tmp1;
	const double hw   = 0.5*std::sqrt(b*tmp2),  vw = 0.5*std::sqrt(a*tmp2);
	const double hoff = -0.5*(u*b-v*h)/tmp1, voff = 0.5*(u*h-v*a)/tmp1;
	adjust_bounding_box((std::min)(hw+hoff,-hw+hoff),
			    (std::min)(vw+voff,-vw+voff));
	adjust_bounding_box((std::max)(hw+hoff,-hw+hoff),
			    (std::max)(vw+voff,-vw+voff));

	#if 0
	// draw bounding box:
	body << "newpath " << (std::min)(hw+hoff,-hw+hoff) << " "
	     << (std::min)(vw+voff,-vw+voff) << " moveto "
	     << (std::max)(hw+hoff,-hw+hoff) << " " 
	     << (std::min)(vw+voff,-vw+voff) << " lineto "
	     << (std::max)(hw+hoff,-hw+hoff) << " "
	     << (std::max)(vw+voff,-vw+voff) << " lineto "
	     << (std::min)(hw+hoff,-hw+hoff)  << " "
	     << (std::max)(vw+voff,-vw+voff) << " lineto closepath stroke\n";
	#endif

	// begin drawing:
	body << "gsave" << endl;

	// draw the ellipse:
	body << "  " << a << ' ' << h << ' ' << b
	     << ' ' << u << ' ' << v << ' ' << mu
	     << " ellipse" << endl;
	if (bm == Solid_filled)
	  body << "  gsave 0.6 setgray eofill grestore" << endl;
	if (bm == Dashed)
	  body << "  [dashlen] 0 setdash" << endl;
	body << "  stroke" << endl;

	// output label:
	// Todo: Not implemented yet.

	// end drawing:
	body << "grestore" << endl;

	// generate next angle (if needed):
	if (lm == Random_angle)
	  next_angle = static_cast<double>(std::rand());
	
	// update counter and label:
	next_label = default_label(++count);
      }

      void adjust_bounding_box(double x,double y)
      // Make sure the bounding box is large enough to contain the point (x,y).
      {
	if (adjust_bb) {
	  bb[0] = (std::min)(x,bb[0]);
	  bb[2] = (std::max)(x,bb[2]);
	  bb[1] = (std::min)(y,bb[1]);
	  bb[3] = (std::max)(y,bb[3]);
	}
      }
      
    private: // utilities:

      std::pair<double,double> find_roots(double a,double b,double c)
      // Assuming a is nonzero, finds the roots of a x^2 + b x + c = 0.
      {
	double sd = std::sqrt(b*b-4.0*a*c);
	if (b >= 0.0)
	  sd = -sd;
	return std::pair<double,double>( (sd-b)/(2*a), 2*c/(sd-b) );
      }
      
      static std::string default_label(int count)
      {
	return tostr(count);
      }
    };
    
  } // namespace Approximate_min_ellipsoid_d_impl
  
} // namespace CGAL

#endif // CGAL_APPROX_MIN_ELL_D_DEBUG_H