Example #1
0
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;
}
Example #2
0
//! [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);        
}
Example #3
0
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;
 }
Example #4
0
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;
}
Example #5
0
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;
}
Example #6
0
/**
 * 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;
}
Example #7
0
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();
}
Example #8
0
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));      
}
Example #9
0
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))
    ));
}
Example #10
0
/**
 * 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;
}
Example #11
0
    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;
    }
Example #12
0
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); 

    }

}    
Example #14
0
// 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;
}
Example #15
0
/**
 * 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 */ 
Example #16
0
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;
}
Example #18
0
// 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;
}
Example #19
0
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.;
  }
}
Example #20
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);
}
Example #21
0
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);
    }
}
Example #22
0
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 );
	}
}
Example #24
0
	// 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;

	}
Example #25
0
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();
		}
	}
Example #27
0
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;
}
Example #28
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] );
	}
}
Example #29
0
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();
}
Example #30
0
	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;
	}