void cEqAppui_PProjInc_M2CPolyn2::ComputeVal()
{
   double tmp0_ = mCompCoord[9];
   double tmp1_ = mCompCoord[10];
   double tmp2_ = cos(tmp1_);
   double tmp3_ = mCompCoord[15];
   double tmp4_ = mCompCoord[16];
   double tmp5_ = mCompCoord[17];
   double tmp6_ = sin(tmp0_);
   double tmp7_ = cos(tmp0_);
   double tmp8_ = sin(tmp1_);
   double tmp9_ = mCompCoord[11];
   double tmp10_ = mLocProjI_x * tmp3_;
   double tmp11_ = mLocProjP0_x + tmp10_;
   double tmp12_ = mLocProjJ_x * tmp4_;
   double tmp13_ = tmp11_ + tmp12_;
   double tmp14_ = mLocProjK_x * tmp5_;
   double tmp15_ = tmp13_ + tmp14_;
   double tmp16_ = mCompCoord[12];
   double tmp17_ = (tmp15_) - tmp16_;
   double tmp18_ = sin(tmp9_);
   double tmp19_ = -(tmp18_);
   double tmp20_ = -(tmp8_);
   double tmp21_ = cos(tmp9_);
   double tmp22_ = mLocProjI_y * tmp3_;
   double tmp23_ = mLocProjP0_y + tmp22_;
   double tmp24_ = mLocProjJ_y * tmp4_;
   double tmp25_ = tmp23_ + tmp24_;
   double tmp26_ = mLocProjK_y * tmp5_;
   double tmp27_ = tmp25_ + tmp26_;
   double tmp28_ = mCompCoord[13];
   double tmp29_ = (tmp27_) - tmp28_;
   double tmp30_ = mLocProjI_z * tmp3_;
   double tmp31_ = mLocProjP0_z + tmp30_;
   double tmp32_ = mLocProjJ_z * tmp4_;
   double tmp33_ = tmp31_ + tmp32_;
   double tmp34_ = mLocProjK_z * tmp5_;
   double tmp35_ = tmp33_ + tmp34_;
   double tmp36_ = mCompCoord[14];
   double tmp37_ = (tmp35_) - tmp36_;
   double tmp38_ = -(tmp6_);
   double tmp39_ = tmp7_ * tmp20_;
   double tmp40_ = tmp6_ * tmp20_;
   double tmp41_ = mCompCoord[0];
   double tmp42_ = tmp38_ * tmp19_;
   double tmp43_ = tmp39_ * tmp21_;
   double tmp44_ = tmp42_ + tmp43_;
   double tmp45_ = (tmp44_) * (tmp17_);
   double tmp46_ = tmp7_ * tmp19_;
   double tmp47_ = tmp40_ * tmp21_;
   double tmp48_ = tmp46_ + tmp47_;
   double tmp49_ = (tmp48_) * (tmp29_);
   double tmp50_ = tmp45_ + tmp49_;
   double tmp51_ = tmp2_ * tmp21_;
   double tmp52_ = tmp51_ * (tmp37_);
   double tmp53_ = tmp50_ + tmp52_;
   double tmp54_ = tmp41_ / (tmp53_);
   double tmp55_ = tmp7_ * tmp2_;
   double tmp56_ = tmp55_ * (tmp17_);
   double tmp57_ = tmp6_ * tmp2_;
   double tmp58_ = tmp57_ * (tmp29_);
   double tmp59_ = tmp56_ + tmp58_;
   double tmp60_ = tmp8_ * (tmp37_);
   double tmp61_ = tmp59_ + tmp60_;
   double tmp62_ = (tmp61_) * (tmp54_);
   double tmp63_ = mCompCoord[1];
   double tmp64_ = tmp62_ + tmp63_;
   double tmp65_ = (tmp64_) - mLocPolyn2_State_1_0;
   double tmp66_ = (tmp65_) / mLocPolyn2_State_0_0;
   double tmp67_ = tmp38_ * tmp21_;
   double tmp68_ = tmp39_ * tmp18_;
   double tmp69_ = tmp67_ + tmp68_;
   double tmp70_ = (tmp69_) * (tmp17_);
   double tmp71_ = tmp7_ * tmp21_;
   double tmp72_ = tmp40_ * tmp18_;
   double tmp73_ = tmp71_ + tmp72_;
   double tmp74_ = (tmp73_) * (tmp29_);
   double tmp75_ = tmp70_ + tmp74_;
   double tmp76_ = tmp2_ * tmp18_;
   double tmp77_ = tmp76_ * (tmp37_);
   double tmp78_ = tmp75_ + tmp77_;
   double tmp79_ = (tmp78_) * (tmp54_);
   double tmp80_ = mCompCoord[2];
   double tmp81_ = tmp79_ + tmp80_;
   double tmp82_ = (tmp81_) - mLocPolyn2_State_2_0;
   double tmp83_ = (tmp82_) / mLocPolyn2_State_0_0;
   double tmp84_ = mCompCoord[3];
   double tmp85_ = mCompCoord[4];
   double tmp86_ = mCompCoord[5];
   double tmp87_ = (tmp66_) * (tmp83_);
   double tmp88_ = mCompCoord[6];
   double tmp89_ = (tmp83_) * (tmp83_);
   double tmp90_ = (tmp66_) * (tmp66_);

  mVal[0] = ((mLocPolyn2_State_1_0 + (((1 + tmp84_) * (tmp66_) + tmp85_ * (tmp83_)) - tmp86_ * 2 * tmp90_ + tmp88_ * tmp87_ + mCompCoord[7] * tmp89_) * mLocPolyn2_State_0_0) - mLocXIm) * mLocScNorm;

  mVal[1] = ((mLocPolyn2_State_2_0 + (((1 - tmp84_) * (tmp83_) + tmp85_ * (tmp66_) + tmp86_ * tmp87_) - tmp88_ * 2 * tmp89_ + mCompCoord[8] * tmp90_) * mLocPolyn2_State_0_0) - mLocYIm) * mLocScNorm;

}

double r8_gamma ( double x )

/******************************************************************************/
/*
  Purpose:

    R8_GAMMA evaluates Gamma(X) for a real argument.

  Discussion:

    The C math library includes the GAMMA ( X ) function which should generally
    be used instead of this function.

    This routine calculates the gamma function for a real argument X.

    Computation is based on an algorithm outlined in reference 1.
    The program uses rational functions that approximate the gamma
    function to at least 20 significant decimal digits.  Coefficients
    for the approximation over the interval (1,2) are unpublished.
    Those for the approximation for 12 <= X are from reference 2.

  Licensing:

    This code is distributed under the GNU LGPL license. 

  Modified:

    11 January 2010

  Author:

    Original FORTRAN77 version by William Cody, Laura Stoltz.
    C version by John Burkardt.

  Reference:

    William Cody,
    An Overview of Software Development for Special Functions,
    in Numerical Analysis Dundee, 1975,
    edited by GA Watson,
    Lecture Notes in Mathematics 506,
    Springer, 1976.

    John Hart, Ward Cheney, Charles Lawson, Hans Maehly,
    Charles Mesztenyi, John Rice, Henry Thatcher,
    Christoph Witzgall,
    Computer Approximations,
    Wiley, 1968,
    LC: QA297.C64.

  Parameters:

    Input, double X, the argument of the function.

    Output, double R8_GAMMA, the value of the function.
*/
{
  double c[7] = {
   -1.910444077728E-03, 
    8.4171387781295E-04, 
   -5.952379913043012E-04, 
    7.93650793500350248E-04, 
   -2.777777777777681622553E-03, 
    8.333333333333333331554247E-02, 
    5.7083835261E-03 };
  double eps = 2.22E-16;
  double fact;
  int i;
  int n;
  double p[8] = {
  -1.71618513886549492533811E+00,
   2.47656508055759199108314E+01, 
  -3.79804256470945635097577E+02,
   6.29331155312818442661052E+02, 
   8.66966202790413211295064E+02,
  -3.14512729688483675254357E+04, 
  -3.61444134186911729807069E+04,
   6.64561438202405440627855E+04 };
  int parity;
  const double pi = 3.1415926535897932384626434;
  double q[8] = {
  -3.08402300119738975254353E+01,
   3.15350626979604161529144E+02, 
  -1.01515636749021914166146E+03,
  -3.10777167157231109440444E+03, 
   2.25381184209801510330112E+04,
   4.75584627752788110767815E+03, 
  -1.34659959864969306392456E+05,
  -1.15132259675553483497211E+05 };
  double res;
  const double sqrtpi = 0.9189385332046727417803297;
  double sum;
  double value;
  double xbig = 171.624;
  double xden;
  double xinf = 1.79E+308;
  double xminin = 2.23E-308;
  double xnum;
  double y;
  double y1;
//.........这里部分代码省略.........

//--------------------------------------------------------------
void AppCore::setup(const int numOutChannels, const int numInChannels,
                    const int sampleRate, const int ticksPerBuffer) {

	ofSetFrameRate(30);
	ofSetVerticalSync(true);
	//ofSetLogLevel(OF_LOG_VERBOSE);
    
	// double check where we are ...
	cout << ofFilePath::getCurrentWorkingDirectory() << endl;

    //----------------------------------- PD START-------------------------------------------
	if(!pd.init(numOutChannels, numInChannels, sampleRate, ticksPerBuffer)) {
		OF_EXIT_APP(1);
	}
    Externals::setup();
	midiChan = 1; // midi channels are 1-16

	// subscribe to receive source names
	pd.subscribe("toOf");
	pd.subscribe("env");

    pd.addReceiver(*this);   // automatically receives from all subscribed sources
	pd.ignore(*this, "env"); // don't receive from "env"
	
	pd.addMidiReceiver(*this);  // automatically receives from all channels

	pd.addToSearchPath("pd/abs");

	pd.start();
    //Patch patch = pd.openPatch("pd/somename.pd");
    //cout << patch << endl;
    //----------------------------------- PD END-------------------------------------------
    
    //----------------------------------- KINECT START -------------------------------------------
    kinect.listDevices();
    kinect.init();
    kinect.open("A00365917784047A");
    kinect.setCameraTiltAngle(0);
    grayImage.allocate(kinect.width, kinect.height);

    kinect1.init();
    kinect1.open("A00364A11700045A");
    kinect1.setCameraTiltAngle(0);
    grayImage1.allocate(kinect1.width, kinect1.height);
    
    bothKinects.allocate(kinect.height*2, kinect.width);
    combinedVideo = (unsigned char*)malloc(640 * 480 * 2 * sizeof(unsigned char*));
    
    blobs.resize(100);
    blobCenterX.resize(100);
    blobCenterXmap.resize(100);
    blobCenterY.resize(100);
    blobCenterYmap.resize(100);
    
    //----------------------------------- KINECT END -------------------------------------------

    // Setup OSC Sender
    sender.setup(HOST, PORT);
    
    allPipes = new ofPipe*[TUBE_NUM]; // an array of pointers for the objects
    persons = new ofPerson*[PERSON_NUM];
    
    //the string is printed at the top of the app
    //to give the user some feedback
    message = "loading data.xml";
    
    //we load our data file
    
    if( XML.loadFile("data.xml") ){
        message = "data.xml loaded!";
    }else{
        message = "unable to load data.xml check data/ folder";
    }
    
    XML.pushTag("document");
    newXML.addTag("document");
    newXML.pushTag("document");
    
    for(int i=0; i<TUBE_NUM; i++){
        
        XML.pushTag("tube",i);
        
        float x = XML.getValue("y",0.0);
        float y = XML.getValue("x",0.0);
        
        // rotate
        float angle = ofDegToRad(-20);
        float rX = (x*cos(angle) - y*sin(angle));
        float rY = (x*sin(angle) + y*cos(angle));
        
        
        // map to the of window size
        float mult = 6;
        x = (rX * mult + ofGetWidth() / 2) - 50;
        y = rY * mult + ofGetHeight() / 2;
        
        float radius = XML.getValue("diameter",0.0 ) / 1.8;
        float length = XML.getValue("length",0.0 );
        float height= XML.getValue("height",0.0);
//.........这里部分代码省略.........

// compute a point on ellipse from theta
void Ellipse(double theta, double *x, double *y)
{
	*x = Rx * cos(theta) + Cx; 
	*y = Ry * sin(theta) + Cy; 
}

void TacticsInstrument_AppTrueWindAngle::DrawForeground(wxGCDC* dc)
{
	wxPoint points[4];
	double data;
	double val;
	double value;
	// The default foreground is the arrow used in most dials
	wxColour cl;
	GetGlobalColor(_T("DASH2"), &cl);
	wxPen pen1;
	pen1.SetStyle(wxSOLID);
	pen1.SetColour(cl);
	pen1.SetWidth(2);
	dc->SetPen(pen1);
	GetGlobalColor(_T("DASH1"), &cl);
	wxBrush brush1;
	brush1.SetStyle(wxSOLID);
	brush1.SetColour(cl);
	dc->SetBrush(brush1);
	dc->DrawCircle(m_cx, m_cy, m_radius / 8);

	/*True Wind*/
    if (!wxIsNaN(m_ExtraValueTrue)){  //m_ExtraValueTrue = True Wind Angle; we have a watchdog for TWS; if TWS becomes NAN, TWA must be NAN as well
      dc->SetPen(*wxTRANSPARENT_PEN);

      GetGlobalColor(_T("BLUE3"), &cl);
      wxBrush brush2;
      brush2.SetStyle(wxSOLID);
      brush2.SetColour(cl);
      dc->SetBrush(brush2);

      /* this is fix for a +/-180° round instrument, when m_MainValue is supplied as <0..180><L | R>
      * for example TWA & AWA */
      if (m_MainValueTrueUnit == _T("\u00B0L"))
        data = 360 - m_MainValueTrue;
      else
        data = m_MainValueTrue;

      // The arrow should stay inside fixed limits
      if (data < m_MainValueMin) val = m_MainValueMin;
      else if (data > m_MainValueMax) val = m_MainValueMax;
      else val = data;

      value = deg2rad((val - m_MainValueMin) * m_AngleRange / (m_MainValueMax - m_MainValueMin)) + deg2rad(m_AngleStart - ANGLE_OFFSET);

      points[0].x = m_cx + (m_radius * 0.95 * cos(value - .010));
      points[0].y = m_cy + (m_radius * 0.95 * sin(value - .010));
      points[1].x = m_cx + (m_radius * 0.95 * cos(value + .015));
      points[1].y = m_cy + (m_radius * 0.95 * sin(value + .015));
      points[2].x = m_cx + (m_radius * 0.22 * cos(value + 2.8));
      points[2].y = m_cy + (m_radius * 0.22 * sin(value + 2.8));
      points[3].x = m_cx + (m_radius * 0.22 * cos(value - 2.8));
      points[3].y = m_cy + (m_radius * 0.22 * sin(value - 2.8));
      dc->DrawPolygon(4, points, 0, 0);
    }
	/* Apparent Wind*/
    if (!wxIsNaN(m_ExtraValueApp)){ //m_ExtraValueApp=AWA; we have a watchdog for AWS; if AWS becomes NAN, AWA will also be NAN ...
      dc->SetPen(*wxTRANSPARENT_PEN);

      GetGlobalColor(_T("DASHN"), &cl);
      wxBrush brush;
      brush.SetStyle(wxSOLID);
      brush.SetColour(cl);
      dc->SetBrush(brush);

      /* this is fix for a +/-180° round instrument, when m_MainValue is supplied as <0..180><L | R>
      * for example TWA & AWA */
      if (m_MainValueAppUnit == _T("\u00B0L"))
        data = 360 - m_MainValueApp;
      else
        data = m_MainValueApp;

      // The arrow should stay inside fixed limits
      if (data < m_MainValueMin) val = m_MainValueMin;
      else if (data > m_MainValueMax) val = m_MainValueMax;
      else val = data;

      value = deg2rad((val - m_MainValueMin) * m_AngleRange / (m_MainValueMax - m_MainValueMin)) + deg2rad(m_AngleStart - ANGLE_OFFSET);

      points[0].x = m_cx + (m_radius * 0.95 * cos(value - .010));
      points[0].y = m_cy + (m_radius * 0.95 * sin(value - .010));
      points[1].x = m_cx + (m_radius * 0.95 * cos(value + .015));
      points[1].y = m_cy + (m_radius * 0.95 * sin(value + .015));
      points[2].x = m_cx + (m_radius * 0.22 * cos(value + 2.8));
      points[2].y = m_cy + (m_radius * 0.22 * sin(value + 2.8));
      points[3].x = m_cx + (m_radius * 0.22 * cos(value - 2.8));
      points[3].y = m_cy + (m_radius * 0.22 * sin(value - 2.8));
      dc->DrawPolygon(4, points, 0, 0);
    }
}

	bool CCP4File::readHeader()
	{
		// first read the complete 1024 bytes of header information
		char header[1024];
		std::fstream::read(header, 1024);

		if (gcount() != 1024)
		{
			Log.error() << "CCP4File::readHeader(): File does not contain a proper CCP4 header. Aborting read." << std::endl;

			return false;
		}
		// Currently only data_mode=2 is allowed, which stores density values as 4-byte float values	
		Index data_mode = readBinValueasInt_(header, 3);
		
		if (data_mode != 2)
		{
			// try to change endianness
			swap_bytes_= true;

			data_mode = readBinValueasInt_(header, 3);
			if (data_mode != 2)
			{
				Log.error() << "CCP4File::readHeader(): Corrupt CCP4 header: data mode not supported, only 32-bit float supported" << std::endl;
				return false;
			}
		}
		
		//check if file claims to have symmetry reocrds stored	
		Size size_of_symops = readBinValueasInt_(header, 23);
		if (size_of_symops != 0)
		{
			offset_symops_ = size_of_symops;
		}
		
		// check internal ordering of coordinate axis
		col_axis_ = readBinValueasInt_(header, 16)-1;
		row_axis_ = readBinValueasInt_(header, 17)-1;
		sec_axis_ = readBinValueasInt_(header, 18)-1;

		extent_.x = (float)readBinValueasInt_(header, 0+col_axis_);
		extent_.y = (float)readBinValueasInt_(header, 0+row_axis_);
		extent_.z = (float)readBinValueasInt_(header, 0+sec_axis_);
		
		start_.x = (float)readBinValueasInt_(header, 4+col_axis_);
		start_.y = (float)readBinValueasInt_(header, 4+row_axis_);
		start_.z = (float)readBinValueasInt_(header, 4+sec_axis_);
		
		sampling_rate_.x = (float)readBinValueasInt_(header, 7);
		sampling_rate_.y = (float)readBinValueasInt_(header, 8);
		sampling_rate_.z = (float)readBinValueasInt_(header, 9);
		
		cell_dimension_.x = readBinValueasFloat_(header, 10);
		cell_dimension_.y = readBinValueasFloat_(header, 11);
		cell_dimension_.z = readBinValueasFloat_(header, 12);
		
		// Angle values of 0 don't make sense, set the Angles to 90 deg
		if (		readBinValueasFloat_(header, 13) == 0
				||	readBinValueasFloat_(header, 14) == 0
				||	readBinValueasFloat_(header, 15) == 0)
		{
			alpha_ = Angle(90.,false);
			beta_ = Angle(90.,false);
			gamma_ = Angle(90.,false);
		}
		else
		{
			alpha_ = Angle(readBinValueasFloat_(header, 13),false);
			beta_ = Angle(readBinValueasFloat_(header, 14),false);
			gamma_ = Angle(readBinValueasFloat_(header, 15),false);
		}	
		
		mean_density_ = readBinValueasFloat_(header, 21);
		space_group_ = readBinValueasInt_(header, 22);
		deviation_sigma_ = readBinValueasFloat_(header, 54);

		Log.info() << "Mean from file: " << mean_density_ << std::endl;
		Log.info() << "Sigma from file: " << deviation_sigma_ << std::endl;
		
		// convert from grid space to cartesian coordinates
		Vector3 scaled_axes(cell_dimension_.x/sampling_rate_.x,
												cell_dimension_.y/sampling_rate_.y,
												cell_dimension_.z/sampling_rate_.z);
		
		Vector3 x_tmp(scaled_axes.x, 0., 0.);
		
		Vector3 y_tmp(cos(gamma_.toRadian()), sin(gamma_.toRadian()), 0.);
		y_tmp *= scaled_axes.y;
		
		Vector3 z_tmp( cos(beta_.toRadian()), 
									(cos(alpha_.toRadian()) - cos(beta_.toRadian())*cos(gamma_.toRadian())) / sin(gamma_.toRadian()),
									0.);
		z_tmp.z = sqrt(1.0 - z_tmp.x*z_tmp.x - z_tmp.y*z_tmp.y);
		z_tmp *= scaled_axes.z;

		origin_.x = x_tmp.x * start_.x + y_tmp.x * start_.y + z_tmp.x * start_.z;
		origin_.y = y_tmp.y * start_.y + z_tmp.y * start_.z;
		origin_.z = z_tmp.z * start_.z;

		xaxis_.x = x_tmp.x * (extent_.x - 1);
//.........这里部分代码省略.........

void playerCamera(Camera *cam, Data *data, Player *p) {
  float dest[3];
  float tdest[3];
  float phi, chi, r;

  /* first, process all movement commands */
  /* that means, check for mouse input mainly */

  /* dt hack: the time since the last frame is not necessarily the game
     time, since the game maybe hasn't started yet, or was paused */
  static Uint32 last=0;
  Uint32 dt;

	if(game2->time.dt == 0) {
		dt = SDL_GetTicks() - last;
		last = SDL_GetTicks();
	} else {
		dt = game2->time.dt;
	}

  if(cam->type->freedom[CAM_FREE_R]) {
    if(game2->input.mouse1 == 1)
      cam->movement[CAM_R] += (cam->movement[CAM_R]-CLAMP_R_MIN+1) * dt / 300.0;
    if(game2->input.mouse2 == 1)
      cam->movement[CAM_R] -= (cam->movement[CAM_R]-CLAMP_R_MIN+1) * dt / 300.0;
    writeCamDefaults(cam, CAM_R);
  }

  if(cam->type->freedom[CAM_FREE_PHI]) {
    cam->movement[CAM_PHI] += - game2->input.mousex * MOUSE_CX;
    writeCamDefaults(cam, CAM_CHI);
  }
  if(cam->type->freedom[CAM_FREE_CHI]) {
    cam->movement[CAM_CHI] += game2->input.mousey * MOUSE_CY;
    writeCamDefaults(cam, CAM_PHI);
  }
  /* done with mouse movement, now clamp the camera to legel values */
  clampCam(cam);

  phi = cam->movement[CAM_PHI];
  chi = cam->movement[CAM_CHI];
  r = cam->movement[CAM_R];

  /* if the cam is coupled to player movement, change the phi accordingly */
  if(cam->type->coupled) {
    int time;
    time = game2->time.current - p->data->turn_time;
    if(time < TURN_LENGTH) {
      int dir, ldir;
      dir = p->data->dir;
      ldir = p->data->last_dir;
      if(dir == 1 && ldir == 2)
	dir = 4;
      if(dir == 2 && ldir == 1)
	ldir = 4;
      phi += ((TURN_LENGTH - time) * camAngles[ldir] + 
	      time * camAngles[dir]) / TURN_LENGTH;
    }
    else
      phi += camAngles[p->data->dir];
  }

  /* position the camera */
  dest[0] = data->posx + r * cos(phi) * sin(chi);
  dest[1] = data->posy + r * sin(phi) * sin(chi);
  dest[2] = r * cos(chi);

  /* ok, now let's calculate the new camera destination coordinates */
  /* also, perform some camera dependant movement */
  switch(cam->type->type) {
  case CAM_TYPE_CIRCLING: /* Andi-cam */
    cam->movement[CAM_PHI] += CAM_SPEED * game2->time.dt;
    tdest[0] = data->posx;
    tdest[1] = data->posy;
    tdest[2] = B_HEIGHT;
    break;
  case CAM_TYPE_FOLLOW: /* Mike-cam */
    tdest[0] = data->posx;
    tdest[1] = data->posy;
    tdest[2] = B_HEIGHT;
    break;
  case CAM_TYPE_COCKPIT: /* 1st person */
    tdest[0] = data->posx + 4.0 * dirsX[ p->data->dir ] + 2.0 * cos(phi);
    tdest[1] = data->posy + 4.0 * dirsY[ p->data->dir ] + 2.0 * sin(phi);
    tdest[2] = CAM_COCKPIT_Z;
    dest[0] = data->posx + 4.0 * dirsX[ p->data->dir ] + 0.1 * cos(phi);
    dest[1] = data->posy + 4.0 * dirsY[ p->data->dir ] + 0.1 * sin(phi);
    dest[2] = CAM_COCKPIT_Z + 0.1;
    break;
  case CAM_TYPE_MOUSE: /* mouse camera */
    tdest[0] = data->posx;
    tdest[1] = data->posy;
    tdest[2] = B_HEIGHT;
    break;
  }
  memcpy(cam->cam, dest, sizeof(cam->cam));
  memcpy(cam->target, tdest, sizeof(cam->target));
}

void LegFinder::laserCallback(std::vector<float>& laser_r, std::vector<float>& laser_t)
{
    if(laser_r.size() != laser_t.size())
    {
        std::cout << "LegFinder.->Something went wrong while getting laser readings :'(" << std::endl;
        return;
    }

    this->rec.clear();
    std::vector<float> laser_x;
    std::vector<float> laser_y;

    this->laserFilter_Mean(laser_r);
    
    for(size_t i=0; i < laser_r.size(); i++)
    {
        laser_x.push_back(laser_r[i] * cos(laser_t[i]));
        laser_y.push_back(laser_r[i] * sin(laser_t[i]));
    }

    std::vector<float> laser_flank;
    std::vector<float> flank_id0;
    std::vector<float> flank_id1;
    std::vector<bool> flank_id2;
    int ant2 = 0;
    
    for(int i= 1; i < laser_r.size(); i++)
    {
        pcl::PointXYZ cua;
        float sumax, sumay, px, py, m1, m2, ang;
        int ant = ant2;
        if(fabs(laser_r[i] - laser_r[i-1]) > FLANK_THRESHOLD)
        {
            if((pow(laser_x[ant] - laser_x[i-1], 2) + pow(laser_y[ant] - laser_y[i-1], 2)) > PIERNA_DELGADA &&
               (pow(laser_x[ant] - laser_x[i-1], 2) + pow(laser_y[ant] - laser_y[i-1], 2)) < PIERNA_GRUESA)
            {
                if(esPierna(laser_x[ant], laser_y[ant], laser_x[i-1], laser_y[i-1]) ||
                   esPierna(laser_x[ant+1], laser_y[ant+1], laser_x[i-2], laser_y[i-2]))
                {
                    sumax = 0;
                    sumay = 0;
                    for(int j= ant; j < i; j++)
                    {
                        sumax += laser_x[j];
                        sumay += laser_y[j];
                    }
                    flank_id0.push_back(sumax / (float)(i - ant));
                    flank_id1.push_back(sumay / (float)(i - ant));
                    flank_id2.push_back(false);
                }
            }
            else 
            {
                if((pow(laser_x[ant] - laser_x[i-1], 2) + pow(laser_y[ant] - laser_y[i-1], 2)) > DOS_PIERNAS_DELGADAS &&
                    (pow(laser_x[ant] - laser_x[i-1], 2) + pow(laser_y[ant] - laser_y[i-1], 2)) < DOS_PIERNAS_GRUESAS)
                {
                    if(esPierna(laser_x[ant], laser_y[ant], laser_x[i-1], laser_y[i-1]) ||
                       esPierna(laser_x[ant+1], laser_y[ant+1], laser_x[i-2], laser_y[i-2]))
                    {
                        sumax = 0;
                        sumay = 0;
                        for(int j= ant; j < i; j++)
                        {
                            sumax += laser_x[j];
                            sumay += laser_y[j];
                        }
                        cua.x = sumax / (float)(i - ant);
                        cua.y = sumay / (float)(i - ant);
                        cua.z = 2;
                        this->rec.push_back(cua);
                    }
                }
            }
            ant2 = i;
        }
        else
        {
            laser_flank.push_back(0);
        }
    }
    
    for(int i=0; i < (int)(flank_id1.size())-2; i++)
    {
        for(int j=1; j < 3; j++)
        {
            pcl::PointXYZ cua;
            float px, py;

            if((pow(flank_id0[i] - flank_id0[i+j], 2) + pow(flank_id1[i] - flank_id1[i+j], 2)) > DOS_PIERNAS_CERCAS &&
               (pow(flank_id0[i] - flank_id0[i+j], 2) + pow(flank_id1[i] - flank_id1[i+j], 2)) < DOS_PIERNAS_LEJOS)
            {
                px = (flank_id0[i] + flank_id0[i + j])/2;
                py = (flank_id1[i] + flank_id1[i + j])/2;
                if((px*px + py*py) < HORIZON_THRESHOLD)
                {
                    cua.x = px;
                    cua.y = py;
                    cua.z = 2;
                    this->rec.push_back(cua);
                    flank_id2[i] = true;
//.........这里部分代码省略.........

vec3 camera::getVector()
{
//	std::cout << camYaw << " " << camPitch << std::endl;
	return (vec3(-cos(camPitch*3.14/180.0)*sin(camYaw*3.14/180.0),sin(camPitch*3.14/180.0),-cos(camPitch*3.14/180.0)*cos(camYaw*3.14/180.0)));
}

TextRenderable::TextRenderable(const TextSymbol* symbol, const TextObject* text_object, const MapColor* color, double anchor_x, double anchor_y, bool framing_line)
 : Renderable(color)
{
	const QFont& font(symbol->getQFont());
	const QFontMetricsF& metrics(symbol->getFontMetrics());
	this->anchor_x = anchor_x;
	this->anchor_y = anchor_y;
	this->rotation = text_object->getRotation();
	scale_factor = symbol->getFontSize() / TextSymbol::internal_point_size;
	this->framing_line = framing_line;
	framing_line_width = framing_line ? (2 * 0.001 * symbol->getFramingLineHalfWidth() / scale_factor) : 0;
	
	path.setFillRule(Qt::WindingFill);	// Otherwise, when text and an underline intersect, holes appear
	
	int num_lines = text_object->getNumLines();
	for (int i=0; i < num_lines; i++)
	{
		const TextObjectLineInfo* line_info = text_object->getLineInfo(i);
		
		double line_y = line_info->line_y;
		
		double underline_x0 = 0.0;
		double underline_y0 = line_info->line_y + metrics.underlinePos();
		double underline_y1 = underline_y0 + metrics.lineWidth();
		
		int num_parts = line_info->part_infos.size();
		for (int j=0; j < num_parts; j++)
		{
			const TextObjectPartInfo& part(line_info->part_infos.at(j));
			if (font.underline())
			{
				if (j > 0)
				{
					// draw underline for gap between parts as rectangle
					// TODO: watch out for inconsistency between text and gap underline
					path.moveTo(underline_x0, underline_y0);
					path.lineTo(part.part_x,  underline_y0);
					path.lineTo(part.part_x,  underline_y1);
					path.lineTo(underline_x0, underline_y1);
					path.closeSubpath();
				}
				underline_x0 = part.part_x;
			}
			path.addText(part.part_x, line_y, font, part.part_text);
		}
	}
	
	extent = path.controlPointRect();
	extent = QRectF(scale_factor * (extent.left() - 0.5f * framing_line_width),
					scale_factor * (extent.top() - 0.5f * framing_line_width),
					scale_factor * (extent.width() + framing_line_width),
					scale_factor * (extent.height() + framing_line_width));
	if (rotation != 0)
	{
		float rcos = cos(-rotation);
		float rsin = sin(-rotation);
		
		std::vector<QPointF> extent_corners;
		extent_corners.push_back(extent.topLeft());
		extent_corners.push_back(extent.topRight());
		extent_corners.push_back(extent.bottomRight());
		extent_corners.push_back(extent.bottomLeft());
		
		for (int i = 0; i < 4; ++i)
		{
			auto x = extent_corners[i].x() * rcos - extent_corners[i].y() * rsin;
			auto y = extent_corners[i].y() * rcos + extent_corners[i].x() * rsin;
			
			if (i == 0)
				extent = QRectF(x, y, 0, 0);
			else
				rectInclude(extent, QPointF(x, y));
		}
	}
	extent = QRectF(extent.left() + anchor_x, extent.top() + anchor_y, extent.width(), extent.height());
	
	Q_ASSERT(extent.right() < 999999);	// assert if bogus values are returned
}

/*
김연아 선수의 모션을 취하는 함수
*/
void Show()
{
	sndPlaySound(TEXT("C:\\sample4.wav"), SND_ASYNC | SND_NOSTOP);
	glLoadIdentity(); //CTM 초기화

	/*
	로봇의 기본적인 관절의 움직임 범위를 제한하는 곳
	*/
	L_Arm_x = (-40) + sin(time2) * 60;//왼쪽 어깨의 각도시작은 -40상태에서 sin()함수를 사용하여 주기적인 움직임 설정
	R_Arm_x = (-80) - L_Arm_x;       //우측 어깨의 각도시작은 -80상태에서 왼쪽어깨 움직임의 반대로 설정
	R_Arm_y = -abs(cos(time2) * 10);  //우측팔뚝 각도조절(팔을 뻗는 움직임표현을위하여 어깨의 sin()함수와 반대인 cos()함수 사용)
	L_Arm_y = -abs(-cos(time2) * 10); //좌측팔뚝 각도조절(팔을 뻗는 움직임표현을위하여 어깨의 sin()함수와 반대인 cos()함수 사용)

	R_Leg_y = abs(-sin(time) * 30 - 30); //우측종아리 각도조절(abs절대값을 줌으로써 종아리가 앞으로 꺾이지 않는 한계점을 설정)
	L_Leg_y = abs(sin(time) * 30 - 30);  //좌측종아리 각도조절(abs절대값을 줌으로써 종아리가 앞으로 꺾이지 않는 한계점을 설정)
	R_Leg_x = sin(time) * 60;          //우측다리는 60도 각도까지 움직이되 sin()함수를 사용하여 주기적인 움직임 설정
	L_Leg_x = -R_Leg_x;             //좌측다리는 우측다리반대로 60도 각도까지 움직이되 sin()함수를 사용하여 주기적인 움직임 설정

	////////////////display////////////////

	cyl = gluNewQuadric(); //실린더 객체 생성
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //초기화
	glMatrixMode(GL_MODELVIEW); //모드 설정

	DrawGround(); //지면 호출
	glLoadIdentity(); //CTM 초기화
	glRotatef(-230.0, 0, 1, 0); //y축기준으로 회전

	/*
	로봇이 피겨동작시 몸이 틀어지는 것을 표현
	*/
	glRotatef(sin(time) * 7, 0, 0, 1); //z축기준으로 7도 까지 각도틀어짐 (sin()함수를 사용하여 주기적인 움직임 설정)
	glRotatef(sin(time) * 7, 0, 1, 0); //y축으로 7도 까지 각도틀어짐 (sin()함수를 사용하여 주기적인 움직임 설정)

	//로봇 몸체 각도 조절
	glTranslatef(0.0, 0.18, 0.0); //y축으로 이동
	glRotatef(80, 1, 0, 0);        //x축 기준으로 회전

	glTranslatef(0.0, 0.5, 0.0);//최초 위치 

	glPushMatrix(); // 처음 저장 위치

	DrawBody(0, 0, 0, 0); // 몸통 함수 호출

	glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
	glPushMatrix();// 처음 저장 좌표 다시 저장

	Drawneck(); // 목함수 호출

	glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
	glPushMatrix();// 처음 저장 좌표 다시 저장

	//머리 위치 설정
	glRotatef(-75, 1, 0, 0); //x축기준으로 회전(머리를 위쪽으로 돌리기)
	glTranslatef(0.0, -0.02, 0.0); //y축으로 이동 (머리 시작점)

	DrawHead(); // 머리 호출

	glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
	glPushMatrix();// 처음 저장 좌표 다시 저장

	//우측전체팔 위치 설정


	DrawR_Arm((R_Arm_y + 30), 1, 0, 0);  //우측팔호출
	DrawR_Hand(-(R_Arm_y - 15), 1, 0, 0); //우측팔뚝

	glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
	glPushMatrix();// 처음 저장 좌표 다시 저장

	//좌측전체팔 위치 설정
	glTranslatef(0.0, -0.16, -0.04);//y축,z축으로 이동(좌측팔 시작점)
	glRotatef(40, 0, 0, 1);     //z축 기준으로 회전

	DrawL_Arm((L_Arm_y + 30), 1, 0, 0);   //좌측팔호출	
	DrawL_Hand(-(L_Arm_y - 15), 1, 0, 0);  //좌측팔뚝

	glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
	glPushMatrix();// 처음 저장 좌표 다시 저장

	//좌측전체 다리 위치 설정
	glTranslatef(0.0, -0.45, -0.25);//y축,z축으로 이동(좌측다리 시작점)
	glRotatef(-90, 1, 0, 0);  //x축 기준으로 회전

	DrawL_Legs(-30, 1, 0, 0);  //좌측다리
	DrawL_foot(10, 1, 0, 0);  //좌측종아리		 

	glPopMatrix(); // 처음 저장 좌표로 돌아 간다.
	glPushMatrix();// 처음 저장 좌표 다시 저장

	//우측전체 다리 위치 설정
	glTranslatef(0.0, -0.5, -0.5);//y축,z축으로 이동(우측다리 시작점)
	glRotatef(-90, 1, 0, 0); //x축 기준으로 회전

	DrawR_Legs(160, 1, 0, 0);  //우측다리
	DrawR_foot(R_Leg_y, 1, 0, 0);  //우측종아리		

//.........这里部分代码省略.........

IMPALGEBRA_BEGIN_NAMESPACE

Vector3D SphericalVector3D::get_cartesian_coordinates() {
  return Vector3D(v_[0] * cos(v_[2]) * sin(v_[1]),
                  v_[0] * sin(v_[2]) * sin(v_[1]), v_[0] * cos(v_[1]));
}

void compute_projections(const Particles& all_particles,
                         const Particles& lig_particles,
                         unsigned int projection_number,
                         double pixel_size, double resolution,
                         boost::ptr_vector<Projection>& projections,
                         int image_size) {

  // get coordinates and mass
  IMP::algebra::Vector3Ds all_points(all_particles.size());
  for (unsigned int i = 0; i < all_particles.size(); i++) {
    all_points[i] = core::XYZ(all_particles[i]).get_coordinates();
  }

  IMP::algebra::Vector3Ds lig_points(lig_particles.size());
  std::vector<double> lig_mass(lig_particles.size());
  for (unsigned int i = 0; i < lig_particles.size(); i++) {
    lig_points[i] = core::XYZ(lig_particles[i]).get_coordinates();
    lig_mass[i] = atom::Mass(lig_particles[i]).get_mass();
  }

  int axis_size = image_size;
  // use image_size if given
  if (!(image_size > 0)) {
    double max_dist = compute_max_distance(all_points);
    static IMP::em::KernelParameters kp(resolution);
    double wrap_length = 2 * kp.get_rkdist() + 1.0;
    axis_size =
        (int)((max_dist + 2 * wrap_length + 2 * pixel_size) / pixel_size + 2);
    if (axis_size <= image_size) axis_size = image_size;
  }

  // storage for rotated points
  IMP::algebra::Vector3Ds rotated_points(all_points.size());
  IMP::algebra::Vector3Ds rotated_ligand_points(lig_points.size());
  // points on a sphere
  IMP::algebra::SphericalVector3Ds spherical_coords;
  quasi_evenly_spherical_distribution(projection_number, spherical_coords, 1.0);

  for (unsigned int i = 0; i < spherical_coords.size(); i++) {
    // convert sphere coordinate to rotation
    IMP::algebra::SphericalVector3D v = spherical_coords[i];
    double cy = cos(v[1] / 2.0);
    double cz = cos(v[2] / 2.0);
    double sy = sin(v[1] / 2.0);
    double sz = sin(v[2] / 2.0);
    // this is a rotation about z axis by an angle v[2]
    // followed by rotation about y axis by an angle v[1]
    IMP::algebra::Rotation3D r(cy * cz, sy * sz, sy * cz, cy * sz);

    // rotate points
    for (unsigned int p_index = 0; p_index < all_points.size(); p_index++)
      rotated_points[p_index] = r * all_points[p_index];
    for (unsigned int p_index = 0; p_index < lig_points.size(); p_index++)
      rotated_ligand_points[p_index] = r * lig_points[p_index];

    // project
    IMP_UNIQUE_PTR<Projection> p(
        new Projection(rotated_points, rotated_ligand_points,
                       lig_mass, pixel_size, resolution, axis_size));
    p->set_rotation(r);
    p->set_axis(IMP::algebra::Vector3D(v.get_cartesian_coordinates()));
    p->set_id(i);
    // rasmol
    // std::cout << "#projection " << i+1 <<"\nreset\nrotate Z "
    //<< IMP_RAD_2_DEG(v[2]);
    // std::cout << "\nrotate Y -" << IMP_RAD_2_DEG(v[1]) << std::endl;
    // chimera
    // std::cout << "#projection " << i+1
    //<<"\nreset;turn x 180; turn z -" << IMP_RAD_2_DEG(v[2]);
    // std::cout << ";turn y -" << IMP_RAD_2_DEG(v[1])
    //<< ";wait;sleep 3;"<< std::endl;
    // string file_name = "projection_" +
    //   string(boost::lexical_cast<string>(i+1)) + ".pgm";
    // p.write_PGM(file_name);
    projections.push_back(p.release());
  }
}

QString PageItem_OSGFrame::getPDFMatrix(QString viewName)
{
    struct viewDefinition defaultView;
    defaultView = viewMap[viewName];
    osg::Vec3d trackerCenter = defaultView.trackerCenter;
    osg::Vec3d cameraPosition = defaultView.cameraPosition;
    osg::Vec3d cameraUp = defaultView.cameraUp;
    double viewx, viewy, viewz;
    double leftx, lefty, leftz;
    double upx, upy, upz;
    double transx, transy, transz;
    double roll = 0.0;
    double roo = getDistance(cameraPosition, trackerCenter);
    cameraPosition.normalize();
    cameraUp.normalize();
    viewx = -cameraPosition[0];
    viewy = -cameraPosition[1];
    viewz = -cameraPosition[2];
    if (viewx == 0.0 && viewy == 0.0 && viewz == 0.0)
    {
        viewy = 1.0;
    }
    leftx = -1.0f;
    lefty =  0.0f;
    leftz =  0.0f;
    if (viewz < 0.0) /* top view*/
    {
        upx = 0.0f;
        upy = 1.0f;
        upz = 0.0f;
    }
    else /* bottom view*/
    {
        upx = 0.0f;
        upy =-1.0f;
        upz = 0.0f;
    }
    if ( fabs(viewx) + fabs(viewy) != 0.0f) /* other views than top and bottom*/
    {
        cameraUp.normalize();
        upx = cameraUp[0];
        upy = cameraUp[1];
        upz = cameraUp[2];
        leftx = viewz*upy - viewy*upz;
        lefty = viewx*upz - viewz*upx;
        leftz = viewy*upx - viewx*upy;
        normalized(leftx, lefty, leftz);
    }
    /* apply camera roll*/
    {
        double leftxprime, leftyprime, leftzprime;
        double upxprime, upyprime, upzprime;
        double sinroll, cosroll;
        sinroll =  sin((roll/180.0f)*M_PI);
        cosroll =  cos((roll/180.0f)*M_PI);
        leftxprime = leftx*cosroll + upx*sinroll;
        leftyprime = lefty*cosroll + upy*sinroll;
        leftzprime = leftz*cosroll + upz*sinroll;
        upxprime = upx*cosroll + leftx*sinroll;
        upyprime = upy*cosroll + lefty*sinroll;
        upzprime = upz*cosroll + leftz*sinroll;
        leftx = leftxprime;
        lefty = leftyprime;
        leftz = leftzprime;
        upx = upxprime;
        upy = upyprime;
        upz = upzprime;
    }
    /* translation vector*/
    roo = fabs(roo);
    if (roo == 0.0)
    {
        roo = 0.000000000000000001;
    }
    transx = trackerCenter[0] - roo*viewx;
    transy = trackerCenter[1] - roo*viewy;
    transz = trackerCenter[2] - roo*viewz;
    if (fabs(leftx) < 0.0000001)
        leftx = 0.0;
    if (fabs(lefty) < 0.0000001)
        lefty = 0.0;
    if (fabs(leftz) < 0.0000001)
        leftz = 0.0;
    if (fabs(upx) < 0.0000001)
        upx = 0.0;
    if (fabs(upy) < 0.0000001)
        upy = 0.0;
    if (fabs(upz) < 0.0000001)
        upz = 0.0;
    if (fabs(transx) < 0.0000001)
        transx = 0.0;
    if (fabs(transy) < 0.0000001)
        transy = 0.0;
    if (fabs(transz) < 0.0000001)
        transz = 0.0;
    QString ret = "";
    QString desc4 = "%1 %2 %3 ";
    ret += desc4.arg(leftx).arg(lefty).arg(leftz);
    ret += desc4.arg(upx).arg(upy).arg(upz);
    ret += desc4.arg(viewx).arg(viewy).arg(viewz);
//.........这里部分代码省略.........

void camera::moveCameraUp(const float& dir)
{
	float rad=(camPitch+dir)*M_PI/180.0;
	loc.v[1]+=sin(rad)*speed;	
}

		inline vector2 rotate(T angle) const
		{
			return vector2(x * cos(angle) - y * sin(angle), y * cos(angle) + x * sin(angle));
		}

static void  geod_for(void) {
      double d,sind,u,V,X,ds,cosds,sinds,ss,de;
	  ellipse = 1;
      ss = 0.;

      if (ellipse) {
            d = geod_S / (D * geod_a);
            if (signS) d = -d;
            u = 2. * (s1 - d);
            V = cos(u + d);
            X = c2 * c2 * (sind = sin(d)) * cos(d) * (2. * V * V - 1.);
            ds = d + X - 2. * P * V * (1. - 2. * P * cos(u)) * sind;
            ss = s1 + s1 - ds;
      } else {
            ds = geod_S / geod_a;
            if (signS) ds = - ds;
      }
      cosds = cos(ds);
      sinds = sin(ds);
      if (signS) sinds = - sinds;
      al21 = N * cosds - sinth1 * sinds;
      if (merid) {
            phi2 = atan( tan(HALFPI + s1 - ds) / onef);
            if (al21 > 0.) {
                  al21 = PI;
                  if (signS)
                        de = PI;
                  else {
                        phi2 = - phi2;
                        de = 0.;
                  }
            } else {
                  al21 = 0.;
                  if (signS) {
                        phi2 = - phi2;
                        de = 0;
                  } else
                        de = PI;
            }
      } else {
            al21 = atan(M / al21);
            if (al21 > 0)
                  al21 += PI;
            if (al12 < 0.)
                  al21 -= PI;
            al21 = adjlon(al21);
            phi2 = atan(-(sinth1 * cosds + N * sinds) * sin(al21) /
                        (ellipse ? onef * M : M));
            de = atan2(sinds * sina12 ,
                       (costh1 * cosds - sinth1 * sinds * cosa12));