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; } } }
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(); } }
/*** 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); }
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); }
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]; }
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); }
/* 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]; } } } }
/******************************************************************************** 直线描述子 ********************************************************************************/ 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); }
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 */
/** * @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>(); }
/** * @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>(); }
//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); }
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 ); } }
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; }
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); }
double argument() const { return atan2(im, re); }
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; }
/* =============== 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 ); }
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); }
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; }
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); }
/** *@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; }
double carg(double complex z) { return (atan2(cimag(z), creal(z))); }
/** *@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); }
double Point3::getAngleXY() const { return atan2(getY(), getX()); }
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); }
/** * 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; }
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; }
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; }
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; } } }