コード例 #1
0
ファイル: yadro.cpp プロジェクト: Ztaz/cpp
void Yadro::getStartVerTexArrays(GLfloat sferaV[80][3][3], int start)
{
    const GLfloat pi=3.141593, k=pi/180;

    GLfloat x21 = sferaV[start][0][0]; GLfloat x22 = sferaV[start][1][0];  GLfloat x23 = sferaV[start][2][0];
    GLfloat y21 = sferaV[start][0][1]; GLfloat y22 = sferaV[start][1][1];  GLfloat y23 = sferaV[start][2][1];
    GLfloat z21 = sferaV[start][0][2]; GLfloat z22 = sferaV[start][1][2];  GLfloat z23 = sferaV[start][2][2];

    GLfloat x2 = (x21+x22+x23)/3;
    GLfloat y2 = (y21+y22+y23)/3;
    GLfloat z2 = (z21+z22+z23)/3;

    GLfloat r2      = sqrt(x2*x2 + y2*y2 + z2*z2);
    GLfloat theta2  = acos(z2/r2);
    GLfloat phi2    = atan2(y2,x2);


    for (int i = 0; i < 6; i++) {

        GLfloat x1 = VertexArrayYadro[i][0];
        GLfloat y1 = VertexArrayYadro[i][1];
        GLfloat z1 = VertexArrayYadro[i][2];

        GLfloat r1 = sqrt(x1*x1 + y1*y1 + z1*z1);
        GLfloat theta1 = acos(z1/r1);
        GLfloat phi1 = atan2(y1,x1);

        if (delta2 == 0.0f) {
            // Установка начальных параметров запуска
            delta2 = 1.0f;
            shag   = 2;
            deltaR = 0.15f;

            deltaTh = fabs(theta2 - theta1);
            deltaPh = fabs( phi2  -  phi1 );
            deltaR  = fabs(  r2   -   r1  );

            deltaTh = deltaTh / (GLfloat)shag;
            deltaPh = deltaPh / (GLfloat)shag;

            if ( theta1 < theta2) {
                signTh  = 1;
            } else {
                signTh  = 0;
            }

            if (  phi1  <  phi2 ) {
                signPh  = 1;
            } else {
                signPh  = 0;
            }
        }

        if ( signTh  == 1 ) {
            theta1+=deltaTh;
        } else {
            theta1-=deltaTh;
        }

        if ( signPh  == 1 ) {
            phi1+=deltaPh;
        } else {
            phi1-=deltaPh;
        }

        r1+= deltaR;

        if (shag > 0) {
            VertexArrayYadro[i][0]=r1*sin(theta1)*cos(phi1);
            VertexArrayYadro[i][1]=r1*sin(theta1)*sin(phi1);
            VertexArrayYadro[i][2]=r1*cos(theta1);
            int h = 43;
            if ( i == 5 ) {
                shag--;
            }
        }
        if (shag == 0 && pli < 80) {
            //delta2 = 0.0f;
            //sfera.kill(pli+plifinish);
            //pli+=plifinish;
        }
    }
}
コード例 #2
0
ファイル: sfml-app.cpp プロジェクト: Impulsation/sfml
int main()
{
	sf::RenderWindow window;
	sf::Texture circle_texture;
	sf::Sprite circle;
	sf::Clock clock;
	sf::Vector2f circle_position;
	sf::Vector2i mouse_position;
	sf::Vector2i destination;

	bool stop_circle = true;
	float angle;
	float move_x;
	float move_y;
	float distance;
	float distance_x;
	float distance_y;

	window.create(sf::VideoMode(WIDTH, HEIGHT), "SFML Movement");

	if (!circle_texture.loadFromFile("circle.png"))
		std::cout << "Error: could not load player image" << std::endl;

	circle.setTexture(circle_texture);
	circle.setOrigin(16, 16);
	circle.setPosition(WIDTH/2, HEIGHT/2);

	while (window.isOpen())
	{
		sf::Event event;
		while (window.pollEvent(event))
		{
			switch (event.type)
			{
				case sf::Event::Closed:
					window.close();
					break;
				case sf::Event::MouseButtonPressed:
					if (event.mouseButton.button == sf::Mouse::Right)
					{
						stop_circle = false;
						destination = sf::Mouse::getPosition(window);

						// get the angle between the circle and mouse click using arc tangent
						angle = atan2(destination.y - circle_position.y, destination.x - circle_position.x);
					}
					break;
				case sf::Event::KeyPressed:
					if (event.key.code == sf::Keyboard::S)
					{
						stop_circle = true;
					}
					break;
			}
		}

		// Vector to hold circle position
		circle_position = circle.getPosition();

		// Current mouse position
		mouse_position = sf::Mouse::getPosition(window);

		// Distance
		distance_x = destination.x - circle_position.x;
		distance_y = destination.y - circle_position.y;
		distance = sqrt(distance_x * distance_x + distance_y * distance_y);

		// Frames and movement
		if (stop_circle == false)
		{
			move_x = cos(angle);
			move_y = sin(angle);

			circle.move(move_x, move_y);

			if (distance < 2)
				stop_circle = true;
		}
		//test
		system("clear");
		std::cout << "Circle X: " << circle_position.x << " Circle Y: " << circle_position.y << std::endl;
		std::cout << "Mouse X: " << mouse_position.x << " Mouse Y: " << mouse_position.y << std::endl;
		std::cout << "Destination " << destination.x << ", " << destination.y << std::endl;
		std::cout << "Angle: " << angle * 180 / PI << std::endl;
		std::cout << "move_x: " << move_x << " move_y: " << move_y << std::endl;
		float x = destination.x - circle_position.x;
		float y = destination.y - circle_position.y;
		std::cout << "Distance: " << sqrt(x * x + y * y) << std::endl;

		window.draw(circle);
		window.display();
		window.clear();
	}
}
コード例 #3
0
ファイル: B0_cor.c プロジェクト: DanIverson/OpenVnmrJ
/*** PROGRAM BEGIN *************************************/
int main(int argc, char *argv[])
{
/*** VARIABLE DECLARATION ******************************/
     FILE *in_file,*out_file,*freq_file;
     double real,imag,phi,abs_data;
     float sw,delta_f;
     int data_int;
     short data_short;
     int i,j,lsfid,no_bytes;
     int bit[16],n,file_status,fs;
     double x;
     int nblks, npts;
     union u_tag {
      float fval;
      int   ival;
     } uval;


/*** CONTROL OF INPUT PARAMETERS ***********************/
     if (argc<2)
         {
         printf("\nName of input file was not passed!\n");
         exit (3);
         }  

     if (argc<3)
         {
         printf("\nName of output file was not passed!\n");
         exit (3);
         }

     if (argc<4)
         {
         printf("\nName of frequency file was not passed!\n");
         exit (3);
         }

     if ((in_file=fopen(argv[1],"rb"))==NULL)
        {
        printf("\nCan not open input file");
        exit (3);
        }

     rewind(in_file);  


     if (fread(&datafilehead,sizeof(datafilehead),1,in_file) != 1)
         {
         fprintf (stderr, "Error in reading input data (datafilehead)\n");
         exit (3);
         }  


     if ((out_file=fopen(argv[2],"wb"))==NULL)
        {
        printf("\nCan not open output file");
        exit (3);
        }

     rewind(out_file);   

     if (fwrite(&datafilehead,sizeof(datafilehead),1,out_file) != 1)
         {
         fprintf (stderr, "Error in writting output data (datafilehead)\n");
         exit (3);
         }

     if ((freq_file=fopen(argv[3],"rb"))==NULL)
        {
        printf("\nCan not open frequency file");
        exit (3);
        }

     fscanf(freq_file,"%f",&sw);
     fscanf(freq_file,"%d",&lsfid);

     no_bytes = ntohl(datafilehead.ebytes);
     

/*** FILE STATUS  ********************************/

     file_status = ntohs(datafilehead.status);
     fs = file_status;
     
     for (n=15;n>=0;n--)
         {
         x = pow(2.0,(double)n);
         bit[n] = 0;
         if (fs >= (int)x)
            {
            fs += -(int)x;
            bit[n] = 1;
            }
         }

     fprintf (stderr, "\n******* B0 DRIFT CORRECTION ********\n");
     fprintf (stderr, "FILE STATUS = %d\n",file_status);
          
     if (bit[3] == 0)
        {
        fprintf (stderr, "bit[3] = %d   Type of data: INTEGER\n",bit[3]);
        }
     if (bit[3] == 1)
        {
        fprintf (stderr, "bit[3] = %d   Type of data: FLOATING POINT\n",bit[3]);
        }


/*** FREQUENCY SHIFT CORRECTION  *******************/

     fprintf (stderr, "sw = %8.1f Hz   lsfid = %d\n",sw,lsfid);
     fprintf (stderr,"\n Spectrum\t Frequency\n");

     nblks = htonl(datafilehead.nblocks);
     npts = htonl(datafilehead.np);
     for (i=0;i<nblks;++i)
         {

         fscanf(freq_file,"%f",&delta_f);
         fprintf(stderr," spect[%3d]\t%+7.3f Hz\n",i+1,delta_f);

         if (fread(&datablockhead,sizeof(datablockhead),1,in_file) != 1)
            {
            fprintf (stderr, "Error in reading input data (datablockhead)\n");
            exit (3);
            }  

         if (fwrite(&datablockhead,sizeof(datablockhead),1,out_file) != 1)
            {
            fprintf (stderr, "Error in writting output data (datablockhead)\n");
            exit (3);
            }

         for (j=0;j<npts/2;++j)
             {
             if (no_bytes == 2)
             {
                if (fread(&data_short,no_bytes,1,in_file) != 1)
                   {
                   fprintf (stderr, "Error in reading input data (short)\n");
                   exit (3);
                   }
                real=(double) ntohs(data_short);
 
                if (fread(&data_short,no_bytes,1,in_file) != 1)
                   {
                   fprintf (stderr, "Error in reading input data (short)\n");
                   exit (3);
                   }
                imag=(double) ntohs(data_short);
               
             }
             else
             {
                if (fread(&data_int,no_bytes,1,in_file) != 1)
                   {
                   fprintf (stderr, "Error in reading input data (int/float)\n");
                   exit (3);
                   }
                uval.ival = ntohl(data_int);
                if (bit[3] == 0)
                   real = (double) uval.ival;
                else
                   real = (double) uval.fval;
                if (fread(&data_int,no_bytes,1,in_file) != 1)
                   {
                   fprintf (stderr, "Error in reading input data (int/float)\n");
                   exit (3);
                   }
                uval.ival = ntohl(data_int);
                if (bit[3] == 0)
                   imag = (double) uval.ival;
                else
                   imag = (double) uval.fval;
             }             

             phi=atan2(imag,real);

             phi=phi+2.0* M_PI *delta_f*(j-lsfid)/sw;

             abs_data=sqrt(real*real+imag*imag);

             real=abs_data*cos(phi);  
             imag=abs_data*sin(phi);   

             if (no_bytes == 2)
                {
                data_short = (short)real;
                data_short = htons(data_short);
                if (fwrite(&data_short,no_bytes,1,out_file) != 1)
                   {
                   fprintf (stderr, "Error in writting output data (short)\n");
                   exit (3);
                   }     
 
                data_short = (short)imag;
                data_short = htons(data_short);
                if (fwrite(&data_short,no_bytes,1,out_file) != 1)
                   {
                   fprintf (stderr, "Error in writting output data (short)\n");
                   exit (3);
                   }
                }
              else
                {
                     
                if (bit[3] == 0)
                   uval.ival = (int)real;
                else
                   uval.fval = (float) real;
                data_int = htonl(uval.ival);
                if (fwrite(&data_int,no_bytes,1,out_file) != 1)
                   {
                   fprintf (stderr, "Error in writting output data (int/float)\n");
                   exit (3);
                   }     
                if (bit[3] == 0)
                   uval.ival = (int)imag;
                else
                   uval.fval = (float) imag;
                data_int = htonl(uval.ival);
 
                if (fwrite(&data_int,no_bytes,1,out_file) != 1)
                   {
                   fprintf (stderr, "Error in writting output data (int/float)\n");
                   exit (3);
                   }          
                }
            }
        }
   fprintf (stderr, "\nB0 CORRECTION WAS PERFORMED!\n");
   
/*** CLOSING FILES *************************************/                                 

      if (fclose(in_file) != 0) 
         {
         printf ("Error closing input file"); 
         exit (3);      
         }

      if (fclose(out_file) != 0) 
         {
         printf ("Error closing output file"); 
         exit (3);      
         }   
   exit(EXIT_SUCCESS);
}
コード例 #4
0
ファイル: cloud_to_scan.cpp プロジェクト: eitanme/ni
    void callback(const PointCloud::ConstPtr& cloud)
    {
        sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
        NODELET_DEBUG("Got cloud");
        //Copy Header
        output->header = cloud->header;
        output->header.frame_id = output_frame_id_;
        output->angle_min = -M_PI/2;
        output->angle_max = M_PI/2;
        output->angle_increment = M_PI/180.0/2.0;
        output->time_increment = 0.0;
        output->scan_time = 1.0/30.0;
        output->range_min = 0.45;
        output->range_max = 10.0;

        uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment);
        output->ranges.assign(ranges_size, output->range_max + 1.0);

        //pcl::PointCloud<pcl::PointXYZ> cloud;
        //pcl::fromROSMsg(*input, cloud);

        for (PointCloud::const_iterator it = cloud->begin(); it != cloud->end(); ++it)
        {
            const float &x = it->x;
            const float &y = it->y;
            const float &z = it->z;

            /*    for (uint32_t u = std::max((uint32_t)u_min_, 0U); u < std::min((uint32_t)u_max_, cloud.height - 1); u++)
              for (uint32_t v = 0; v < cloud.width -1; v++)
              {
                //NODELET_ERROR("%d %d,  %d %d", u, v, cloud.height, cloud.width);
                pcl::PointXYZ point = cloud.at(v,u); ///WTF Column major?
                const float &x = point.x;
                const float &y = point.y;
                const float &z = point.z;
            */

            if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
            {
                NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
                continue;
            }
            if (-y > max_height_ || -y < min_height_)
            {
                NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", x, min_height_, max_height_);
                continue;
            }
            double angle = -atan2(x, z);
            if (angle < output->angle_min || angle > output->angle_max)
            {
                NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max);
                continue;
            }
            int index = (angle - output->angle_min) / output->angle_increment;
            //printf ("index xyz( %f %f %f) angle %f index %d\n", x, y, z, angle, index);
            double range_sq = z*z+x*x;
            if (output->ranges[index] * output->ranges[index] > range_sq)
                output->ranges[index] = sqrt(range_sq);


        }

        pub_.publish(output);
    }
コード例 #5
0
ファイル: Whistle.cpp プロジェクト: aaven/urMus
StkFloat Whistle :: tick( unsigned int )
{
  StkFloat soundMix, tempFreq;
  StkFloat envOut = 0, temp, temp1, temp2, tempX, tempY;
  double phi, cosphi, sinphi;
  double gain = 0.5, mod = 0.0;

  if ( --subSampCount_ <= 0 )	{
    tempVectorP_ = pea_.getPosition();
    subSampCount_ = subSample_;
    temp = bumper_.isInside( tempVectorP_ );
#ifdef WHISTLE_ANIMATION
    frameCount += 1;
    if ( frameCount >= (1470 / subSample_) ) {
      frameCount = 0;
      printf("%f %f %f\n",tempVectorP_->getX(),tempVectorP_->getY(),envOut);
      fflush(stdout);
    }
#endif
    envOut = envelope_.tick();

    if (temp < (BUMP_RADIUS + PEA_RADIUS)) {
      tempX = envOut * tickSize_ * 2000 * noise_.tick();
      tempY = -envOut * tickSize_ * 1000 * (1.0 + noise_.tick());
      pea_.addVelocity( tempX, tempY, 0 ); 
      pea_.tick( tickSize_ );
    }

    mod  = exp(-temp * 0.01);	  // exp. distance falloff of fipple/pea effect
    temp = onepole_.tick(mod);	// smooth it a little
    gain = (1.0 - (fippleGainMod_*0.5)) + (2.0 * fippleGainMod_ * temp);
    gain *= gain;	              // squared distance/gain
    //    tempFreq = 1.0				//  Normalized Base Freq
    //			+ (fippleFreqMod_ * 0.25) - (fippleFreqMod_ * temp) // fippleModulation 
    //			- (blowFreqMod_) + (blowFreqMod_ * envOut); // blowingModulation
    // short form of above
    tempFreq = 1.0 + fippleFreqMod_*(0.25-temp) + blowFreqMod_*(envOut-1.0);
    tempFreq *= baseFrequency_;

    sine_.setFrequency(tempFreq);

    tempVectorP_ = pea_.getPosition();
    temp = can_.isInside(tempVectorP_);
    temp  = -temp;       // We know (hope) it's inside, just how much??
    if (temp < (PEA_RADIUS * 1.25)) {
      pea_.getVelocity( &tempVector_ );  // This is the can/pea collision
      tempX = tempVectorP_->getX();     // calculation.  Could probably
      tempY = tempVectorP_->getY();     // simplify using tables, etc.
      phi = -atan2(tempY,tempX);

      cosphi = cos(phi);
      sinphi = sin(phi);
      temp1 = (cosphi*tempVector_.getX()) - (sinphi*tempVector_.getY());
      temp2 = (sinphi*tempVector_.getX()) + (cosphi*tempVector_.getY());
      temp1 = -temp1;
      tempX = (cosphi*temp1) + (sinphi*temp2);
      tempY = (-sinphi*temp1) + (cosphi*temp2);
      pea_.setVelocity(tempX, tempY, 0);
      pea_.tick(tickSize_);
      pea_.setVelocity( tempX*canLoss_, tempY*canLoss_, 0 );
      pea_.tick(tickSize_);
    }

    temp = tempVectorP_->getLength();	
    if (temp > 0.01) {
      tempX = tempVectorP_->getX();
      tempY = tempVectorP_->getY();
      phi = atan2( tempY, tempX );
      phi += 0.3 * temp / CAN_RADIUS;
      cosphi = cos(phi);
      sinphi = sin(phi);
      tempX = 3.0 * temp * cosphi;
      tempY = 3.0 * temp * sinphi;
    }
    else {
      tempX = 0.0;
      tempY = 0.0;
    }

    temp = (0.9 + 0.1*subSample_*noise_.tick()) * envOut * 0.6 * tickSize_;
    pea_.addVelocity( temp * tempX, (temp*tempY) - (GRAVITY*tickSize_), 0 );
    pea_.tick( tickSize_ );

    // bumper_.tick(0.0);
  }

  temp = envOut * envOut * gain / 2;
  soundMix = temp * ( sine_.tick() + ( noiseGain_*noise_.tick() ) );
  lastFrame_[0] = 0.20 * soundMix; // should probably do one-zero filter here

  return lastFrame_[0];
}
コード例 #6
0
TEST(ReachabilitySphere, getPrincipalComponents)
{
    ReachabilitySphere sphere;
    std::vector<Vector> vectors;
    std::vector<Vector> components;

    // a line in x direction should give the main principal component lying in x direction
    vectors.push_back(Vector(0.0, 0.0, 0.0));
    vectors.push_back(Vector(1.0, 0.0, 0.0));
    vectors.push_back(Vector(2.0, 0.0, 0.0));
    vectors.push_back(Vector(3.0, 0.0, 0.0));
    
    components = sphere.getPrincipalComponents(vectors);

    ASSERT_TRUE(components[2].x == 1.0);
    ASSERT_TRUE(components[2].y == 0.0);
    ASSERT_TRUE(components[2].z == 0.0);

    vectors.clear();

    // a line in y direction should give the main principal component lying in y direction
    vectors.push_back(Vector(0.0, 0.0, 0.0));
    vectors.push_back(Vector(0.0, 1.0, 0.0));
    vectors.push_back(Vector(0.0, 2.0, 0.0));
    vectors.push_back(Vector(0.0, 3.0, 0.0));
    
    components = sphere.getPrincipalComponents(vectors);

    ASSERT_TRUE(components[2].x == 0.0);
    ASSERT_TRUE(components[2].y == 1.0);
    ASSERT_TRUE(components[2].z == 0.0);

    vectors.clear();

    // a line in z direction should give the main principal component lying in z direction
    vectors.push_back(Vector(0.0, 0.0, 0.0));
    vectors.push_back(Vector(0.0, 0.0, 1.0));
    vectors.push_back(Vector(0.0, 0.0, 2.0));
    vectors.push_back(Vector(0.0, 0.0, 3.0));
    
    components = sphere.getPrincipalComponents(vectors);

    ASSERT_TRUE(components[2].x == 0.0);
    ASSERT_TRUE(components[2].y == 0.0);
    ASSERT_TRUE(components[2].z == 1.0);

    vectors.clear();

    // filling just x and y directions should give the least principal component in z direction
    vectors.push_back(Vector(1.0, 2.0, 0.0));
    vectors.push_back(Vector(2.0, 1.0, 0.0));
    vectors.push_back(Vector(1.0, 0.0, 0.0));
    vectors.push_back(Vector(0.0, 1.0, 0.0));
    
    components = sphere.getPrincipalComponents(vectors);

    ASSERT_TRUE(components[0].x == 0.0);
    ASSERT_TRUE(components[0].y == 0.0);
    ASSERT_TRUE(components[0].z == 1.0);

    vectors.clear();

    // filling just y and z directions should give the least principal component in x direction
    vectors.push_back(Vector(0.0, 2.0, 0.0));
    vectors.push_back(Vector(0.0, 1.0, 2.0));
    vectors.push_back(Vector(0.0, 0.0, 1.0));
    vectors.push_back(Vector(0.0, 1.0, 1.0));
    
    components = sphere.getPrincipalComponents(vectors);

    ASSERT_TRUE(components[0].x == 1.0);
    ASSERT_TRUE(components[0].y == 0.0);
    ASSERT_TRUE(components[0].z == 0.0);

    vectors.clear();

    // filling just x and z directions should give the least principal component in y direction
    vectors.push_back(Vector(1.0, 0.0, 0.0));
    vectors.push_back(Vector(2.0, 0.0, 2.0));
    vectors.push_back(Vector(2.0, 0.0, 1.0));
    vectors.push_back(Vector(0.0, 0.0, 1.0));
    
    components = sphere.getPrincipalComponents(vectors);

    ASSERT_TRUE(components[0].x == 0.0);
    ASSERT_TRUE(components[0].y == 1.0);
    ASSERT_TRUE(components[0].z == 0.0);

    vectors.clear();

    // a line in x-y direction should give the main principal component lying in x-y direction
    vectors.push_back(Vector(0.0, 0.0, 0.0));
    vectors.push_back(Vector(1.0, 1.0, 0.0));
    vectors.push_back(Vector(2.0, 2.0, 0.0));
    vectors.push_back(Vector(3.0, 3.0, 0.0));
    
    components = sphere.getPrincipalComponents(vectors);

    ASSERT_TRUE(fuzzyEquals(std::abs(components[2].x), 0.707106781));
    ASSERT_TRUE(fuzzyEquals(std::abs(components[2].y), 0.707106781));
    ASSERT_TRUE(components[2].z == 0.0);


    // test Capability of type cone
    Capability cone(CONE, 70.0, 55.0, 35.0);
    
    sphere = createReachabilitySphereFromCapability(cone, 400);

    components = sphere.getPrincipalComponents(sphere._reachableDirections);

    // test phi
    ASSERT_TRUE(atan2(components[0].y, components[0].x) * 180.0 / M_PI < 72.0);
    ASSERT_TRUE(atan2(components[0].y, components[0].x) * 180.0 / M_PI > 68.0);

    // test theta
    ASSERT_TRUE(acos(components[0].z) * 180.0 / M_PI < 56.0);
    ASSERT_TRUE(acos(components[0].z) * 180.0 / M_PI > 54.0);


    // test Capability of type cylinder_1
    Capability cylinder1(CYLINDER_1, 70.0, 55.0, 35.0);
    
    sphere = createReachabilitySphereFromCapability(cylinder1, 400);

    components = sphere.getPrincipalComponents(sphere._reachableDirections);

    // test phi
    ASSERT_TRUE(atan2(components[2].y, components[2].x) * 180.0 / M_PI < 72.0);
    ASSERT_TRUE(atan2(components[2].y, components[2].x) * 180.0 / M_PI > 68.0);

    // test theta
    ASSERT_TRUE(acos(components[2].z) * 180.0 / M_PI < 56.0);
    ASSERT_TRUE(acos(components[2].z) * 180.0 / M_PI > 54.0);


    // test Capability of type cylinder_2
    Capability cylinder2(CYLINDER_2, 70.0, 55.0, 35.0);
    
    sphere = createReachabilitySphereFromCapability(cylinder2, 400);

    components = sphere.getPrincipalComponents(sphere._reachableDirections);

    // test phi
    ASSERT_TRUE(atan2(components[0].y, components[0].x) * 180.0 / M_PI < 72.0);
    ASSERT_TRUE(atan2(components[0].y, components[0].x) * 180.0 / M_PI > 68.0);

    // test theta
    ASSERT_TRUE(acos(components[0].z) * 180.0 / M_PI < 56.0);
    ASSERT_TRUE(acos(components[0].z) * 180.0 / M_PI > 54.0);
}
コード例 #7
0
ファイル: rdrxlat.C プロジェクト: jbuonagurio/lrose-core
/* assume the source is an array which is n_az x n_bins in size and we need
   to process the lot
*/
void rdr_xlat::relocate(unsigned char *src, unsigned char *dest, int n_az, 
		    int src_n_bins, int dest_n_bins,
		    float rng_res, float src_start_rng, float dest_start_rng, 
		    double dest_ofs_x, double dest_ofs_y)
{
    int i, j;    /* array location for current destination point of interest */
    double t, r; /* polar co-ordinates for destination */
    double dest_x, dest_y; /* cartesian co-ordinates for destination */
    double dest_t, dest_i_f;          /* polar angle for source data */
    int dest_radl_ofs;
    int dest_i, dest_j;      /* array location for the nearest dest point */

    double src_x, src_y;   /* cartesion co-ordinates for source */
    double src_t, src_i_f;          /* polar angle for source data */
    int src_radl_ofs;
    int src_i, src_j;      /* array location for the nearest source point */

    double cost, sint;

#ifdef RINGS
    {
        extern void dump(char *);
        int i, j;
        for(j=0; j<n_bins; j++) { /*fill columns first */
            for(i=0; i< n_az; i++) {
                src[i*n_bins + j] = 'a' + (j/3)%26;
            } 
        }
        dump(src);
    }
#endif /* RINGS */
#ifdef WEDGES
{
        extern void dump(char *);
        int i, j;
        for(i=0; i< n_az; i++) /*fill rows first */{
            for(j=0; j<n_bins; j++) { 
                src[i*n_bins + j] = 'a' + (i/3)%26;
            } 
        }
        dump(src);
    }
#endif /* WEDGES */

    /* traverse destination volume and get nearest src point */
    /* rows are the polar angle, columns are the radial distance */

    for(i=0; i<n_az; i++) {  /*traverse along the row (range) first */
	t = DEG2RAD * i;
	cost = cos(t);
	sint = sin(t);
	dest_radl_ofs = i*dest_n_bins;
	for(j=0; j<dest_n_bins; j++) {
            r = dest_start_rng + (rng_res * (j + 0.5));	// add 0.5 to get centre of range bin
            dest_x = r * cost;
            dest_y = r * sint;

            src_x = dest_x - dest_ofs_x;
            src_y = dest_y - dest_ofs_y;
            src_t = atan2(src_y, src_x);
            src_i_f = src_t / DEG2RAD;
	    if (src_i_f < -0.5) src_i_f += 360.0;
	    src_i = (int)(0.5 + src_i_f);
            src_j = (int)(0.5 + ((hypot(src_x, src_y) - src_start_rng) /
		rng_res));

            if((src_j < src_n_bins)&&(src_j >= 0)) { 
                dest[dest_radl_ofs + j] = src[src_i*src_n_bins + src_j];
            }
        }
    }
    
    /* traverse src array and xlat to dest, replace dest cell if src > dest */
    for(i=0; i<n_az; i++) {  /*traverse along the row (range) first */
	t = DEG2RAD * i;
	cost = cos(t);
	sint = sin(t);
	src_radl_ofs = i*src_n_bins;
	for(j=0; j<src_n_bins; j++) {
            r = src_start_rng + (rng_res * (j + 0.5));	// add 0.5 to get centre of range bin
            src_x = r * cost;
            src_y = r * sint;

            dest_x = src_x + dest_ofs_x;
            dest_y = src_y + dest_ofs_y;
            dest_t = atan2(dest_y, dest_x);
            dest_i_f = dest_t / DEG2RAD;
	    if (dest_i_f < -0.5) dest_i_f += 360.0;
	    dest_i = (int)(0.5 + dest_i_f);
	    dest_j = (int)(0.5 + ((hypot(dest_x, dest_y) - dest_start_rng) /
		rng_res));

            if((dest_j < dest_n_bins)&&(dest_j >= 0)&&	// only overwrite smaller value
		(dest[dest_i*dest_n_bins + dest_j] < src[src_radl_ofs + j])) { 
                dest[dest_i*dest_n_bins + dest_j] = src[src_radl_ofs + j];
            }
}
    }
}
コード例 #8
0
ファイル: descriptor.cpp プロジェクト: bverhagen/SMSLD
/********************************************************************************
					直线描述子
********************************************************************************/
void  CDescriptor::ComputeLineDescriptor()
{
	//计算梯度图像
	ConputeGaussianGrad(m_pDxImage,m_pImageData,m_nWidth,m_nHeight,m_fSigma,11);
	ConputeGaussianGrad(m_pDyImage,m_pImageData,m_nWidth,m_nHeight,m_fSigma,12);
	ComputeMag(m_pMagImage,m_pDxImage,m_pDyImage,m_nTotolPixels);

	//计算每一条直线的描述子
	int nPtsPos = 0;
	double* pSubDesLineDes	= new double[nMaxRegionNum*8];
	for(int nNo = 0; nNo < m_nLineCount;nNo++)
	{
		//*************************************************
		//		1 如果直线上无效的点超过一半,则无效
		//*************************************************
		int nPtsPos_bak = nPtsPos;
		int nInValid	= 0;
		double dDxAvg	= 0;
		double dDyAvg	= 0;
		for(int nT1=0; nT1<m_szPtsCounts[nNo]; nT1++)
		{
			//获得当前点的位置信息
			int	nCenterR	= (int)m_pLinePts[2*nPtsPos];
			int	nCenterC	= (int)m_pLinePts[2*nPtsPos+1];
			int nCenterP	= nCenterR*m_nWidth + nCenterC;
			dDxAvg			= dDxAvg + m_pDxImage[nCenterP];
			dDyAvg			= dDyAvg + m_pDyImage[nCenterP];
			nPtsPos++;

			//判断是否越界
			if(	nCenterR < SCRadius+1 || nCenterR > m_nHeight-SCRadius-1 || 
				nCenterC < SCRadius+1 || nCenterC > m_nWidth-SCRadius-1)
			{
				nInValid ++;
			}
		}
		int nValid = m_szPtsCounts[nNo] - nInValid;
		if(nInValid > m_szPtsCounts[nNo]/2)
		{
			m_pByValidFlag[nNo] = 0;
			continue;
		}
		else
		{
			m_pByValidFlag[nNo] = 1;
		}
		
		//***************************************************
		//		2	计算直线的主方向
		//***************************************************
		double dMainArc = 0;
		if(matchType == 1)
			dMainArc = ComputeLineDir(&m_pLinePts[2*nPtsPos_bak],m_szPtsCounts[nNo],dDxAvg,dDyAvg);

		//***************************************************
		//		3	计算子区域描述子矩阵
		//***************************************************
		int nReCount = 0;
		nPtsPos = nPtsPos_bak;
		double* pSubDesMatrix  = new double[nMaxRegionNum*4*nValid];
		wzhSet(pSubDesMatrix,0,nMaxRegionNum*4*nValid);
		for(int nT1=0; nT1<m_szPtsCounts[nNo]; nT1++)
		{
			//获得当前点的位置信息
			int	nCenterR	= (int)m_pLinePts[2*nPtsPos];
			int	nCenterC	= (int)m_pLinePts[2*nPtsPos+1];
			int nCenterP	= nCenterR*m_nWidth + nCenterC;
			nPtsPos++;

			//判断是否越界
			if(	nCenterR < SCRadius+1 || nCenterR > m_nHeight-SCRadius-1 || 
				nCenterC < SCRadius+1 || nCenterC > m_nWidth-SCRadius-1)
			{
				continue;
			}

			//当前梯度信息
			if(matchType != 1)
			{
				dMainArc = atan2(-m_pDyImage[nCenterP],-m_pDxImage[nCenterP]);
				dMainArc = LimitArc(dMainArc);
			}

			//计算直线点描述子矩阵表
			double pSingleSubDes[nMaxRegionNum*4];
			ComputeSubRegionProjection(pSingleSubDes,dMainArc,nCenterR,nCenterC);
			memcpy(&pSubDesMatrix[nMaxRegionNum*4*nReCount],pSingleSubDes,sizeof(double)*nMaxRegionNum*4);
			nReCount++;
		}

		//***************************************************
		//		4	计算描述子并存入描述子
		//***************************************************
		ComputeDescriptorByMatrix(pSubDesLineDes,pSubDesMatrix,nMaxRegionNum*4,nValid);
		for(int g = 0; g < nMaxRegionNum*8; g++)
		{
			m_scDes[nNo*m_nDesDim+g] = (float)pSubDesLineDes[g];
		}

		//***************************************************
		//		5	释放内存
		//***************************************************
		wzhFreePointer(pSubDesMatrix);
	}
	wzhFreePointer(pSubDesLineDes);
}
コード例 #9
0
ファイル: polarst.c プロジェクト: TomWSmith/direwolf
long Convert_Polar_Stereographic_To_Geodetic (double Easting,
                                              double Northing,
                                              double *Latitude,
                                              double *Longitude)

{ /*  BEGIN Convert_Polar_Stereographic_To_Geodetic  */
/*
 *  The function Convert_Polar_Stereographic_To_Geodetic converts Polar
 *  Stereographic coordinates (easting and northing) to geodetic
 *  coordinates (latitude and longitude) according to the current ellipsoid
 *  and Polar Stereographic projection Parameters. If any errors occur, the
 *  code(s) are returned by the function, otherwise POLAR_NO_ERROR
 *  is returned.
 *
 *  Easting          : Easting (X), in meters                   (input)
 *  Northing         : Northing (Y), in meters                  (input)
 *  Latitude         : Latitude, in radians                     (output)
 *  Longitude        : Longitude, in radians                    (output)
 *
 */

  double dy = 0, dx = 0;
  double rho = 0;
  double t;
  double PHI, sin_PHI;
  double tempPHI = 0.0;
  double essin;
  double pow_es;
  double delta_radius;
  long Error_Code = POLAR_NO_ERROR;
  double min_easting = Polar_False_Easting - Polar_Delta_Easting;
  double max_easting = Polar_False_Easting + Polar_Delta_Easting;
  double min_northing = Polar_False_Northing - Polar_Delta_Northing;
  double max_northing = Polar_False_Northing + Polar_Delta_Northing;

  if (Easting > max_easting || Easting < min_easting)
  { /* Easting out of range */
    Error_Code |= POLAR_EASTING_ERROR;
  }
  if (Northing > max_northing || Northing < min_northing)
  { /* Northing out of range */
    Error_Code |= POLAR_NORTHING_ERROR;
  }

  if (!Error_Code)
  {
    dy = Northing - Polar_False_Northing;
    dx = Easting - Polar_False_Easting;

    /* Radius of point with origin of false easting, false northing */
    rho = sqrt(dx * dx + dy * dy);   
    
    delta_radius = sqrt(Polar_Delta_Easting * Polar_Delta_Easting + Polar_Delta_Northing * Polar_Delta_Northing);

    if(rho > delta_radius)
    { /* Point is outside of projection area */
      Error_Code |= POLAR_RADIUS_ERROR;
    }

    if (!Error_Code)
    { /* no errors */
      if ((dy == 0.0) && (dx == 0.0))
      {
        *Latitude = PI_OVER_2;
        *Longitude = Polar_Origin_Long;

      }
      else
      {
        if (Southern_Hemisphere != 0)
        {
          dy *= -1.0;
          dx *= -1.0;
        }

        if (fabs(fabs(Polar_Origin_Lat) - PI_OVER_2) > 1.0e-10)
          t = rho * tc / (Polar_a_mc);
        else
          t = rho * e4 / (two_Polar_a);
        PHI = PI_OVER_2 - 2.0 * atan(t);
        while (fabs(PHI - tempPHI) > 1.0e-10)
        {
          tempPHI = PHI;
          sin_PHI = sin(PHI);
          essin =  es * sin_PHI;
          pow_es = POLAR_POW(essin);
          PHI = PI_OVER_2 - 2.0 * atan(t * pow_es);
        }
        *Latitude = PHI;
        *Longitude = Polar_Origin_Long + atan2(dx, -dy);

        if (*Longitude > PI)
          *Longitude -= TWO_PI;
        else if (*Longitude < -PI)
          *Longitude += TWO_PI;


        if (*Latitude > PI_OVER_2)  /* force distorted values to 90, -90 degrees */
          *Latitude = PI_OVER_2;
        else if (*Latitude < -PI_OVER_2)
          *Latitude = -PI_OVER_2;

        if (*Longitude > PI)  /* force distorted values to 180, -180 degrees */
          *Longitude = PI;
        else if (*Longitude < -PI)
          *Longitude = -PI;

      }
      if (Southern_Hemisphere != 0)
      {
        *Latitude *= -1.0;
        *Longitude *= -1.0;
      }
    }
  }
  return (Error_Code);
} /* END OF Convert_Polar_Stereographic_To_Geodetic */
コード例 #10
0
 /**
  * @return \e azi0 the azimuth (degrees) of the geodesic line as it crosses
  * the equator in a northward direction.
  **********************************************************************/
 Math::real EquatorialAzimuth() const throw() {
   return Init() ?
     atan2(_salp0, _calp0) / Math::degree<real>() : Math::NaN<real>();
 }
コード例 #11
0
 /**
  * @return \e a1 the arc length (degrees) between the northward equatorial
  * crossing and point 1.
  **********************************************************************/
 Math::real EquatorialArc() const throw() {
   return Init() ?
     atan2(_ssig1, _csig1) / Math::degree<real>() : Math::NaN<real>();
 }
コード例 #12
0
//Character should fall back to a safer position
Action* FallBack::run() {

	//flood the map of all the places we can move to within a long range
	vector<Tile*> fallbackPositions;
	PathFinding::floodMap(fallbackPositions, Tree->Character->CurrentTile, Tree->Character->Speed * _tilesDeep);

	for (int i = 0; i < (int)fallbackPositions.size(); i++) {
		
		//remove any possible fall back positions that are occupied by other characters
		if (fallbackPositions[i]->IsOccupied == true) {

			fallbackPositions.erase(fallbackPositions.begin() + i);
			i--;
		}
	}

	//get the assumed enemy infulence in the level
	vector<InfulenceData> enemyInfulence = Tree->CharTeam->getAssumedEnemyInfulencedTiles();

	//weight the tiles against the enemies infulence across them
	for (int i = 0; i < (int)fallbackPositions.size(); i++) {
		for (int j = 0; j < (int)enemyInfulence.size(); j++) {

			InfulenceData loopedInfulence = enemyInfulence[j];

			//check to see if the infulence is that of the looped fall back position
			if (fallbackPositions[i] == loopedInfulence.TheTile) {

				//increase the weighting
				fallbackPositions[i]->Weighting += loopedInfulence.Infulence;
				break;
			}
		}
	}

	vector<AICharacter*> visibleEnemies = Tree->CharTeam->getVisibleEnemies();

	//weight the possible positions by how far away they are from enemies
	for (int i = 0; i < (int)fallbackPositions.size(); i++) {

		//find the angle from the characters current position to the looped enemy
		Vector3 dir = fallbackPositions[i]->Position - Tree->Character->Position;

		//resolve vector into an angle, where 0 is facing up n degrees
		float angleToTile = ((atan2(dir.y, dir.x) * 180) / 3.1415926) + 90;

		for (int j = 0; j < (int)visibleEnemies.size(); j++) {

			float distance = visibleEnemies[j]->Position.distance(fallbackPositions[i]->Position);

			if (distance <= FALLBACK_ENEMY_TOO_CLOSE_CONST)
				fallbackPositions[i]->Weighting += distance * 10;

			else
				fallbackPositions[i]->Weighting -= visibleEnemies[j]->Position.distance(fallbackPositions[i]->Position) / 10;

			//find the angle from the characters current position to the looped enemy
			dir = visibleEnemies[j]->Position - Tree->Character->Position;

			//resolve vector into an angle, where 0 is facing up n degrees
			float angleToEnemy = ((atan2(dir.y, dir.x) * 180) / 3.1415926) + 90;

			//if the angle to the enemy is similair to the angle to the possible fallback position, then we are moving towards an enemy
			//and thus should be peanalised heavily
			if (Rotations::rotationsSimilair(angleToTile, angleToEnemy, 100) == true) {

				fallbackPositions[i]->Weighting += FALLBACK_FLANK_PENALTY_CONST;
			}
		}

		//now increase weighting by how far we guess we have to move
		int xCost = abs(Tree->Character->CurrentTile->X - fallbackPositions[i]->X);
		int yCost = abs(Tree->Character->CurrentTile->Y - fallbackPositions[i]->Y);

		//guess the distance to the end tile
		int Hcost = (xCost + yCost) * 10;
		fallbackPositions[i]->Weighting += Hcost * 10;
	}

	//loop through all of the possible points and find that with the lowest weighting
	Tile * bestTile = fallbackPositions[0];
	for (int i = 1; i < (int)fallbackPositions.size(); i++) {

		if (bestTile->Weighting > fallbackPositions[i]->Weighting) {
			bestTile = fallbackPositions[i];
			i = 1;
		}
	}

	//reset all of the tile's weighting for any later pathfinding
	for (int i = 0; i < (int)fallbackPositions.size(); i++) {
		fallbackPositions[i]->Weighting = 0;
	}

	Tree->log("Wants to fall back");
	return new MoveAction(Tree->Character, bestTile, 1);
}
コード例 #13
0
ファイル: Route.cpp プロジェクト: vipserv/OpenCPN
void Route::RenderSegment( ocpnDC& dc, int xa, int ya, int xb, int yb, ViewPort &VP,
                           bool bdraw_arrow, int hilite_width )
{
    //    Get the dc boundary
    int sx, sy;
    dc.GetSize( &sx, &sy );

    //    Try to exit early if the segment is nowhere near the screen
    wxRect r( 0, 0, sx, sy );
    wxRect s( xa, ya, 1, 1 );
    wxRect t( xb, yb, 1, 1 );
    s.Union( t );
    if( !r.Intersects( s ) ) return;

    //    Clip the line segment to the dc boundary
    int x0 = xa;
    int y0 = ya;
    int x1 = xb;
    int y1 = yb;

    //    If hilite is desired, use a Native Graphics context to render alpha colours
    //    That is, if wxGraphicsContext is available.....

    if( hilite_width ) {
        if( Visible == cohen_sutherland_line_clip_i( &x0, &y0, &x1, &y1, 0, sx, 0, sy ) ) {
            wxPen psave = dc.GetPen();

            wxColour y = GetGlobalColor( _T ( "YELO1" ) );
            wxColour hilt( y.Red(), y.Green(), y.Blue(), 128 );

            wxPen HiPen( hilt, hilite_width, wxPENSTYLE_SOLID );

            dc.SetPen( HiPen );
            dc.StrokeLine( x0, y0, x1, y1 );

            dc.SetPen( psave );
            dc.StrokeLine( x0, y0, x1, y1 );
        }
    } else {
        if( Visible == cohen_sutherland_line_clip_i( &x0, &y0, &x1, &y1, 0, sx, 0, sy ) )
            dc.StrokeLine( x0, y0, x1, y1 );
    }

    if( bdraw_arrow ) {
        //    Draw a direction arrow

        double theta = atan2( (double) ( yb - ya ), (double) ( xb - xa ) );
        theta -= PI / 2.;

        wxPoint icon[10];
        double icon_scale_factor = 100 * VP.view_scale_ppm;
        icon_scale_factor = fmin ( icon_scale_factor, 1.5 );              // Sets the max size
        icon_scale_factor = fmax ( icon_scale_factor, .10 );

        //    Get the absolute line length
        //    and constrain the arrow to be no more than xx% of the line length
        double nom_arrow_size = 20.;
        double max_arrow_to_leg = .20;
        double lpp = sqrt( pow( (double) ( xa - xb ), 2 ) + pow( (double) ( ya - yb ), 2 ) );

        double icon_size = icon_scale_factor * nom_arrow_size;
        if( icon_size > ( lpp * max_arrow_to_leg ) ) icon_scale_factor = ( lpp * max_arrow_to_leg )
                    / nom_arrow_size;

        for( int i = 0; i < 7; i++ ) {
            int j = i * 2;
            double pxa = (double) ( s_arrow_icon[j] );
            double pya = (double) ( s_arrow_icon[j + 1] );

            pya *= icon_scale_factor;
            pxa *= icon_scale_factor;

            double px = ( pxa * sin( theta ) ) + ( pya * cos( theta ) );
            double py = ( pya * sin( theta ) ) - ( pxa * cos( theta ) );

            icon[i].x = (int) ( px ) + xb;
            icon[i].y = (int) ( py ) + yb;
        }
        wxPen savePen = dc.GetPen();
        dc.SetPen( *wxTRANSPARENT_PEN );
        dc.StrokePolygon( 6, &icon[0], 0, 0 );
        dc.SetPen( savePen );
    }
}
コード例 #14
0
ファイル: rotation.c プロジェクト: skrieder/oops
ErrorCode update_torsional_angle(IndexValue bond, struct ResidueData *residues, IndexValue res, FloatValue *coords)
{
    if (residues[res].bonds[bond].type == NOT_SET)
    {
        return ACCESSING_UNSET_BOND_ERROR;
    }

    IndexValue atom0, atom1, atom2, atom3;
    FloatValue x0, y0, z0;
    FloatValue x1, y1, z1;
    FloatValue x2, y2, z2;
    FloatValue x3, y3, z3;

    FloatValue x01, y01, z01;
    FloatValue x12, y12, z12;
    FloatValue x32, y32, z32;

    FloatValue px, py, pz;
    FloatValue qx, qy, qz;
    FloatValue rx, ry, rz;

    FloatValue u, v, u1, v1;
    FloatValue a;

    atom0 = residues[res].bonds[bond].atom0;
    x0 = coords[xcoord_index(atom0)];
    y0 = coords[ycoord_index(atom0)];
    z0 = coords[zcoord_index(atom0)];

    atom1 = residues[res].bonds[bond].atom1;
    x1 = coords[xcoord_index(atom1)];
    y1 = coords[ycoord_index(atom1)];
    z1 = coords[zcoord_index(atom1)];
    
    atom2 = residues[res].bonds[bond].atom2;
    x2 = coords[xcoord_index(atom2)];
    y2 = coords[ycoord_index(atom2)];
    z2 = coords[zcoord_index(atom2)];

    atom3 = residues[res].bonds[bond].atom3;
    x3 = coords[xcoord_index(atom3)];
    y3 = coords[ycoord_index(atom3)];
    z3 = coords[zcoord_index(atom3)];

    vector_subtract(&x01, &y01, &z01, x0, y0, z0, x1, y1, z1);     // v01 = v0 - v1
    vector_subtract(&x32, &y32, &z32, x3, y3, z3, x2, y2, z2);     // v32 = v3 - v2
    vector_subtract(&x12, &y12, &z12, x1, y1, z1, x2, y2, z2);     // v12 = v1 - v2

    vector_crossprod(&px, &py, &pz, x12, y12, z12, x01, y01, z01); // p = v12 x v01
    vector_crossprod(&qx, &qy, &qz, x12, y12, z12, x32, y32, z32); // q = v12 x v32
    vector_crossprod(&rx, &ry, &rz, x12, y12, z12, qx, qy, qz);    // r = v12 x q

    vector_dotprod(&u, qx, qy, qz, qx, qy, qz);                    // u = q * q
    vector_dotprod(&v, rx, ry, rz, rx, ry, rz);                    // v = r * r

    if (u <= 0.0 || v <= 0.0)
    {
        a = 360.0;
    }
    else 
    {
        vector_dotprod(&u1, px, py, pz, qx, qy, qz);               // u1 = p * q
        vector_dotprod(&v1, px, py, pz, rx, ry, rz);               // v1 = p * r

        u = u1 / sqrt(u);
        v = v1 / sqrt(v);

        if (fabs(u) > MIN_FLOAT_DIFF || fabs(v) > MIN_FLOAT_DIFF) a = atan2(v, u) * RADIANS_TO_DEGREES;
        else a = 360.0;
    }

    residues[res].bonds[bond].angle = a;
    residues[res].bonds[bond].newangle = a;

    return NO_ERROR;
}
コード例 #15
0
ファイル: grid.c プロジェクト: aesop972/ngspice-gss
static double
cliparc(double cx, double cy, double rad, double start, double end, int iclipx, int iclipy, int icliprad, int flag)
{
    double clipx, clipy, cliprad;
    double sclip = 0.0, eclip = 0.0;
    double x, y, tx, ty, dist;
    double alpha, theta, phi, a1, a2, d, l;
    bool in;

    clipx = (double) iclipx;
    clipy = (double) iclipy;
    cliprad = (double) icliprad;
    x = cx - clipx;
    y = cy - clipy;
    dist = sqrt((double) (x * x + y * y));

    if (!rad || !cliprad)
        return(-1);
    if (dist + rad < cliprad) {
        /* The arc is entirely in the boundary. */
        Arc((int)cx, (int)cy, (int)rad, start, end);
        return(flag?start:end);
    } else if ((dist - rad >= cliprad) || (rad - dist >= cliprad)) {
        /* The arc is outside of the boundary. */
        return(-1);
    }
    /* Now let's figure out the angles at which the arc crosses the
     * circle. We know dist != 0.
     */
    if (x)
        phi = atan2((double) y, (double) x);
    else if (y > 0)
        phi = M_PI * 1.5;
    else
        phi = M_PI / 2;
    if (cx > clipx)
        theta = M_PI + phi;
    else
        theta = phi;

    alpha = (double) (dist * dist + rad * rad - cliprad * cliprad) /
            (2 * dist * rad);

    /* Sanity check */
    if (alpha > 1.0)
	alpha = 0.0;
    else if (alpha < -1.0)
	alpha = M_PI;
    else
        alpha = acos(alpha);

    a1 = theta + alpha;
    a2 = theta - alpha;
    while (a1 < 0)
        a1 += M_PI * 2;
    while (a2 < 0)
        a2 += M_PI * 2;
    while (a1 >= M_PI * 2)
        a1 -= M_PI * 2;
    while (a2 >= M_PI * 2)
        a2 -= M_PI * 2;

    tx = cos(start) * rad + x;
    ty = sin(start) * rad + y;
    d = sqrt((double) tx * tx + ty * ty);
    in = (d > cliprad) ? FALSE : TRUE;

    /* Now begin with start.  If the point is in, draw to either end, a1,
     * or a2, whichever comes first.
     */
    d = M_PI * 3;
    if ((end < d) && (end > start))
        d = end;
    if ((a1 < d) && (a1 > start))
        d = a1;
    if ((a2 < d) && (a2 > start))
        d = a2;
    if (d == M_PI * 3) {
        d = end;
        if (a1 < d)
            d = a1;
        if (a2 < d)
            d = a2;
    }

    if (in) {
	if (start > d) {
	    double tmp;
	    tmp = start;
	    start = d;
	    d = tmp;
	}
        Arc((int)cx, (int)cy, (int)rad, start, d);
	sclip = start;
	eclip = d;
    }
    if (d == end)
        return(flag?sclip:eclip);
    if (a1 != a2)
        in = in ? FALSE : TRUE;

    /* Now go from here to the next point. */
    l = d;
    d = M_PI * 3;
    if ((end < d) && (end > l))
        d = end;
    if ((a1 < d) && (a1 > l))
        d = a1;
    if ((a2 < d) && (a2 > l))
        d = a2;
    if (d == M_PI * 3) {
        d = end;
        if (a1 < d)
            d = a1;
        if (a2 < d)
            d = a2;
    }

    if (in) {
        Arc((int)cx, (int)cy, (int)rad, l, d);
	sclip = l;
	eclip = d;
    }
    if (d == end)
        return(flag?sclip:eclip);
    in = in ? FALSE : TRUE;
    
    /* And from here to the end. */
    if (in) {
        Arc((int)cx, (int)cy, (int)rad, d, end);
	/* special case */
	if (flag != 2) {
	  sclip = d;
	  eclip = end;
	}
    }
    return(flag%2?sclip:eclip);
}
コード例 #16
0
ファイル: complex09.cpp プロジェクト: sdukshis/cpp
 double argument() const { return atan2(im, re); }
コード例 #17
0
ファイル: grid.c プロジェクト: aesop972/ngspice-gss
static void
drawpolargrid(GRAPH *graph)
{
    double tenpowmag, theta;
    int hmt, lmt, i, step, mag;
    int relcx, relcy, relrad, dist, degs;
    int x1, y1, x2, y2;
    double minrad, maxrad, pixperunit;
    char buf[64];

    hmt = graph->grid.xaxis.circular.hmt;
    lmt = graph->grid.xaxis.circular.lmt;
    mag = graph->grid.xaxis.circular.mag;
    tenpowmag = pow(10.0, (double) mag);
    maxrad = hmt * tenpowmag;
    minrad = lmt * tenpowmag; 

    if ((minrad == 0) && ((hmt - lmt) > 5)) {
	if (!((hmt - lmt) % 2))
	    step = 2;
	else if (!((hmt - lmt) % 3))
	    step = 3;
	else
	    step = 1;
	} else
	    step = 1;
    pixperunit = graph->grid.xaxis.circular.radius * 2 /
	(graph->datawindow.xmax - graph->datawindow.xmin);

    relcx = - (graph->datawindow.xmin + graph->datawindow.xmax) / 2
            * pixperunit;
    relcy = - (graph->datawindow.ymin + graph->datawindow.ymax) / 2
            * pixperunit;

    /* The distance from the center of the plotting area to the center of
     * the logical area.
     */
    dist = sqrt((double) (relcx * relcx + relcy * relcy));

    SetLinestyle(0);
    Arc(graph->grid.xaxis.circular.center,
            graph->grid.yaxis.circular.center,
            graph->grid.xaxis.circular.radius,
            (double) 0.0, (double) 0.0);
    SetLinestyle(1);

    /* Now draw the circles. */
    for (i = lmt;
	(relrad = i * tenpowmag * pixperunit)
	<= dist + graph->grid.xaxis.circular.radius;
	i += step)
    {
        cliparc((double) graph->grid.xaxis.circular.center + relcx,
                (double) graph->grid.yaxis.circular.center + relcy,
                (double) relrad, 0.0, 0.0,
                graph->grid.xaxis.circular.center,
                graph->grid.yaxis.circular.center,
                graph->grid.xaxis.circular.radius, 0);
        /* Toss on the label */
        if (relcx || relcy)
            theta = atan2((double) relcy, (double) relcx);
        else
            theta = M_PI;
        if (i && (relrad > dist - graph->grid.xaxis.circular.radius))
            addradlabel(graph, i, theta,
                (int) (graph->grid.xaxis.circular.center -
                    (relrad - dist) * cos(theta)),
                (int) (graph->grid.yaxis.circular.center
                    - (relrad - dist) * sin(theta)));
    }

    /* Now draw the spokes.  We have two possible cases -- first, the
     * origin may be inside the area -- in this case draw 12 spokes.
     * Otherwise, draw several spokes at convenient places.
     */
    if ((graph->datawindow.xmin <= 0.0)
            && (graph->datawindow.xmax >= 0.0)
            && (graph->datawindow.ymin <= 0.0)
            && (graph->datawindow.ymax >= 0.0)) {
        for (i = 0; i < 12; i++) {
            x1 = graph->grid.xaxis.circular.center + relcx;
            y1 = graph->grid.yaxis.circular.center + relcy;
            x2 = x1 + graph->grid.xaxis.circular.radius * 2
                    * cos(i * M_PI / 6);
            y2 = y1 + graph->grid.xaxis.circular.radius * 2
                    * sin(i * M_PI / 6);
            if (!clip_to_circle(&x1, &y1, &x2, &y2,
                    graph->grid.xaxis.circular.center,
                    graph->grid.yaxis.circular.center,
                    graph->grid.xaxis.circular.radius))
            {
	        DrawLine(x1, y1, x2, y2); 
                /* Add a label here */
		/*XXXX*/
                adddeglabel(graph, i * 30, x2, y2, x1, y1,
		    graph->grid.xaxis.circular.center,
		    graph->grid.yaxis.circular.center);
            }
        }
    } else {
        /* Figure out the angle that we have to fill up */
        theta = 2 * asin((double) graph->grid.xaxis.circular.radius
                / dist);
        theta = theta * 180 / M_PI;   /* Convert to degrees. */
        
        /* See if we should put lines at 30, 15, 5, or 1 degree 
         * increments.
         */
        if (theta / 30 > 3)
            degs = 30;
        else if (theta / 15 > 3)
            degs = 15;
        else if (theta / 5 > 3)
            degs = 5;
        else
            degs = 1;

        /* We'll be cheap */
        for (i = 0; i < 360; i+= degs) {
            x1 = graph->grid.xaxis.circular.center + relcx;
            y1 = graph->grid.yaxis.circular.center + relcy;
            x2 = x1 + dist * 2 * cos(i * M_PI / 180);
            y2 = y1 + dist * 2 * sin(i * M_PI / 180);
            if (!clip_to_circle(&x1, &y1, &x2, &y2,
                    graph->grid.xaxis.circular.center,
                    graph->grid.yaxis.circular.center,
                    graph->grid.xaxis.circular.radius)) {
                DrawLine(x1, y1, x2, y2);
                /* Put on the label */
                adddeglabel(graph, i, x2, y2, x1, y1,
		    graph->grid.xaxis.circular.center,
		    graph->grid.yaxis.circular.center);
            }
        }
    }

    (void) sprintf(buf, "e%d", mag);
    Text(buf, graph->grid.xaxis.circular.center
              + graph->grid.xaxis.circular.radius,
            graph->grid.yaxis.circular.center
              - graph->grid.xaxis.circular.radius);
    Update();
    return;
}
コード例 #18
0
/*
===============
UI_DrawPlayer
===============
*/
void UI_DrawPlayer( float x, float y, float w, float h, playerInfo_t *pi, int time ) {
	refdef_t		refdef;
	refEntity_t		legs;
	refEntity_t		torso;
	refEntity_t		head;
	refEntity_t		gun;
	refEntity_t		barrel;
	refEntity_t		flash;
	vec3_t			origin;
	int				renderfx;
	vec3_t			mins = {-16, -16, -24};
	vec3_t			maxs = {16, 16, 32};
	float			len;
	float			xx;

	if ( !pi->legsModel || !pi->torsoModel || !pi->headModel || !pi->animations[0].numFrames ) {
		return;
	}

	// this allows the ui to cache the player model on the main menu
	if (w == 0 || h == 0) {
		return;
	}

	dp_realtime = time;

	if ( pi->pendingWeapon != -1 && dp_realtime > pi->weaponTimer ) {
		pi->weapon = pi->pendingWeapon;
		pi->lastWeapon = pi->pendingWeapon;
		pi->pendingWeapon = -1;
		pi->weaponTimer = 0;
		if( pi->currentWeapon != pi->weapon ) {
			trap_S_StartLocalSound( weaponChangeSound, CHAN_LOCAL );
		}
	}

	UI_AdjustFrom640( &x, &y, &w, &h );

	y -= jumpHeight;

	memset( &refdef, 0, sizeof( refdef ) );
	memset( &legs, 0, sizeof(legs) );
	memset( &torso, 0, sizeof(torso) );
	memset( &head, 0, sizeof(head) );

	refdef.rdflags = RDF_NOWORLDMODEL;

	AxisClear( refdef.viewaxis );

	refdef.x = x;
	refdef.y = y;
	refdef.width = w;
	refdef.height = h;

	refdef.fov_x = (int)((float)refdef.width / uiInfo.uiDC.xscale / 640.0f * 90.0f);
	xx = refdef.width / uiInfo.uiDC.xscale / tan( refdef.fov_x / 360 * M_PI );
	refdef.fov_y = atan2( refdef.height / uiInfo.uiDC.yscale, xx );
	refdef.fov_y *= ( 360 / (float)M_PI );

	// calculate distance so the player nearly fills the box
	len = 0.7 * ( maxs[2] - mins[2] );
	origin[0] = len / tan( DEG2RAD(refdef.fov_x) * 0.5 );
	origin[1] = 0.5 * ( mins[1] + maxs[1] );
	origin[2] = -0.5 * ( mins[2] + maxs[2] );

	refdef.time = dp_realtime;

	trap_R_ClearScene();

	// get the rotation information
	UI_PlayerAngles( pi, legs.axis, torso.axis, head.axis );
	
	// get the animation state (after rotation, to allow feet shuffle)
	UI_PlayerAnimation( pi, &legs.oldframe, &legs.frame, &legs.backlerp,
		 &torso.oldframe, &torso.frame, &torso.backlerp );

	renderfx = RF_LIGHTING_ORIGIN | RF_NOSHADOW;

	//
	// add the legs
	//
	legs.hModel = pi->legsModel;
	legs.customSkin = pi->legsSkin;

	VectorCopy( origin, legs.origin );

	VectorCopy( origin, legs.lightingOrigin );
	legs.renderfx = renderfx;
	VectorCopy (legs.origin, legs.oldorigin);

	trap_R_AddRefEntityToScene( &legs );

	if (!legs.hModel) {
		return;
	}

	//
	// add the torso
	//
	torso.hModel = pi->torsoModel;
	if (!torso.hModel) {
		return;
	}

	torso.customSkin = pi->torsoSkin;

	VectorCopy( origin, torso.lightingOrigin );

	UI_PositionRotatedEntityOnTag( &torso, &legs, pi->legsModel, "tag_torso");

	torso.renderfx = renderfx;

	trap_R_AddRefEntityToScene( &torso );

	//
	// add the head
	//
	head.hModel = pi->headModel;
	if (!head.hModel) {
		return;
	}
	head.customSkin = pi->headSkin;

	VectorCopy( origin, head.lightingOrigin );

	UI_PositionRotatedEntityOnTag( &head, &torso, pi->torsoModel, "tag_head");

	head.renderfx = renderfx;

	trap_R_AddRefEntityToScene( &head );

	//
	// add the gun
	//
	if ( pi->currentWeapon != WP_NONE ) {
		memset( &gun, 0, sizeof(gun) );
		gun.hModel = pi->weaponModel;
		VectorCopy( origin, gun.lightingOrigin );
		UI_PositionEntityOnTag( &gun, &torso, pi->torsoModel, "tag_weapon");
		gun.renderfx = renderfx;
		trap_R_AddRefEntityToScene( &gun );
	}

	//
	// add the spinning barrel
	//
	if ( pi->realWeapon == WP_MACHINEGUN || pi->realWeapon == WP_GAUNTLET || pi->realWeapon == WP_BFG ) {
		vec3_t	angles;

		memset( &barrel, 0, sizeof(barrel) );
		VectorCopy( origin, barrel.lightingOrigin );
		barrel.renderfx = renderfx;

		barrel.hModel = pi->barrelModel;
		angles[YAW] = 0;
		angles[PITCH] = 0;
		angles[ROLL] = UI_MachinegunSpinAngle( pi );
		if( pi->realWeapon == WP_GAUNTLET || pi->realWeapon == WP_BFG ) {
			angles[PITCH] = angles[ROLL];
			angles[ROLL] = 0;
		}
		AnglesToAxis( angles, barrel.axis );

		UI_PositionRotatedEntityOnTag( &barrel, &gun, pi->weaponModel, "tag_barrel");

		trap_R_AddRefEntityToScene( &barrel );
	}

	//
	// add muzzle flash
	//
	if ( dp_realtime <= pi->muzzleFlashTime ) {
		if ( pi->flashModel ) {
			memset( &flash, 0, sizeof(flash) );
			flash.hModel = pi->flashModel;
			VectorCopy( origin, flash.lightingOrigin );
			UI_PositionEntityOnTag( &flash, &gun, pi->weaponModel, "tag_flash");
			flash.renderfx = renderfx;
			trap_R_AddRefEntityToScene( &flash );
		}

		// make a dlight for the flash
		if ( pi->flashDlightColor[0] || pi->flashDlightColor[1] || pi->flashDlightColor[2] ) {
			trap_R_AddLightToScene( flash.origin, 200 + (rand()&31), pi->flashDlightColor[0],
				pi->flashDlightColor[1], pi->flashDlightColor[2] );
		}
	}

	//
	// add the chat icon
	//
	if ( pi->chat ) {
		UI_PlayerFloatSprite( pi, origin, trap_R_RegisterShaderNoMip( "sprites/balloon3" ) );
	}

	//
	// add an accent light
	//
	origin[0] -= 100;	// + = behind, - = in front
	origin[1] += 100;	// + = left, - = right
	origin[2] += 100;	// + = above, - = below
	trap_R_AddLightToScene( origin, 500, 1.0, 1.0, 1.0 );

	origin[0] -= 100;
	origin[1] -= 100;
	origin[2] -= 100;
	trap_R_AddLightToScene( origin, 500, 1.0, 0.0, 0.0 );

	trap_R_RenderScene( &refdef );
}
コード例 #19
0
ファイル: Transport.cpp プロジェクト: Raszagal/TrinityCore
void Transport::Update(uint32 diff)
{
    uint32 const positionUpdateDelay = 200;

    if (AI())
        AI()->UpdateAI(diff);
    else if (!AIM_Initialize())
        TC_LOG_ERROR("entities.transport", "Could not initialize GameObjectAI for Transport");

    if (GetKeyFrames().size() <= 1)
        return;

    m_goValue.Transport.PathProgress += diff;

    uint32 timer = m_goValue.Transport.PathProgress % GetPeriod();

    // Set current waypoint
    // Desired outcome: _currentFrame->DepartureTime < timer < _nextFrame->ArriveTime
    // ... arrive | ... delay ... | departure
    //      event /         event /
    for (;;)
    {
        if (timer >= _currentFrame->ArriveTime)
        {
            if (!_triggeredArrivalEvent)
            {
                DoEventIfAny(*_currentFrame, false);
                _triggeredArrivalEvent = true;
            }

            if (timer < _currentFrame->DepartureTime)
            {
                SetMoving(false);
                if (_pendingStop)
                    SetGoState(GO_STATE_READY);
                break;  // its a stop frame and we are waiting
            }
        }

        if (_pendingStop && timer >= _currentFrame->DepartureTime && GetGoState() == GO_STATE_READY)
        {
            m_goValue.Transport.PathProgress = (m_goValue.Transport.PathProgress / GetPeriod());
            m_goValue.Transport.PathProgress *= GetPeriod();
            m_goValue.Transport.PathProgress += _currentFrame->ArriveTime;
            break;
        }

        if (timer >= _currentFrame->DepartureTime && !_triggeredDepartureEvent)
        {
            DoEventIfAny(*_currentFrame, true); // departure event
            _triggeredDepartureEvent = true;
        }

        if (timer >= _currentFrame->DepartureTime && timer < _currentFrame->NextArriveTime)
            break;  // found current waypoint

        MoveToNextWaypoint();

        // not waiting anymore
        SetMoving(true);

        // Enable movement
        if (GetGOInfo()->moTransport.canBeStopped)
            SetGoState(GO_STATE_ACTIVE);

        sScriptMgr->OnRelocate(this, _currentFrame->Node->index, _currentFrame->Node->mapid, _currentFrame->Node->x, _currentFrame->Node->y, _currentFrame->Node->z);

        TC_LOG_DEBUG("entities.transport", "Transport %u (%s) moved to node %u %u %f %f %f", GetEntry(), GetName().c_str(), _currentFrame->Node->index, _currentFrame->Node->mapid, _currentFrame->Node->x, _currentFrame->Node->y, _currentFrame->Node->z);

        // Departure event
        if (_currentFrame->IsTeleportFrame())
            if (TeleportTransport(_nextFrame->Node->mapid, _nextFrame->Node->x, _nextFrame->Node->y, _nextFrame->Node->z))
                return; // Update more in new map thread
    }

    // Set position
    _positionChangeTimer.Update(diff);
    if (_positionChangeTimer.Passed())
    {
        _positionChangeTimer.Reset(positionUpdateDelay);
        if (IsMoving())
        {
            float t = CalculateSegmentPos(float(timer) * 0.001f);
            G3D::Vector3 pos, dir;
            _currentFrame->Spline->evaluate_percent(_currentFrame->Index, t, pos);
            _currentFrame->Spline->evaluate_derivative(_currentFrame->Index, t, dir);
            UpdatePosition(pos.x, pos.y, pos.z, atan2(dir.x, dir.y));
        }
    }

    sScriptMgr->OnTransportUpdate(this, diff);
}
コード例 #20
0
ファイル: IPLCanny.cpp プロジェクト: rodoviario/ImagePlay
bool IPLCanny::processInputData(IPLImage* image , int, bool useOpenCV)
{
    // delete previous result
    delete _result;
    _result = NULL;
    delete _binaryImage;
    _binaryImage = NULL;

    int width = image->width();
    int height = image->height();

    _result = new IPLImage( image->type(), width, height );
    _binaryImage = new IPLImage( IPLData::IMAGE_BW, width, height );

    // get properties
    int window              = getProcessPropertyInt("window");
    double sigma            = getProcessPropertyDouble("sigma");
    double lowThreshold     = getProcessPropertyDouble("lowThreshold");
    double highThreshold    = getProcessPropertyDouble("highThreshold");

    std::stringstream s;
    s << "Window: ";
    s << window;
    addInformation(s.str());

    //! @todo currently only the opencv implementation works
    if(useOpenCV || true)
    {
        notifyProgressEventHandler(-1);
        cv::Mat input;
        cv::Mat output;
        cvtColor(image->toCvMat(), input, CV_BGR2GRAY);
        cv::Canny(input, output, lowThreshold*255, highThreshold*255, window);

        delete _result;
        _result = new IPLImage(output);

        return true;
    }

    return false;

    // Create a Gaussian 1D filter
    int N = ceil( sigma * sqrt( 2.0*log( 1.0/0.015 ) ) + 1.0 );
    double ssq = sigma*sigma;
    double* gau = new double [window];
    double* dgau = new double [window];
    for( int k = -N; k <= N; ++k )
    {
        gau[k+N] = gauss ( (double)k, ssq );
        dgau[k+N] = dGauss ( (double)k, 0, ssq );
    }

    // Create a directional derivative of 2D Gaussian (along X-axis)
    // Since the result is symmetric along X, we can get the derivative along
    // Y-axis simply by transposing the result for X direction.
//		DoubleImage* dgau = new DoubleImage( window, window );
//		for( int y = -N; y <= N; ++y )
//			for( int x = -N; x <= N; ++x )
//				dgau->f(x+N, y+N) = dGauss( x, y, ssq );

    int progress = 0;
    int maxProgress = width * image->getNumberOfPlanes();
    int nrOfPlanes = image->getNumberOfPlanes();

    //#pragma omp parallel for
    for( int planeNr=0; planeNr < nrOfPlanes; planeNr++ )
    {
        IPLImagePlane* plane = image->plane( planeNr );
        IPLImagePlane* newplane = _result->plane( planeNr );

// ******** Gaussian filtering of input image
        IPLImagePlane* gI = new IPLImagePlane( width, height );

        // horizontal run (normalizing original image)
        IPLImagePlane* tmpI = new IPLImagePlane( width, height );
        for(int x=0; x<width; x++)
        {            // progress
            notifyProgressEventHandler(100*progress++/maxProgress);

            for(int y=0; y<height; y++)
            {
                double sum = 0;
                int i = 0;
                for( int kx=-N; kx<=N; kx++ )
                {
                        double img = (double) plane->bp(x+kx, y);
                        sum += (img * gau[i++]);
                }
                tmpI->p(x,y) = (double) (sum);
            }
        }
        // vertiacl run
        for(int x=0; x<width; x++)
        {
            for(int y=0; y<height; y++)
            {
                double sum = 0;
                int i = 0;
                for( int ky=-N; ky<=N; ky++ )
                {
                        double img = tmpI->bp(x, y+ky);
                        sum += (img * gau[i++]);
                }
                gI->p(x,y) = sum;
            }
        }
        //delete tmpI;

// ******** Apply directional derivatives ...

        // ... in x-direction
        IPLImagePlane* dx = new IPLImagePlane( width, height );
        for(int x=0; x<width; x++)
        {
            for(int y=0; y<height; y++)
            {
                dx->p(x,y) = 0.0;
                for( int k=1; k<N; k++ )
                {
                    dx->p(x,y) += ( gI->bp(x-k,y) - gI->bp(x+k,y) ) * dgau[k];
                }
            }
        }
//			double maxVal = 0.0;
//			for(int x=0; x<width; x++)
//				for(int y=0; y<height; y++)
//					if( dx->f(x,y) > maxVal ) maxVal = dx->f(x,y);

        // ... in y-direction
        IPLImagePlane* dy = new IPLImagePlane( width, height );
        for(int x=0; x<width; x++)
        {
            for(int y=0; y<height; y++)
            {
                dy->p(x,y) = 0.0;
                for( int k=1; k<N; k++ )
                {
                    dy->p(x,y) += ( gI->bp(x,y-k) - gI->bp(x,y+k) ) * dgau[k];
                }
            }
        }

// ******** Compute magnitude and binarization thresholds
        IPLImagePlane* mag = new IPLImagePlane( width, height );
        double magMax = 0.0;
        double magMin = 999999999.0;
        for(int x=0; x<width; x++)
        {
            for(int y=0; y<height; y++)
            {
                double val = sqrt( dx->p(x,y)*dx->p(x,y) + dy->p(x,y)*dy->p(x,y) );
                mag->p(x,y) = val;
                if( val > magMax ) magMax = val;
                if( val < magMin ) magMin = val;
            }
        }

//// ******** Non-maxima suppression - edge pixels should be a local maximum
        _orientedImage = new IPLOrientedImage( width, height );
        for(int x=0; x<width; x++)
        {
            for(int y=0; y<height; y++)
            {
                double ix = dx->p(x,y);
                double iy = dy->p(x,y);
                double g = mag->p(x,y);

                // determine 4-neighbor direction of gradient
                int dir4 = 0;
                if( (iy<=0.0 && ix>-iy) || (iy>=0.0 && ix<-iy) )
                    dir4 = 1;
                else if( (ix>0.0 && -iy>=ix) || (ix<0.0 && -iy<=ix) )
                    dir4 = 2;
                else if( (ix<=0.0 && ix>iy) || (ix>=0.0 && ix<iy) )
                    dir4 = 3;
                else if( (iy<0.0 && ix<=iy) || (iy>0.0 && ix>=iy) )
                    dir4 = 4;
                else
                    continue;

                double gradmag1, gradmag2, d;
                switch(dir4)
                {
                    case 1: d = std::fabs(iy/ix);
                            gradmag1 = mag->bp(x+1,y)*(1-d) + mag->bp(x+1,y-1)*d;
                            gradmag2 = mag->bp(x-1,y)*(1-d) + mag->bp(x-1,y+1)*d;
                            break;
                    case 2: d = std::fabs(ix/iy);
                            gradmag1 = mag->bp(x,y-1)*(1-d) + mag->bp(x+1,y-1)*d;
                            gradmag2 = mag->bp(x,y+1)*(1-d) + mag->bp(x-1,y+1)*d;
                            break;
                    case 3: d = std::fabs(ix/iy);
                            gradmag1 = mag->bp(x,y-1)*(1-d) + mag->bp(x-1,y-1)*d;
                            gradmag2 = mag->bp(x,y+1)*(1-d) + mag->bp(x+1,y+1)*d;
                            break;
                    case 4: d = std::fabs(iy/ix);
                            gradmag1 = mag->bp(x-1,y)*(1-d) + mag->bp(x-1,y-1)*d;
                            gradmag2 = mag->bp(x+1,y)*(1-d) + mag->bp(x+1,y+1)*d;
                            break;
                }

                if( g > gradmag1 && g > gradmag2 )
                {
                    _orientedImage->magnitude(x,y) = g;
                    _orientedImage->phase(x,y) = atan2(iy,ix);
                }
            }

        }


        for(int x=0; x<width; x++)
        {
            for(int y=0; y<height; y++)
            {
                _orientedImage->magnitude(x,y) /= magMax;
                double val = _orientedImage->magnitude(x,y)*255.0;
//					double val = mag->f(x,y)/magMax*255.0;
                if (val > 255.0 ) val = 255.0;
                if (val < 0.0 ) val = 0.0;
                newplane->p(x,y) = (unsigned char ) val;
            }
        }

// ******** Binarize with hysteresis threshold
        double hist[ 256 ];
        for( int i=0; i<256; ++i )
            hist[i] = 0;
        int pixCount = 0;
        for(int x=0; x<width; x++)
        {
            for(int y=0; y<height; y++)
            {
                if( _orientedImage->magnitude(x,y) > 0.0 )
                {
                    int index = floor( _orientedImage->magnitude(x,y)*256.0+0.5 );
                    ++hist[ index ];
                    ++pixCount;
                }
            }
        }
        double PercentOfPixelsNotEdges = 0.7*pixCount;
        double highThresh = 0.0;
        double cumsum = 0.0;
        for( int i=0; i<256; ++i )
        {
            cumsum += hist[i];
            if( cumsum > PercentOfPixelsNotEdges )
            {
                highThresh = (double)i / 256.0;
                break;
            }
        }
        double lowThresh = 0.4 * highThresh;
        IPLImagePlane* binPlane = _binaryImage->plane( 0 );
        for(int x=0; x<width; x++)
        {
            for(int y=0; y<height; y++)
            {
                if(_orientedImage->magnitude(x,y) >= highThresh)
                    trace(x, y, lowThresh, _orientedImage, binPlane);
            }
        }
        //delete dx;
        //delete dy;
        //delete gI;

        thinning(_orientedImage, binPlane, newplane );
    }

    //delete [] gau;
    //delete [] dgau;

    return true;
}
コード例 #21
0
void AlbersConEqArea::_inverse(double x, double y)
{
	double rh1;			/* height above ellipsoid	*/
	double qs;			/* function q			*/
	double con;			/* temporary sign value		*/
	double theta;			/* angle			*/
	long   flag;			/* error flag;			*/
	
	flag = 0;
	x -= m_falseEasting;
	y = m_rh - y + m_falseNorthing;;
	
	if (m_ns0 >= 0)
	{
		rh1 = sqrt(x * x + y * y);
		con = 1.0;
    }
	
	else
	{
		rh1 = -sqrt(x * x + y * y);
		con = -1.0;
	}

	theta = 0.0;
	if (rh1 != 0.0)
		theta = atan2(con * x, con * y);
	
	con = rh1 * m_ns0 / m_rMajor;
	qs = (m_c - con * con) / m_ns0;
	
	if (m_e >= 1e-10)
	{
		con = 1 - .5 * (1.0 - m_es) * log((1.0 - m_e) / (1.0 + m_e))/m_e;
		if (fabs(fabs(con) - fabs(qs)) > .0000000001 )
		{
			m_latitude = Util::phi1z(m_e,qs,&flag);
			if (flag != 0) {
				setError(flag);
				return;
			}
		}

		else
	
		{
		 if (qs >= 0)
			m_latitude = .5 * PI;
		 else
			m_latitude = -.5 * PI;
		}
	}
	
	else
	{
		m_latitude = Util::phi1z(m_e,qs,&flag);
		if (flag != 0) {
			setError(flag);
			return;
		}
    }

	m_longitude = Util::adjust_lon(theta/m_ns0 + m_centerLon);

}
コード例 #22
0
ファイル: HMC5883.cpp プロジェクト: Quirade/TiltyIMU
/**
*@param[in] x X magnetometer value.
*@param[in] y Y magnetometer value.
*/
float HMC5883::getHeading(int x, int y)
{	
	return (atan2(y, x) + M_PI) * 180/M_PI;
}
コード例 #23
0
ファイル: s_carg.c プロジェクト: MattDooner/freebsd-west
double
carg(double complex z)
{

	return (atan2(cimag(z), creal(z)));
}
コード例 #24
0
ファイル: HMC5883.cpp プロジェクト: Quirade/TiltyIMU
/**
*@param[in] x X magnetometer value.
*@param[in] y Y magnetometer value.
*/
float HMC5883::getHeadingRadians(int x, int y)
{	
	return (atan2(y, x) + M_PI);
}
コード例 #25
0
ファイル: Point3.cpp プロジェクト: DKarlberg/OpenDaVINCI
 double Point3::getAngleXY() const {
     return atan2(getY(), getX());
 }
コード例 #26
0
ファイル: plan_arc.c プロジェクト: Cicciuzz/TinyG
static stat_t _compute_arc()
{
	// Compute radius. A non-zero radius value indicates a radius arc
    if (fp_NOT_ZERO(arc.radius)) {                  // indicates a radius arc
        _compute_arc_offsets_from_radius();
    } else {                                        // compute start radius
        arc.radius = hypotf(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]);
    }

    // Test arc specification for correctness according to:
    // http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc
    // "It is an error if: when the arc is projected on the selected plane, the distance from
    //  the current point to the center differs from the distance from the end point to the
    //  center by more than (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)."

    // Compute end radius from the center of circle (offsets) to target endpoint
    float end_0 = arc.gm.target[arc.plane_axis_0] - arc.position[arc.plane_axis_0] - arc.offset[arc.plane_axis_0];
    float end_1 = arc.gm.target[arc.plane_axis_1] - arc.position[arc.plane_axis_1] - arc.offset[arc.plane_axis_1];
    float err = fabs(hypotf(end_0, end_1) - arc.radius);   // end radius - start radius
    if ( (err > ARC_RADIUS_ERROR_MAX) || 
        ((err < ARC_RADIUS_ERROR_MIN) && 
         (err > arc.radius * ARC_RADIUS_TOLERANCE)) ) {
//        return (STAT_ARC_HAS_IMPOSSIBLE_CENTER_POINT);
        return (STAT_ARC_SPECIFICATION_ERROR);
    }

	// Calculate the theta (angle) of the current point (position)
	// arc.theta is angular starting point for the arc (also needed later for calculating center point)
    arc.theta = atan2(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]);

    // g18_correction is used to invert G18 XZ plane arcs for proper CW orientation
    float g18_correction = (cm.gm.select_plane == CANON_PLANE_XZ) ? -1 : 1;

	if (arc.full_circle) {                                  // if full circle you can skip the stuff in the else clause
    	arc.angular_travel = 0;                             // angular travel always starts as zero for full circles
    	if (fp_ZERO(arc.rotations)) {                       // handle the valid case of a full circle arc w/P=0
            arc.rotations = 1.0;
        }
    } else {                                                // ... it's not a full circle
        arc.theta_end = atan2(end_0, end_1);

        // Compute the angular travel
        if (fp_EQ(arc.theta_end, arc.theta)) {
	        arc.angular_travel = 0;                         // very large radii arcs can have zero angular travel (thanks PartKam)
        } else {
	        if (arc.theta_end < arc.theta) {                // make the difference positive so we have clockwise travel
                arc.theta_end += (2*M_PI * g18_correction);
            }
	        arc.angular_travel = arc.theta_end - arc.theta; // compute positive angular travel
    	    if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) { // reverse travel direction if it's CCW arc
                arc.angular_travel -= (2*M_PI * g18_correction);
            }
        }
	}

    // Add in travel for rotations
    if (cm.gm.motion_mode == MOTION_MODE_CW_ARC) {
        arc.angular_travel += (2*M_PI * arc.rotations * g18_correction);
    } else {
        arc.angular_travel -= (2*M_PI * arc.rotations * g18_correction);
    }

	// Calculate travel in the depth axis of the helix and compute the time it should take to perform the move
	// arc.length is the total mm of travel of the helix (or just a planar arc)
	arc.linear_travel = arc.gm.target[arc.linear_axis] - arc.position[arc.linear_axis];
	arc.planar_travel = arc.angular_travel * arc.radius;
	arc.length = hypotf(arc.planar_travel, arc.linear_travel);  // NB: hypot is insensitive to +/- signs
	_estimate_arc_time();	// get an estimate of execution time to inform arc_segment calculation

	// Find the minimum number of arc_segments that meets these constraints...
	float arc_segments_for_chordal_accuracy = arc.length / sqrt(4*cm.chordal_tolerance * (2 * arc.radius - cm.chordal_tolerance));
	float arc_segments_for_minimum_distance = arc.length / cm.arc_segment_len;
	float arc_segments_for_minimum_time = arc.arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC;

	arc.arc_segments = floor(min3(arc_segments_for_chordal_accuracy,
							      arc_segments_for_minimum_distance,
							      arc_segments_for_minimum_time));

	arc.arc_segments = max(arc.arc_segments, 1);            //...but is at least 1 arc_segment
 	arc.gm.move_time = arc.arc_time / arc.arc_segments;     // gcode state struct gets arc_segment_time, not arc time
	arc.arc_segment_count = (int32_t)arc.arc_segments;
	arc.arc_segment_theta = arc.angular_travel / arc.arc_segments;
	arc.arc_segment_linear_travel = arc.linear_travel / arc.arc_segments;
    arc.center_0 = arc.position[arc.plane_axis_0] - sin(arc.theta) * arc.radius;
    arc.center_1 = arc.position[arc.plane_axis_1] - cos(arc.theta) * arc.radius;
	arc.gm.target[arc.linear_axis] = arc.position[arc.linear_axis];	// initialize the linear target
	return (STAT_OK);
}
コード例 #27
0
ファイル: OSVR-Tracker.cpp プロジェクト: leiwen83/Perception
/**
* Handle OSVR tracking.
***/
void* OSVR_Tracker::Provoke(void* pThis, int eD3D, int eD3DInterface, int eD3DMethod, DWORD dwNumberConnected, int& nProvokerIndex)
{
	// update game timer
	m_cGameTimer.Tick();

	static UINT unFrameSkip = 200;
	if (unFrameSkip > 0)
	{
		unFrameSkip--;
		return nullptr;
	}

	if ((!m_psOSVR_ClientContext) || (!m_psOSVR_ClientInterface))
	{
		// create client context handle
		m_psOSVR_ClientContext =
			osvrClientInit("com.mtbs3d.vireio.osvr.tracker", 0);

		// get client interface
		osvrClientGetInterface(m_psOSVR_ClientContext, "/me/head", &m_psOSVR_ClientInterface);
	}
	else
	{
		// update the client context
		osvrClientUpdate(m_psOSVR_ClientContext);

		// let's read the tracker state.
		OSVR_ReturnCode cRet =
			osvrGetPoseState(m_psOSVR_ClientInterface, &m_sTimestamp, &m_sState);
		if (cRet != OSVR_RETURN_SUCCESS)
		{
			OutputDebugStringA("No pose state!\n");
		}
		else
		{
			m_bControlUpdate = true;

			// backup old euler angles and velocity
			float afEulerOld[3];
			float afEulerVelocityOld[3];
			memcpy(&afEulerOld[0], &m_afEuler[0], sizeof(float)* 3);
			memcpy(&afEulerVelocityOld[0], &m_afEulerVelocity[0], sizeof(float)* 3);

			// quaternion -> euler angles
			const float w = (float)m_sState.rotation.data[0];
			const float x = (float)m_sState.rotation.data[1];
			const float y = (float)m_sState.rotation.data[2];
			const float z = (float)m_sState.rotation.data[3];

			float sqw = w*w;
			float sqx = x*x;
			float sqy = y*y;
			float sqz = z*z;

			float unit = sqx + sqy + sqz + sqw;
			float test = x*y + z*w;

			if (test > 0.499*unit)
			{
				// singularity at north pole
				m_afEuler[1] = 2 * atan2(x, w);
				m_afEuler[2] = FLOAT_PI / 2;
				m_afEuler[0] = 0;
			}
			else if (test < -0.499*unit)
			{
				// singularity at south pole
				m_afEuler[1] = -2 * atan2(x, w);
				m_afEuler[2] = -FLOAT_PI / 2;
				m_afEuler[0] = 0;
			}
			else
			{
				m_afEuler[1] = atan2(2 * y*w - 2 * x*z, sqx - sqy - sqz + sqw);
				m_afEuler[2] = asin(2 * test / unit);
				m_afEuler[0] = atan2(2 * x * w - 2 * y * z, -sqx + sqy - sqz + sqw);
			}

			// PITCH = atan2(2.0 * (x * y + w * z), w * w + x * x - y * y - z * z);
			// ROLL = atan2(2 * y * w - 2 * x * z, 1 - 2 * y * y - 2 * z * z);

			// get euler velocity + acceleration
			float afEulerAcceleration[3];
			for (UINT unI = 0; unI < 3; unI++)
			{
				// get the velocity
				m_afEulerVelocity[unI] = (m_afEuler[unI] - afEulerOld[unI]) / (float)m_cGameTimer.DeltaTime();

				// get the acceleration
				afEulerAcceleration[unI] = (m_afEulerVelocity[unI] - afEulerVelocityOld[unI]) / (float)m_cGameTimer.DeltaTime();
			}

			// get predicted euler
			for (UINT unI = 0; unI < 3; unI++)
			{
				// compute predicted euler
				m_afEulerPredicted[unI] = (0.5f * afEulerAcceleration[unI] * ((float)m_cGameTimer.DeltaTime() * (float)m_cGameTimer.DeltaTime())) + (m_afEulerVelocity[unI] * (float)m_cGameTimer.DeltaTime()) + m_afEuler[unI];
			}

			// set position
			m_afPosition[0] = (float)m_sState.translation.data[0];
			m_afPosition[1] = (float)m_sState.translation.data[1];
			m_afPosition[2] = (float)m_sState.translation.data[2];

#ifdef _DEBUG
			// output debug data
			std::wstringstream szPose;
			szPose << L"Got POSE state: Position = ("
				<< m_sState.translation.data[0] << L", "
				<< m_sState.translation.data[1] << L", "
				<< m_sState.translation.data[2] << L"), orientation = ("
				<< osvrQuatGetW(&(m_sState.rotation)) << L", "
				<< osvrQuatGetX(&(m_sState.rotation)) << L", "
				<< osvrQuatGetY(&(m_sState.rotation)) << L", "
				<< osvrQuatGetZ(&(m_sState.rotation)) << L")";
			OutputDebugString(szPose.str().c_str());
#endif
		}
	}

	return nullptr;
}
コード例 #28
0
ファイル: grid.c プロジェクト: aesop972/ngspice-gss
static void
arcset(GRAPH *graph, double rad, double prevrad, double irad, double iprevrad, double radoff, int maxrad, int centx, int centy, int xoffset, int yoffset, char *plab, char *nlab, int pdeg, int ndeg, int pxmin, int pxmax)
{
    double aclip;
    double angle = atan2((double) iprevrad, (double) rad);
    double iangle = atan2((double) prevrad, (double) irad);
    int x, xlab, ylab;

    /* Let's be lazy and just draw everything -- we won't get called too
     * much and the circles get clipped anyway...
     */
    SetColor(18);

    cliparc((double) (centx + xoffset + radoff - rad),
	    (double) (centy + yoffset), rad, 2*angle,
            2 * M_PI - 2 * angle, centx, centy, maxrad, 0);


    /* These circles are not part of the smith chart
     * Let's draw them anyway
     */
    cliparc((double) (centx + xoffset + radoff + rad),
	    (double) (centy + yoffset), rad, M_PI + 2 * angle,
            M_PI - 2 * angle, centx, centy, maxrad, 0);

    /* Draw the upper and lower circles.  */
    SetColor(19);
    aclip = cliparc((double) (centx + xoffset + radoff),
	    (double) (centy + yoffset + irad), irad,
	    (double) (M_PI * 1.5 + 2 * iangle),
            (double) (M_PI * 1.5 - 2 * iangle), centx, centy, maxrad, 1);
    if ((aclip > M_PI / 180) && (pdeg > 1)) {
	xlab = centx + xoffset + radoff + irad * cos(aclip);
	ylab = centy + yoffset + irad * (1 + sin(aclip));
	if ((ylab - gr_ycenter) > graph->fontheight) {
	SetColor(1);
	adddeglabel(graph, pdeg, xlab, ylab,
	    gr_xcenter, gr_ycenter, gr_xcenter, gr_ycenter);
/*
	ylab = centy + yoffset - irad * (1 + sin(aclip));
	adddeglabel(graph, ndeg, xlab, ylab,
	    gr_xcenter, gr_ycenter, gr_xcenter, gr_ycenter);
 */
	SetColor(19);
	}
    }
    aclip = cliparc((double) (centx + xoffset + radoff),
	    (double) (centy + yoffset - irad), irad,
	    (double) (M_PI / 2 + 2 * iangle),
            (double) (M_PI / 2 - 2 * iangle), centx, centy, maxrad,
	    (iangle == 0)?2:0);
    if ((aclip >= 0 && aclip < 2*M_PI - M_PI/180) && (pdeg < 359)) {
	xlab = centx + xoffset + radoff + irad * cos(aclip);
	ylab = centy + yoffset + irad * (sin(aclip) - 1);
	SetColor(1);
	adddeglabel(graph, ndeg, xlab, ylab,
	    gr_xcenter, gr_ycenter, gr_xcenter, gr_ycenter);
	SetColor(19);
    }
    
    /* Now toss the labels on... */
    SetColor(1);

    x = centx + xoffset + (int)radoff - 2 * (int)rad -
	gi_fntwidth * strlen(plab) - 2;
    if ((x > pxmin) && (x < pxmax)) {
	if ((yoffset > - gr_radius) && (yoffset < gr_radius))
	    Text(plab, x, centy + yoffset - gi_fntheight - 1);
	else
	    Text(plab, x, gr_ymargin - 3 * gi_fntheight - 2);
    }
/*
    x = centx + xoffset + (int) radoff + 2 * (int)rad -
	gi_fntwidth * strlen(nlab) - 2;
    if ((x > gr_xmargin) && (x < gi_maxx))
        Text(nlab, x, centy + yoffset - gi_fntheight - 1);
 */

    return;
}
コード例 #29
0
ファイル: bsdftest.cpp プロジェクト: NickYang/pbrt-v3
int main(int argc, char* argv[]) {
    Options opt;
    pbrtInit(opt);

    // number of monte carlo estimates
    // const int estimates = 1;
    const int estimates = 10000000;

    // radiance of uniform environment map
    const double environmentRadiance = 1.0;

    fprintf(stderr,
            "outgoing radiance from a surface viewed\n"
            "straight on with uniform lighting\n\n"
            "    uniform incoming radiance = %.3f\n"
            "    monte carlo samples = %d\n\n\n",
            environmentRadiance, estimates);

    CreateBSDFFunc BSDFFuncArray[] = {
        createLambertian,
        createOrenNayar0,
        createOrenNayar20,
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, true, true, 0.5, 0.5); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, false, true, 0.5, 0.5); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, true, true, 0.2, 0.1); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, false, true, 0.2, 0.1); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, true, true, 0.15, 0.25); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, false, true, 0.15, 0.25); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, true, true, 0.33, 0.033); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, false, true, 0.33, 0.033); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, true, false, 0.5, 0.5); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, false, false, 0.5, 0.5); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, true, false, 0.2, 0.1); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, false, false, 0.2, 0.1); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, true, false, 0.15, 0.25); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, false, false, 0.15, 0.25); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, true, false, 0.33, 0.033); },
        [](BSDF* bsdf) -> void
        { createMicrofacet(bsdf, false, false, 0.33, 0.033); },
        [](BSDF* bsdf) -> void
        { createFresnelBlend(bsdf, true, true, 0.15, 0.25); },
        [](BSDF* bsdf) -> void
        { createFresnelBlend(bsdf, false, true, 0.15, 0.25); },
        [](BSDF* bsdf) -> void
        { createFresnelBlend(bsdf, true, false, 0.15, 0.25); },
        [](BSDF* bsdf) -> void
        { createFresnelBlend(bsdf, false, false, 0.15, 0.25); },
    };

    const char* BSDFFuncDescripArray[] = {
        "Lambertian",
        "Oren Nayar (sigma 0)",
        "Oren Nayar (sigma 20)",
        "Beckmann (roughness 0.5, sample visible mf area)",
        "Trowbridge-Reitz (roughness 0.5, sample visible mf area)",
        "Beckmann (roughness 0.2/0.1, sample visible mf area)",
        "Trowbridge-Reitz (roughness 0.2/0.1, sample visible mf area)",
        "Beckmann (roughness 0.15/0.25, sample visible mf area)",
        "Trowbridge-Reitz (roughness 0.15/0.25, sample visible mf area)",
        "Beckmann (roughness 0.33/0.033, sample visible mf area)",
        "Trowbridge-Reitz (roughness 0.33/0.033, sample visible mf area)",
        "Beckmann (roughness 0.5, traditional sample wh)",
        "Trowbridge-Reitz (roughness 0.5, traditional sample wh)",
        "Beckmann (roughness 0.2/0.1, traditional sample wh)",
        "Trowbridge-Reitz (roughness 0.2/0.1, traditional sample wh)",
        "Beckmann (roughness 0.15/0.25, traditional sample wh)",
        "Trowbridge-Reitz (roughness 0.15/0.25, traditional sample wh)",
        "Beckmann (roughness 0.33/0.033, traditional sample wh)",
        "Trowbridge-Reitz (roughness 0.33/0.033, traditional sample wh)",
        "Fresnel Blend Beckmann (roughness 0.15/0.25, sample visible mf area)",
        "Fresnel Blend Trowbridge-Reitz (roughness 0.15/0.25, sample visible mf area)",
        "Fresnel Blend Beckmann (roughness 0.15/0.25, traditional sample wh)",
        "Fresnel Blend Trowbridge-Reitz (roughness 0.15/0.25, traditional sample wh)",
    };

    GenSampleFunc SampleFuncArray[] = {
        Gen_Sample_f,
        // CO        Gen_CosHemisphere,
        // CO        Gen_UniformHemisphere,
    };

    const char* SampleFuncDescripArray[] = {
        "BSDF Importance Sampling",
        // CO        "Cos Hemisphere",
        // CO        "Uniform Hemisphere",
    };

    int numModels = sizeof(BSDFFuncArray) / sizeof(BSDFFuncArray[0]);
    int numModelsDescrip =
        sizeof(BSDFFuncDescripArray) / sizeof(BSDFFuncDescripArray[0]);
    int numGenerators = sizeof(SampleFuncArray) / sizeof(SampleFuncArray[0]);
    int numGeneratorsDescrip =
        sizeof(SampleFuncDescripArray) / sizeof(SampleFuncDescripArray[0]);

    if (numModels != numModelsDescrip) {
        fprintf(stderr,
                "BSDFFuncArray and BSDFFuncDescripArray out of sync!\n");
        exit(1);
    }

    if (numGenerators != numGeneratorsDescrip) {
        fprintf(stderr,
                "SampleFuncArray and SampleFuncDescripArray out of sync!\n");
        exit(1);
    }

    // for each bsdf model
    for (int model = 0; model < numModels; model++) {
        BSDF* bsdf;

        // create BSDF which requires creating a Shape, casting a Ray
        // that hits the shape to get a SurfaceInteraction object.
        {
            Transform t = RotateX(-90);
            bool reverseOrientation = false;
            ParamSet p;

            std::shared_ptr<Shape> disk(
                new Disk(new Transform(t), new Transform(Inverse(t)),
                         reverseOrientation, 0., 1., 0, 360.));
            Point3f origin(
                0.1, 1, 0);  // offset slightly so we don't hit center of disk
            Vector3f direction(0, -1, 0);
            Float tHit;
            Ray r(origin, direction);
            SurfaceInteraction isect;
            disk->Intersect(r, &tHit, &isect);
            bsdf = ARENA_ALLOC(arena, BSDF)(isect);
            (BSDFFuncArray[model])(bsdf);
        }

        // facing directly at normal
        Vector3f woL = Normalize(Vector3f(0, 0, 1));
        Vector3f wo = bsdf->LocalToWorld(woL);
        // was bsdf->dgShading.nn
        const Normal3f n = Normal3f(bsdf->LocalToWorld(Vector3f(0, 0, 1)));

        // for each method of generating samples over the hemisphere
        for (int gen = 0; gen < numGenerators; gen++) {
            double redSum = 0.0;

            const int numHistoBins = 10;
            double histogram[numHistoBins][numHistoBins];
            for (int i = 0; i < numHistoBins; i++) {
                for (int j = 0; j < numHistoBins; j++) {
                    histogram[i][j] = 0;
                }
            }
            int badSamples = 0;
            int outsideSamples = 0;

            int warningTarget = 1;
            for (int sample = 0; sample < estimates; sample++) {
                Vector3f wi;
                Float pdf;
                Spectrum f;

                // sample hemisphere around bsdf, wo is fixed
                (SampleFuncArray[gen])(bsdf, wo, &wi, &pdf, &f);

                double redF = spectrumRedValue(f);

                // add hemisphere sample to histogram
                Vector3f wiL = bsdf->WorldToLocal(wi);
                float x = Clamp(wiL.x, -1.f, 1.f);
                float y = Clamp(wiL.y, -1.f, 1.f);
                float wiPhi = (atan2(y, x) + Pi) / (2.0 * Pi);
                float wiCosTheta = wiL.z;
                bool validSample = (wiCosTheta > 1e-7);
                if (wiPhi < -0.0001 || wiPhi > 1.0001 || wiCosTheta > 1.0001) {
                    // wiCosTheta can be less than 0
                    fprintf(stderr, "bad wi! %.3f %.3f %.3f, (%.3f %.3f)\n",
                            wiL[0], wiL[1], wiL[2], wiPhi, wiCosTheta);
                } else if (validSample) {
                    int histoPhi = (int)(wiPhi * numHistoBins);
                    if (histoPhi == numHistoBins)
                      --histoPhi;
                    int histoCosTheta = (int)(wiCosTheta * numHistoBins);
                    if (histoCosTheta == numHistoBins)
                      --histoCosTheta;
                    assert(histoPhi >= 0 && histoPhi < numHistoBins);
                    assert(histoCosTheta >= 0 && histoCosTheta < numHistoBins);
                    histogram[histoCosTheta][histoPhi] += 1.0 / pdf;
                }

                if (!validSample) {
                    outsideSamples++;
                } else if (pdf == 0.f || std::isnan(pdf) || redF < 0 ||
                           std::isnan(redF)) {
                    if (badSamples == warningTarget) {
                        fprintf(stderr,
                                "warning %d, bad sample %d! "
                                "pdf: %.3f, redF: %.3f\n",
                                warningTarget, sample, pdf, redF);
                        warningTarget *= 10;
                    }
                    badSamples++;
                } else {
                    // outgoing radiance estimate =
                    //   bsdf * incomingRadiance * cos(wi) / pdf
                    redSum += redF * environmentRadiance * AbsDot(wi, n) / pdf;
                }
            }
            int goodSamples = estimates - badSamples;

            // print results
            fprintf(stderr,
                    "*** BRDF: '%s', Samples: '%s'\n\n"
                    "wi histogram showing the relative weight in each bin\n"
                    "  all entries should be close to 2pi = %.5f:\n"
                    "  (%d bad samples, %d outside samples)\n\n"
                    "                          phi bins\n",
                    BSDFFuncDescripArray[model], SampleFuncDescripArray[gen],
                    Pi * 2.0, badSamples, outsideSamples);
            double totalSum = 0.0;
            for (int i = 0; i < numHistoBins; i++) {
                fprintf(stderr, "  cos(theta) bin %02d:", i);
                for (int j = 0; j < numHistoBins; j++) {
                    fprintf(stderr, " %5.2f", histogram[i][j] * numHistoBins *
                                                  numHistoBins / goodSamples);
                    totalSum += histogram[i][j];
                }
                fprintf(stderr, "\n");
            }
            fprintf(stderr,
                    "\n  final average :  %.5f (error %.5f)\n\n"
                    "  radiance = %.5f\n\n",
                    totalSum / goodSamples, totalSum / goodSamples - Pi * 2.0,
                    redSum / goodSamples);
        }
    }

    pbrtCleanup();
    return 0;
}
コード例 #30
0
ファイル: yadro.cpp プロジェクト: Ztaz/cpp
void Yadro::fly(GLfloat sferaV[80][3][3]/*, int flag*/, Sfera &sfera)
{
    const GLfloat pi=3.141593, k=pi/180;


    GLfloat x21 = sferaV[pli+plifinish][0][0]; GLfloat x22 = sferaV[pli+plifinish][1][0];  GLfloat x23 = sferaV[pli+plifinish][2][0];
    GLfloat y21 = sferaV[pli+plifinish][0][1]; GLfloat y22 = sferaV[pli+plifinish][1][1];  GLfloat y23 = sferaV[pli+plifinish][2][1];
    GLfloat z21 = sferaV[pli+plifinish][0][2]; GLfloat z22 = sferaV[pli+plifinish][1][2];  GLfloat z23 = sferaV[pli+plifinish][2][2];

    GLfloat x2 = (x21+x22+x23)/3;
    GLfloat y2 = (y21+y22+y23)/3;
    GLfloat z2 = (z21+z22+z23)/3;

    GLfloat r2      = sqrt(x2*x2 + y2*y2 + z2*z2);
    GLfloat theta2  = acos(z2/r2);
    GLfloat phi2    = atan2(y2,x2);


    for (int i = 0; i < 6; i++) {
        // Расчеты координат движущегося ядра
        GLfloat x1 = VertexArrayYadro[i][0];
        GLfloat y1 = VertexArrayYadro[i][1];
        GLfloat z1 = VertexArrayYadro[i][2];

        GLfloat r1 = sqrt(x1*x1 + y1*y1 + z1*z1);
        GLfloat theta1 = acos(z1/r1);
        GLfloat phi1 = atan2(y1,x1);

        if (delta1 == 0.0f) {
            // установка начальных параметров запуска
            delta1 = 1.0f;
            shag   = 50;
            deltaR = 0.05f;
            deltaTh = fabs(theta2 - theta1);
            deltaPh = fabs( phi2  -  phi1 );
            deltaTh = deltaTh / (GLfloat)shag;
            deltaPh = deltaPh / (GLfloat)shag;
            if ( theta1 < theta2) {
                signTh  = 1;
            } else {
                signTh  = 0;
            }

            if (  phi1  <  phi2 ) {
                signPh  = 1;
            } else {
                signPh  = 0;
            }
        }

        // По флагам усанавливаем стороны вращения
        if ( signTh  == 1 ) {
            theta1+=deltaTh;
        } else {
            theta1-=deltaTh;
        }

        if ( signPh  == 1 ) {
            phi1+=deltaPh;
        } else {
            phi1-=deltaPh;
        }

        if ( shag > 25) {
            r1+= deltaR;
        } else {
            r1-= deltaR;
        }


        if (shag > 0) {
            VertexArrayYadro[i][0]=r1*sin(theta1)*cos(phi1);
            VertexArrayYadro[i][1]=r1*sin(theta1)*sin(phi1);
            VertexArrayYadro[i][2]=r1*cos(theta1);
            int h = 43;
            if ( i == 5 ) {
                shag--;
            }
            doletelo = 0;
        }
        if (shag == 0 && pli < 80) {
            // Признак сработал. Ракета прилетела
            sfera.fail(pli+plifinish);
            doletelo = 1;
        }
    }
}