static void setSpherePoint(vec3 *p, vec3* n, vec3* t, int latitude, int longitude, double sx, double sy, double sz, double ox, double oy, double oz) { double dToR = M_PI / 180.0; double len; // Set vertex position p->x = ox + sin(longitude * dToR) * cos(latitude * dToR) * sx; p->y = oy + sin(latitude * dToR) * sy; p->z = oz + cos(longitude * dToR) * cos(latitude * dToR) * sz; // Set texture coordinate t->x = 0.0; t->y = 0.0; // calculate normal, this actually doesn't take the sphere size // per axis into account, but should still be usable n->x = p->x - ox; n->y = p->y - oy; n->z = p->z - oz; len = n->x*n->x + n->y*n->y + n->z*n->z; n->x /= len; n->y /= len; n->z /= len; }

//! [4] void Edge::paint(QPainter *painter, const QStyleOptionGraphicsItem *, QWidget *) { if (!source || !dest) return; QLineF line(sourcePoint, destPoint); if (qFuzzyCompare(line.length(), qreal(0.))) return; //! [4] //! [5] // Draw the line itself painter->setPen(QPen(Qt::black, 1, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); painter->drawLine(line); //! [5] //! [6] // Draw the arrows double angle = ::acos(line.dx() / line.length()); if (line.dy() >= 0) angle = TwoPi - angle; QPointF sourceArrowP1 = sourcePoint + QPointF(sin(angle + Pi / 3) * arrowSize, cos(angle + Pi / 3) * arrowSize); QPointF sourceArrowP2 = sourcePoint + QPointF(sin(angle + Pi - Pi / 3) * arrowSize, cos(angle + Pi - Pi / 3) * arrowSize); QPointF destArrowP1 = destPoint + QPointF(sin(angle - Pi / 3) * arrowSize, cos(angle - Pi / 3) * arrowSize); QPointF destArrowP2 = destPoint + QPointF(sin(angle - Pi + Pi / 3) * arrowSize, cos(angle - Pi + Pi / 3) * arrowSize); painter->setBrush(Qt::black); painter->drawPolygon(QPolygonF() << line.p1() << sourceArrowP1 << sourceArrowP2); painter->drawPolygon(QPolygonF() << line.p2() << destArrowP1 << destArrowP2); }

void poisson_r(double *q, int m, int NR, int NZ, double dr, double dz) { int j; double *a,*b,*c,*r,alpha,beta; alpha=2.0*(cos(2.0*PI*m/(NZ))-1.0)/pow(dz,2); beta=4.0/pow(dr,2); a = new double[NR]; b = new double[NR]; c = new double[NR]; r = new double[NR]; for(j=1; j<NR; j++) { a[j]=(1.0-0.5/((double)j))/pow(dr,2); c[j]=(1.0+0.5/((double)j))/pow(dr,2); b[j]=2.0*((cos(2.0*PI*m/(NZ))-1.0)/pow(dz,2)-1.0/pow(dr,2)); r[j]=-q[j]/eps0; } b[0]=alpha-beta; c[0]=beta; r[0]=-q[0]/eps0; tridiag(q,a,b,c,r,NR); delete a; delete b; delete c; delete r; }

void CFPCamera::SetFPCamera(Vector3F & pos, float fPhi, float fTheta) { m_Pos = pos; m_fPhi = fPhi; m_fTheta = fTheta; m_RightVec.x = cosf(m_fPhi-D3DX_PI/2.); m_RightVec.y = 0; m_RightVec.z = sinf(m_fPhi-D3DX_PI/2.); m_ForwardVec.x = cos(m_fPhi)*cos(m_fTheta); m_ForwardVec.y = sin(m_fTheta); m_ForwardVec.z = sin(m_fPhi)*cos(m_fTheta); m_ForwardVec = m_ForwardVec.Normalize(); m_UpVec = CrossXYZ(m_ForwardVec, m_RightVec); float x = -DotXYZ(m_Pos, m_RightVec); float y = -DotXYZ(m_Pos, m_UpVec); float z = -DotXYZ(m_Pos, m_ForwardVec); m_View.m[0][0] = m_RightVec.x, m_View.m[1][0] = m_RightVec.y, m_View.m[2][0] = m_RightVec.z, m_View.m[3][0] = x; m_View.m[0][1] = m_UpVec.x, m_View.m[1][1] = m_UpVec.y, m_View.m[2][1] = m_UpVec.z, m_View.m[3][1] = y; m_View.m[0][2] = m_ForwardVec.x, m_View.m[1][2] = m_ForwardVec.y, m_View.m[2][2] = m_ForwardVec.z, m_View.m[3][2] = z; m_View.m[0][3] = 0, m_View.m[1][3] = 0, m_View.m[2][3] = 0, m_View.m[3][3] = 1; }

GAIAGEO_DECLARE double gaiaGreatCircleDistance (double a, double b, double lat1, double lon1, double lat2, double lon2) { /* / Calculate great-circle distance (in m) between two points specified by / latitude/longitude (in decimal degrees) using Aviation Formulary / / http://williams.best.vwh.net/avform.htm#Dist / */ double latrad1 = lat1 * DEG2RAD; double lonrad1 = lon1 * DEG2RAD; double latrad2 = lat2 * DEG2RAD; double lonrad2 = lon2 * DEG2RAD; double avg_radius; double k1 = (sin ((latrad1 - latrad2) / 2.0)); double k2 = (sin ((lonrad1 - lonrad2) / 2.0)); double dist; dist = 2.0 * asin (sqrt (k1 * k1 + cos (latrad1) * cos (latrad2) * k2 * k2)); if (dist < 0.0) dist = dist + PI; if (a == b) avg_radius = a; else avg_radius = (2.0 * a + b) / 3.0; dist = dist * avg_radius; return dist; }

/** * Vector a is rotated along each axis by the number of degrees specified by that component in the rotation vector. * This means that if you pass the rotation vector (45, 90, 0), vector a will be rotated 45 degrees along the * i axis, 90 degrees along the j axis, and 0 degrees along the k axis. */ DLLEX Vector *vectorRotate(Vector *a, Vector *rotation) { Vector *rotated = vectorCopy(a); double *i = &(rotated->components[0]), // declare pointers to save typing, and avoid having to reassign each component by hand *j = &(rotated->components[1]), // even if it means more memory references because I'm lazy. *k = &(rotated->components[2]); double iRot = rotation->components[0] * PI / 180.0, // Convert each rotational value to radians jRot = rotation->components[1] * PI / 180.0, kRot = rotation->components[2] * PI / 180.0; double sinI = sin(iRot), // Compute the sine and cosine of each rotation to avoid computing them multiple times cosI = cos(iRot), sinJ = sin(jRot), cosJ = cos(jRot), sinK = sin(kRot), cosK = cos(kRot); // rotate around i axis (i component remains constant) // not hardcore enough to write the rotations as one-liners *j = (*j)*cosI - (*k)*sinI; *k = (*j)*sinI + (*k)*cosI; // rotate around j axis (j component remains constant) *i = (*k)*sinJ + (*i)*cosJ; *k = (*k)*cosJ - (*i)*sinJ; // rotate around k axis (k component remains constant) *i = (*i)*cosK - (*j)*sinK; *j = (*i)*sinK + (*j)*cosK; return rotated; }

void SelfLocator::initSamplesAtGivenPositions(const vector<Pose2D>& poses, const vector< Pose2D >& standardDeviations) { for(int i = 0; i < samples->size(); ++i) { Sample& sample(samples->at(i)); if(poses.size()) { // Select one of the poses from the list: unsigned int index = random((short)poses.size()); Pose2D pose = poses[index]; Pose2D stdDev = standardDeviations[index]; // Create sample: sample.translation.x = static_cast<int>(pose.translation.x); sample.translation.x += sampleTriangularDistribution(static_cast<int>(stdDev.translation.x)); sample.translation.y = static_cast<int>(pose.translation.y); sample.translation.y += sampleTriangularDistribution(static_cast<int>(stdDev.translation.y)); float rotation = normalize(pose.rotation + sampleTriangularDistribution(stdDev.rotation)); sample.angle = rotation; sample.rotation = Vector2<int>(int(cos(rotation) * 1024), int(sin(rotation) * 1024)); } else //No given positions, spread uniformly: { Pose2D pose(bb->theFieldDimensions.randomPoseOnField()); sample.translation = Vector2<int>(int(pose.translation.x), int(pose.translation.y)); sample.angle = pose.rotation; sample.rotation = Vector2<int>(int(cos(pose.rotation) * 1024), int(sin(pose.rotation) * 1024)); } } lastOdometry = bb->theOdometryData; poseCalculator->init(); }

void GICPOptimizer::compute_dr(gsl_vector const* x, gsl_matrix const* gsl_temp_mat_r, gsl_vector *g) { double dR_dPhi[3][3]; double dR_dTheta[3][3]; double dR_dPsi[3][3]; gsl_matrix_view gsl_d_rx = gsl_matrix_view_array(&dR_dPhi[0][0],3, 3); gsl_matrix_view gsl_d_ry = gsl_matrix_view_array(&dR_dTheta[0][0],3, 3); gsl_matrix_view gsl_d_rz = gsl_matrix_view_array(&dR_dPsi[0][0],3, 3); double phi = gsl_vector_get(x ,3); double theta = gsl_vector_get(x ,4); double psi = gsl_vector_get(x ,5); double cphi = cos(phi), sphi = sin(phi); double ctheta = cos(theta), stheta = sin(theta); double cpsi = cos(psi), spsi = sin(psi); dR_dPhi[0][0] = 0.; dR_dPhi[1][0] = 0.; dR_dPhi[2][0] = 0.; dR_dPhi[0][1] = sphi*spsi + cphi*cpsi*stheta; dR_dPhi[1][1] = -cpsi*sphi + cphi*spsi*stheta; dR_dPhi[2][1] = cphi*ctheta; dR_dPhi[0][2] = cphi*spsi - cpsi*sphi*stheta; dR_dPhi[1][2] = -cphi*cpsi - sphi*spsi*stheta; dR_dPhi[2][2] = -ctheta*sphi; dR_dTheta[0][0] = -cpsi*stheta; dR_dTheta[1][0] = -spsi*stheta; dR_dTheta[2][0] = -ctheta; dR_dTheta[0][1] = cpsi*ctheta*sphi; dR_dTheta[1][1] = ctheta*sphi*spsi; dR_dTheta[2][1] = -sphi*stheta; dR_dTheta[0][2] = cphi*cpsi*ctheta; dR_dTheta[1][2] = cphi*ctheta*spsi; dR_dTheta[2][2] = -cphi*stheta; dR_dPsi[0][0] = -ctheta*spsi; dR_dPsi[1][0] = cpsi*ctheta; dR_dPsi[2][0] = 0.; dR_dPsi[0][1] = -cphi*cpsi - sphi*spsi*stheta; dR_dPsi[1][1] = -cphi*spsi + cpsi*sphi*stheta; dR_dPsi[2][1] = 0.; dR_dPsi[0][2] = cpsi*sphi - cphi*spsi*stheta; dR_dPsi[1][2] = sphi*spsi + cphi*cpsi*stheta; dR_dPsi[2][2] = 0.; // set d/d_rx = tr(dR_dPhi'*gsl_temp_mat_r) [= <dR_dPhi, gsl_temp_mat_r>] gsl_vector_set(g, 3, mat_inner_prod(&gsl_d_rx.matrix, gsl_temp_mat_r)); // set d/d_ry = tr(dR_dTheta'*gsl_temp_mat_r) = [<dR_dTheta, gsl_temp_mat_r>] gsl_vector_set(g, 4, mat_inner_prod(&gsl_d_ry.matrix, gsl_temp_mat_r)); // set d/d_rz = tr(dR_dPsi'*gsl_temp_mat_r) = [<dR_dPsi, gsl_temp_mat_r>] gsl_vector_set(g, 5, mat_inner_prod(&gsl_d_rz.matrix, gsl_temp_mat_r)); }

void Camera::mouseDrag(double x, double y) { static bool firstMouse = true; if (firstMouse) { lastX = (float)x; lastY = (float)y; firstMouse = false; } Float xOffset = (float)x - lastX; Float yOffset = (float)y - lastY; lastX = (float)x; lastY = (float)y; Float sensitivity = 0.05f; xOffset *= sensitivity; yOffset *= sensitivity; yaw += xOffset; pitch += yOffset; if (pitch > 89.0f) pitch = 89.0f; if (pitch < -89.0f) pitch = -89.0f; front = glm::normalize(Vector( cos(glm::radians(yaw)) * cos(glm::radians(pitch)), sin(glm::radians(pitch)), sin(glm::radians(yaw)) * cos(glm::radians(pitch)) )); }

/** * transformRGB: applies current transformation to frame * Parameters: * td: private data structure of this filter * Return value: * 0 for failture, 1 for success * Preconditions: * The frame must be in RGB format /// TODO Add zoom! */ int _FLT(transformRGB)(TransformData* td, Transform t) { int x = 0, y = 0, z = 0; unsigned char *D_1, *D_2; D_1 = td->src; D_2 = td->destbuf; float c_s_x = td->fiSrc.width/2.0; float c_s_y = td->fiSrc.height/2.0; float c_d_x = td->fiDest.width/2.0; float c_d_y = td->fiDest.height/2.0; /* for each pixel in the destination image we calc the source * coordinate and make an interpolation: * p_d = c_d + M(p_s - c_s) + t * where p are the points, c the center coordinate, * _s source and _d destination, * t the translation, and M the rotation matrix * p_s = M^{-1}(p_d - c_d - t) + c_s */ /* All 3 channels */ if (fabs(t.alpha) > td->rotationThreshhold) { for (x = 0; x < td->fiDest.width; x++) { for (y = 0; y < td->fiDest.height; y++) { float x_d1 = (x - c_d_x); float y_d1 = (y - c_d_y); float x_s = cos(-t.alpha) * x_d1 + sin(-t.alpha) * y_d1 + c_s_x -t.x; float y_s = -sin(-t.alpha) * x_d1 + cos(-t.alpha) * y_d1 + c_s_y -t.y; for (z = 0; z < 3; z++) { // iterate over colors unsigned char* dest = &D_2[(x + y * td->fiDest.width)*3+z]; _FLT(interpolateN)(dest, x_s, y_s, D_1, td->fiSrc.width, td->fiSrc.height, 3, z, td->crop ? 16 : *dest); } } } } else { /* no rotation, just translation *(also no interpolation, since no size change (so far) */ int round_tx = myround(t.x); int round_ty = myround(t.y); for (x = 0; x < td->fiDest.width; x++) { for (y = 0; y < td->fiDest.height; y++) { for (z = 0; z < 3; z++) { // iterate over colors short p = PIXELN(D_1, x - round_tx, y - round_ty, td->fiSrc.width, td->fiSrc.height, 3, z, -1); if (p == -1) { if (td->crop == 1) D_2[(x + y * td->fiDest.width)*3+z] = 16; } else { D_2[(x + y * td->fiDest.width)*3+z] = (unsigned char)p; } } } } } return 1; }

std::vector<geometry_msgs::Pose> RoadNavigation::getTargets(geometry_msgs::Pose current_pose, nav_msgs::Path target_traj) { double min_distance = 5.; // In meters, along the traj int index = 0; for (; getDistance(target_traj.poses[0].pose, target_traj.poses[index].pose) < min_distance; index++); index--; geometry_msgs::Pose end; end.position.x = target_traj.poses[index].pose.position.x; end.position.y = target_traj.poses[index].pose.position.y; end.position.z = target_traj.poses[index].pose.position.z; double m = atan((current_pose.position.y - end.position.y) / (current_pose.position.x - end.position.x)); double dist_seeds = 10; // Need to get laterally shifted trajectories std::vector<geometry_msgs::Pose> targets; geometry_msgs::Pose poss_target; for (int i = 1; i < 40; i += 1) { poss_target.position.x = end.position.x + i * dist_seeds * sin(m); poss_target.position.y = end.position.y - i * dist_seeds * cos(m); poss_target.position.z = end.position.z; targets.push_back(poss_target); } for (int i = -1; i>-40; i -= 1) { poss_target.position.x = end.position.x + i * dist_seeds * sin(m); poss_target.position.y = end.position.y - i * dist_seeds * cos(m); poss_target.position.z = end.position.z; targets.push_back(poss_target); } return targets; }

cv::Point2f framePic2Field(const int& width, const int& height, cv::Point& pt,const float gama0 ,const float m_Height ,const float alpha0, const float beta0 ) { // ros::Time cur_time=ros::Time::now(); // ros::Time last_time=ros::Time::now(); // alpha0=0.262;//m_VerShiYeAng;摄影机垂直视野角；单位为rad // gama0= 1.047;//m_FuYangAng; 摄影机俯仰角 // beta0=0.35;//m_HorShiYeAng;摄影机水平视野角 // m_Height=32; //摄影机高度 //opencv坐标转换为图像坐标 int xp = pt.x - (width - 1) / 2; int yp = (height - 1) / 2 - pt.y; //图像坐标转换为实际场景坐标 float tan_alpha1 = 2 * yp * tan(alpha0) / height; cv::Point2f pt_; //得到的XPYP即为距离镜头中心的实际坐标距离； pt_.y = m_Height * tan_alpha1 * ((1 + tan(gama0) * tan(gama0)) / (1 - tan(gama0) * tan_alpha1)); float UG = m_Height * (tan(gama0) - tan(gama0 - alpha0)) * cos(gama0 - alpha0) / (cos(gama0 - alpha0) - cos(gama0)); pt_.x = 2 * (UG + pt_.y) * m_Height * xp * tan(beta0) / (UG * cos(gama0) * width); // cur_time=ros::Time::now(); //double dt=(cur_time-last_time).toSec(); //ROS_INFO("framePic2Field() time: %f",dt); return pt_; } //framePic2Field();

// Same as the runKernel, m1 and m2 should be // 1 row x n col x 2 channels. // And also, error has to be of CV_32FC1. void CvOnePointEstimator::computeReprojError( const CvMat* m1, const CvMat* m2, const CvMat* model, CvMat* error ) { cv::Mat X1(m1), X2(m2); int n = X1.cols; X1 = X1.reshape(1, n); X2 = X2.reshape(1, n); X1.convertTo(X1, CV_64F); X2.convertTo(X2, CV_64F); double theta = model->data.db[0]; // Note this E is for image normal camera system, not the system in 1-pt paper cv::Mat E = (cv::Mat_<double>(3, 3) << 0, -cos(theta * 0.5), 0, cos(theta * 0.5), 0, -sin(theta * 0.5), 0, -sin(theta * 0.5), 0); for (int i = 0; i < n; i++) { cv::Mat x1 = (cv::Mat_<double>(3, 1) << X1.at<double>(i, 0), X1.at<double>(i, 1), 1.0); cv::Mat x2 = (cv::Mat_<double>(3, 1) << X2.at<double>(i, 0), X2.at<double>(i, 1), 1.0); double x2tEx1 = x2.dot(E * x1); cv::Mat Ex1 = E * x1; cv::Mat Etx2 = E * x2; double a = Ex1.at<double>(0) * Ex1.at<double>(0); double b = Ex1.at<double>(1) * Ex1.at<double>(1); double c = Etx2.at<double>(0) * Etx2.at<double>(0); double d = Etx2.at<double>(0) * Etx2.at<double>(0); error->data.fl[i] = x2tEx1 * x2tEx1 / (a + b + c + d); } }

// Calculate the coordinates, normal vector and texture coordinates for // a hemisphere point at the given latitude and longitude (in degrees). // The radius of the hemisphere is s, the center point (at the base of the // hemisphere) is ox,oy,oz. static void setHemispherePoint(vec3 *p, vec3* n, vec3* t, int latitude, int longitude, double s, double ox, double oy, double oz) { double dToR = M_PI / 180.0; double len; // Set vertex position p->x = ox + sin(longitude * dToR) * cos(latitude * dToR) * s; p->y = oy + sin(latitude * dToR) * s; p->z = oz + cos(longitude * dToR) * cos(latitude * dToR) * s; // Set texture coordinate t->x = longitude / 360.0; t->y = latitude / 90.0; // calculate normal n->x = p->x - ox; n->y = p->y - oy; n->z = p->z - oz; len = n->x*n->x + n->y*n->y + n->z*n->z; n->x /= len; n->y /= len; n->z /= len; }

/** * Deconvolves a Gaussian "beam" from a Gaussian component. * Routine translated from the AIPSish APL/SUB/DECONV.FOR/DECONV * \param fmaj Convolved major axis * \param fmin Convolved minor axis * \param fpa Convolved position angle of major axis * \param cmaj Beam major axis * \param cmin Beam minor axis * \param cpa Beam position angle of major axis * \param rmaj [out] Actual major axis; = 0 => unable to fit * \param rmin [out] Actual minor axis; = 0 => unable to fit * \param rpa [out] Actual position angle of major axis */ void ObitConvUtilDeconv (ofloat fmaj, ofloat fmin, ofloat fpa, ofloat cmaj, ofloat cmin, ofloat cpa, ofloat *rmaj, ofloat *rmin, ofloat *rpa) { ofloat cmj2, cmn2, fmj2, fmn2, sinc, cosc, rhoc, sigic2, det, rhoa, lfpa, lcpa, konst = 28.647888; olong csux; /* Get useful constants */ csux = (olong) ((fpa+900.0)/180.0); lfpa = (fpa+900.0) - csux*180.0; csux = (olong) ((cpa+900.0)/180.0); lcpa = (cpa+900.0) - csux*180.0; cmj2 = cmaj * cmaj; cmn2 = cmin * cmin; fmj2 = fmaj * fmaj; fmn2 = fmin * fmin; sinc = (lfpa - lcpa) / konst; cosc = cos (sinc); sinc = sin (sinc); /* Trigonometry now */ rhoc = (fmj2 - fmn2) * cosc - (cmj2 - cmn2); if (rhoc == 0.0) { sigic2 = 0.0; rhoa = 0.0; } else { sigic2 = atan((fmj2 - fmn2) * sinc / rhoc); rhoa = ((cmj2 - cmn2) - (fmj2 - fmn2) * cosc) / (2.0 * cos (sigic2)); } (*rpa) = sigic2 * konst + lcpa; det = ((fmj2 + fmn2) -(cmj2 + cmn2)) / 2.0; (*rmaj) = det - rhoa; (*rmin) = det + rhoa; /* Swap to get major > minor */ (*rmaj) = MAX (0.0, *rmaj); (*rmin) = MAX (0.0, *rmin); (*rmaj) = sqrt (fabs (*rmaj)); (*rmin) = sqrt (fabs (*rmin)); if (*rmaj < *rmin) { sinc = (*rmaj); (*rmaj) = (*rmin); (*rmin) = sinc; (*rpa) = (*rpa)+90.0; } /* Fix up PA */ csux = (olong) ((*rpa+900.0)/180.0); *rpa = (*rpa+900.0) - csux*180.0; if (*rmaj == 0.0) { (*rpa) = 0.0; } else if (*rmin == 0.0) { if ((fabs(*rpa-lfpa) > 45.0) && (fabs(*rpa-lfpa) < 135.0)) { csux = (olong) ((*rpa+450.0)/180.0); *rpa = (*rpa+450.0) - csux*180.0; } } } /* end of routine ObitConvUtilDeconv */

void Camera::updateCameraRotation() { double xPos, yPos; glfwGetCursorPos(m_window, &xPos, &yPos); // get current mouse position // if mouse still in window center, no need to recalculate view matrix if (xPos == m_windowWidth / 2 && yPos == m_windowHeight / 2) { return; } centerMouse(); m_horizontalAngle += float(m_windowWidth / 2 - xPos) * m_mouseSpeed; m_verticalAngle += float(m_windowHeight / 2 - yPos) * m_mouseSpeed; glm::vec3 oldLookIn = getLookAt() - getCenter(); glm::vec3 lookInHorizontal = glm::vec3(sin(m_horizontalAngle), oldLookIn.y, cos(m_horizontalAngle)); glm::vec3 lookInVertical = glm::vec3(oldLookIn.x * cos(m_verticalAngle), sin(m_verticalAngle), oldLookIn.z * cos(m_verticalAngle)); glm::vec3 newLookIn = glm::normalize(lookInHorizontal + lookInVertical); setLookAt(getCenter() + newLookIn); float rightHorizontalAngle = m_horizontalAngle - PI / 2.0f; setRight(glm::vec3(sin(rightHorizontalAngle), getRight().y, cos(rightHorizontalAngle))); setUp(glm::normalize(glm::cross(getRight(), newLookIn))); }

unsigned int CContactTarget3D_Mesh::FindInOut_Boxel (double px, double py, double pz) const { assert( pBoxel_ != 0 ); double p[3] = { px, py, pz }; if( pBoxel_->IsOut(p) && !is_hole ){ return 1; } unsigned int icnt_in = 0; unsigned int icnt_out = 0; for(unsigned int i=0;i<10;i++){ const double theta = i*6.28/10+0.15432452356673; const double dir[3] = { sin(theta)*cos(theta*2), sin(theta)*sin(theta*2), cos(theta) }; // const double dir[3] = { -0.35, 0.1342, 0.3 }; unsigned int ires = FindInOut_IntersectionRay_Boxel(px,py,pz, dir); // unsigned int ires1 = FindInOut_IntersectionRay(px,py,pz, dir); /* if( ires != ires1 ){ std::cout << "hoge " << px << " " << py << " " << pz << std::endl; }*/ if( ires != 2 && !is_hole ){ return ires; } if( ires == 0 ) icnt_in++; if( ires == 1 ) icnt_out++; } if( icnt_in > 5 ) return 0; if( icnt_out > 5 ) return 1; return 2; }

// Retourne : une liste de points représentant un arc de cercle // A utiliser dans le dessin des boutons ei_linked_point_t* ei_draw_arc(ei_point_t centre, float R, float angledebut, float anglefin) { ei_linked_point_t* liste = NULL; float angle = angledebut; while (angle < anglefin) { ei_linked_point_t* new_point = malloc(sizeof(ei_linked_point_t)); new_point->point = ei_point( (int)(R*cos(angle)) + centre.x, (int)(R*sin(angle)) + centre.y); new_point->next = NULL; concat_lists(&liste, &new_point); // Pas de rotation = 0.1 angle = angle + 0.1; } ei_linked_point_t* last_point = malloc(sizeof(ei_linked_point_t)); last_point->point = ei_point( (int)(R*cos(anglefin)) + centre.x, (int)(R*sin(anglefin)) + centre.y); last_point->next = NULL; concat_lists(&liste, &last_point); return liste; }

static double psc_harris_init_field(struct psc *psc, double x[3], int m) { struct psc_harris *harris = to_psc_harris(psc); double BB = harris->B0; double LLz = psc->domain.length[2], LLy = psc->domain.length[1]; double LLL = harris->LLL; double AA = harris->AA; switch (m) { case HZ: return BB * tanh((x[1]) / LLL) - AA * M_PI/LLy * cos(2.*M_PI * (x[2] - .5 * LLz) / LLz) * sin(M_PI * x[1] / LLy); case HY: return AA * 2.*M_PI / LLz * sin(2.*M_PI * (x[2] - .5 * LLz) / LLz) * cos(M_PI * x[1] / LLy); case JXI: return BB / LLL * (1./sqr(cosh(x[1] / LLL))) - AA * sqr(M_PI) * (1./sqr(LLy) + 4./sqr(LLz)) * cos(2.*M_PI * (x[2] - .5 *LLz) / LLz) * cos(M_PI * x[1] / LLy); default: return 0.; } }

Bala::Bala(float _v, sf::Sprite& _cannon, sf::Image& imagen) { float cannonA, cannonX, cannonY, cannonH; //Cargo sprite sprite.SetImage(imagen); sprite.SetCenter(imagen.GetWidth()/2, imagen.GetHeight()/2); //Inicializo variables dt = 0.0f; cannonA = _cannon.GetRotation(); cannonX = _cannon.GetPosition().x; cannonY = _cannon.GetPosition().y; //Tomo el tamaño sobre el eje x como el largo del cañon //porque sin rotar la misma se encuentra en horizontal cannonH = _cannon.GetSize().x; //Descompongo la velocidad inicial vx = _v * (cos(cannonA/180*PI)); vy = _v * (sin(cannonA/180*PI)); //Posicion inicial bala según la posición del cañon //usando trigonometria xi = cannonX + ((cos(cannonA/180*PI) * cannonH)); yi = cannonY - ((sin(cannonA/180*PI) * cannonH)); sprite.SetPosition(xi, yi); }

static void gwy_data_field_mark_facets(GwyDataField *dtheta, GwyDataField *dphi, gdouble theta0, gdouble phi0, gdouble tolerance, GwyDataField *mask) { gdouble cr, cth0, sth0, cro; const gdouble *xd, *yd; gdouble *md; gint i; cr = cos(tolerance); cth0 = cos(theta0); sth0 = sin(theta0); xd = gwy_data_field_get_data_const(dtheta); yd = gwy_data_field_get_data_const(dphi); md = gwy_data_field_get_data(mask); for (i = gwy_data_field_get_xres(dtheta)*gwy_data_field_get_yres(dtheta); i; i--, xd++, yd++, md++) { cro = cth0*cos(*xd) + sth0*sin(*xd)*cos(*yd - phi0); *md = (cro >= cr); } }

FGLocation::FGLocation(double lon, double lat, double radius) : mCacheValid(false) { e2 = c = 0.0; a = ec = ec2 = 1.0; epa = 0.0; mLon = mLat = mRadius = 0.0; mGeodLat = GeodeticAltitude = 0.0; mTl2ec.InitMatrix(); mTec2l.InitMatrix(); mTi2ec.InitMatrix(); mTec2i.InitMatrix(); mTi2l.InitMatrix(); mTl2i.InitMatrix(); double sinLat = sin(lat); double cosLat = cos(lat); double sinLon = sin(lon); double cosLon = cos(lon); mECLoc = FGColumnVector3( radius*cosLat*cosLon, radius*cosLat*sinLon, radius*sinLat ); }

void clParticleEmitter_Sphere::EmitParticles( const clPtr<clParticleSystem>& PS, float DeltaTime ) const { FAccumulatedTime += DeltaTime; while ( FAccumulatedTime > 1.0f / FEmissionRate && PS->GetParticles().size() < FMaxParticles ) { FAccumulatedTime -= 1.0f / FEmissionRate; sParticle P; float Theta = Math::RandomInRange( 0.0f, Math::TWOPI ); float Phi = Math::RandomInRange( 0.0f, Math::TWOPI ); float R = FRadius; float SinTheta = sin( Theta ); float x = R * SinTheta * cos( Phi ); float y = R * SinTheta * sin( Phi ); float z = R * cos( Theta ); P.FPosition = FCenter + vec3( x, y, z ); P.FVelocity = vec3( x, y, z ).GetNormalized() * Math::RandomInRange( FRadialVelocityMin, FRadialVelocityMax ); P.FAcceleration = vec3( 0.0f ); P.FTTL = Math::RandomInRange( FLifetimeMin, FLifetimeMax ); P.FLifeTime = P.FTTL; P.FRGBA = Math::RandomVector4InRange( FColorMin, FColorMax ); P.FRGBA.w = 1.0f; P.FSize = Math::RandomInRange( FSizeMin, FSizeMax ); PS->AddParticle( P ); } }

// Public general function int JuliaSet::Iterate( const int p_X, const int p_Y ) { ComplexNumber<double> c; c.x = (sin(cos(m_Zoom / 10) * 10) + cos(m_Zoom * 2.0) / 4.0 + sin(m_Zoom * 3.0) / 6.0) * 0.8; c.y = (cos(sin(m_Zoom / 10) * 10) + sin(m_Zoom * 2.0) / 4.0 + cos(m_Zoom * 3.0) / 6.0) * 0.8; const double scalex = 3.0; const double scaley = 2.0; const double diffx = -1.5f; const double diffy = -1.0f; ComplexNumber<double> z; z.x = ( scalex / ( (double)m_Width / (double)p_X ) + diffx ) / m_Zoom; z.y = ( scaley / ( (double)m_Height / (double)p_Y ) + diffy) / m_Zoom; int n; for( n = 0; n < m_Precision; n++ ) { double newx = ( z.x * z.x - z.y * z.y ) + c.x; double newy = ( z.y * z.x + z.x * z.y ) + c.y; if( ( newx * newx + newy * newy ) > 4.0f ) { break; } z.x = newx; z.y = newy; } return n; }

float_type root_raised_cosine_imp(float_type alpha, float_type xin, float_type rate, long num_taps) //----------------------------------------------------------------------- // Calculates the square root raised cosine pulse shape given the // excess bandwidth value beta and the index. //----------------------------------------------------------------------- { float_type x1,x2,x3; float_type nom,denom; float_type xindx = xin-num_taps/2; x1 = PI*xindx/rate; x2 = 4*alpha*xindx/rate; x3 = x2*x2-1; if (x3!=0) { if (x1!=0) { nom = cos((1+alpha)*x1); nom += sin((1-alpha)*x1)/(4*alpha*xindx/rate); } else { nom = cos((1+alpha)*x1); nom += (1-alpha)*PI/(4*alpha); } denom = x3*PI; } else { if (alpha==1) return(-1); x3 = (1-alpha)*x1; x2 = (1+alpha)*x1; nom = sin(x2)*(1+alpha)*PI - cos(x3)*((1-alpha)*PI*rate)/(4*alpha*xindx) + sin(x3)*rate*rate/(4*alpha*xindx*xindx); denom = -32*PI*alpha*alpha*xindx/rate; } return(4*alpha*nom/denom); }

//Private void UIElement::UpdateModel(){ bUpdateMe = false; glm::mat4 trans(1), rot(1), scale(1); trans[3] = v4Translate; if (m_parent == nullptr){ trans[3].z = App::GetDepth() * -0.5f; } trans[3].z += fOffset; scale[0][0] = v2Scale.x; scale[1][1] = v2Scale.y; rot[0][0] = cos(fRotation);rot[0][1] = -sin(fRotation); rot[1][0] = sin(fRotation);rot[1][1] = cos(fRotation); if (m_parent == nullptr) { m4Model = trans * rot * scale; } else { m4Model = m_parent->m4Model * (trans * rot * scale); } for (auto child : mcuiElements){ if (child.second != nullptr) child.second->UpdateModel(); } }

int main () { int signal[1024]; double x, DFT[1024]; int n,k; for(n=0;n<1024;n++) { x=100.0*cos(n*M_PI/32.0); signal[n]=(int)x; } for(n=0;n<1024;n++){ DFT[n]=0.0; for(k=0;k<1024;k++) { DFT[n]+=signal[k]*cos(2*M_PI*k*n/1024); } } int i; for(i=0;i<1024;i++) { printf("%5d, %4d, %20.16f \n", i, signal[i], DFT[i]); } system("PAUSE"); return 0; }

/* ================= R_SetupFrustum Setup that culling frustum planes for the current view ================= */ void R_SetupFrustum (void) { int i; float xs, xc; float ang; ang = tr.viewParms.fovX / 180 * M_PI * 0.5f; xs = sin( ang ); xc = cos( ang ); VectorScale( tr.viewParms.ori.axis[0], xs, tr.viewParms.frustum[0].normal ); VectorMA( tr.viewParms.frustum[0].normal, xc, tr.viewParms.ori.axis[1], tr.viewParms.frustum[0].normal ); VectorScale( tr.viewParms.ori.axis[0], xs, tr.viewParms.frustum[1].normal ); VectorMA( tr.viewParms.frustum[1].normal, -xc, tr.viewParms.ori.axis[1], tr.viewParms.frustum[1].normal ); ang = tr.viewParms.fovY / 180 * M_PI * 0.5f; xs = sin( ang ); xc = cos( ang ); VectorScale( tr.viewParms.ori.axis[0], xs, tr.viewParms.frustum[2].normal ); VectorMA( tr.viewParms.frustum[2].normal, xc, tr.viewParms.ori.axis[2], tr.viewParms.frustum[2].normal ); VectorScale( tr.viewParms.ori.axis[0], xs, tr.viewParms.frustum[3].normal ); VectorMA( tr.viewParms.frustum[3].normal, -xc, tr.viewParms.ori.axis[2], tr.viewParms.frustum[3].normal ); for (i=0 ; i<4 ; i++) { tr.viewParms.frustum[i].type = PLANE_NON_AXIAL; tr.viewParms.frustum[i].dist = DotProduct (tr.viewParms.ori.origin, tr.viewParms.frustum[i].normal); SetPlaneSignbits( &tr.viewParms.frustum[i] ); } }

void Agent::Wander(float radius, float jitter, float distance, float weight) { auto &m = m_movementInfo; // determine some random angles for circle/jitter float cangle = rand(); float jangle = rand(); // create circle/jitter vectors float xCircle = cos(cangle) * radius; float yCircle = sin(cangle) * radius; float xJitter = cos(jangle) * jitter; float yJitter = sin(jangle) * jitter; //combine float xOffset = xCircle + xJitter; float yOffset = yCircle + yJitter; float offMag = sqrt(xOffset*xOffset + yOffset*yOffset); //normalize to size of circle xOffset *= radius / offMag; yOffset *= radius / offMag; // find normal of vector between, Mult by distance float velMag = sqrtf(m.m_velocityX*m.m_velocityX + m.m_velocityY*m.m_velocityY); float xPosition = (m.m_velocityX / velMag) * distance; float yPosition = (m.m_velocityY / velMag) * distance; // add our offset and circle position to the player's position to get our target float xTarget = xOffset + xPosition + m.m_positionX; float yTarget = yOffset + yPosition + m.m_positionY; Patrol(); }

inline Matrix4<T_ObjType>& ConstructRotationMatrixDegrees( Matrix4<T_ObjType>& outMatrix, float YawDegrees, float PitchDegrees, float RollDegrees ) { memset(outMatrix.m_Matrix, 0, 16 * static_cast<int>( sizeof(T_ObjType) ) ); outMatrix.m_Matrix[0] = 1.0f; outMatrix.m_Matrix[5] = 1.0f; outMatrix.m_Matrix[10] = 1.0f; outMatrix.m_Matrix[15] = 1.0f; if( RollDegrees != 0) { Matrix4f RollMatrix; float RollRadians = RollDegrees * CONVERT_TO_RADIANS; float rcos = cos(RollRadians); float rsin = sin(RollRadians); RollMatrix.m_Matrix[0] = rcos; RollMatrix.m_Matrix[5] = rcos; RollMatrix.m_Matrix[4] = -rsin; RollMatrix.m_Matrix[1] = rsin; outMatrix *= RollMatrix; } if( PitchDegrees != 0) { Matrix4f PitchMatrix; float PitchRadians = PitchDegrees * CONVERT_TO_RADIANS; float pcos = cos(PitchRadians); float psin = sin(PitchRadians); PitchMatrix.m_Matrix[5] = pcos; PitchMatrix.m_Matrix[10] = pcos; PitchMatrix.m_Matrix[9] = -psin; PitchMatrix.m_Matrix[6] = psin; outMatrix *= PitchMatrix; } if( YawDegrees != 0 ) { Matrix4f YawMatrix; float YawRadians = YawDegrees * CONVERT_TO_RADIANS; float ycos = cos(YawRadians); float ysin = sin(YawRadians); YawMatrix.m_Matrix[0] = ycos; YawMatrix.m_Matrix[10] = ycos; YawMatrix.m_Matrix[2] = -ysin; YawMatrix.m_Matrix[8] = ysin; outMatrix *= YawMatrix; } return outMatrix; }