Example #1
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 #2
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 #3
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 #4
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 #5
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 #6
0
unsigned int	set_plaspix(float x, float y, t_plsm *data)
{
  return (((63.0 + (63.0 * sin(x / 16.0))
	    + 63.0 + (63.0 * sin(y / data->zoom))
	    + 63.0 + (63 * sin((x + y + data->move) / data->modif))
	    + 63.0 + (63 * sin(sqrt(x * x + y * y) / 2))) / 7));
}
Example #7
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 #8
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 #9
0
// main
void main(void) {
	vec2 uv = gl_FragCoord.xy / iResolution.xy;
    uv = uv * 2.0 - 1.0;
    uv.x *= iResolution.x / iResolution.y;    
    float time = iGlobalTime * 0.3;
        
    // ray
    vec3 ang = vec3(sin(time*3.0)*0.1,sin(time)*0.2+0.4,time);
    if(iMouse.z > 0.0) ang = vec3(0.0,clamp(2.0-iMouse.y*0.01,-0.3,PI),iMouse.x*0.01);
	mat4 rot = fromEuler(ang);
    
    vec3 ori = vec3(0.0,0.2,time*1.0);
    ori.y += abs(map_detailed(-ori));
    vec3 dir = normalize(vec3(uv.xy,-1.0));
    dir = rotate(normalize(dir),rot);
    
    // tracing
    vec3 p;
    float dens = hftracing(ori,dir,p);
    vec3 dist = p - ori;
    vec3 n = getNormal(p, dot(dist,dist)*EPSILON_NRM);
             
    // color
    vec3 color = sea_color(p,n,dir,dist);
    vec3 light = normalize(vec3(0.0,1.0,0.8));  
    color += vec3(diffuse(n,light,80.0) * SEA_WATER_COLOR) * 0.12; 
    color += vec3(specular(n,light,dir,60.0));  
    
    // post
    color = mix(sky_color(dir),color, clamp(1.0-length(dist)/100.0,0.0,1.0)); 
    color = pow(color,vec3(0.75));
	gl_FragColor = vec4(color,1.0);
}
Example #10
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)));
}
Example #11
0
/*******************************************************************************
 * Function Name  : ComputeViewMatrix
 * Description    : Calculate the view matrix turning around the balloon
 *******************************************************************************/
void OGLES3Skybox2::ComputeViewMatrix()
{
	PVRTVec3 vFrom;

	/* Calculate the distance to balloon */
	float fDistance = fViewDistance + fViewAmplitude * (float) sin(fViewAmplitudeAngle);
	fDistance = fDistance / 5.0f;
	fViewAmplitudeAngle += 0.004f;

	/* Calculate the vertical position of the camera */
	float updown = fViewUpDownAmplitude * (float) sin(fViewUpDownAngle);
	updown = updown / 5.0f;
	fViewUpDownAngle += 0.005f;

	/* Calculate the angle of the camera around the balloon */
	vFrom.x = fDistance * (float) cos(fViewAngle);
	vFrom.y = updown;
	vFrom.z = fDistance * (float) sin(fViewAngle);
	fViewAngle += 0.003f;

	/* Compute and set the matrix */
	m_mView = PVRTMat4::LookAtRH(vFrom, vTo, vUp);

	/* Remember the camera position to draw the Skybox around it */
	vCameraPosition = vFrom;
}
Example #12
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 #13
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 #14
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 #15
0
inline float solution_biharmonic(float X,float Y,float Z){


  float pi=M_PI;
  float analytic=0;
  float SIGMA2=SIGMA*SIGMA;
  float SIGMA3=SIGMA*SIGMA2;
  float SIGMA4=SIGMA*SIGMA3;
  float f=std::exp( -SIGMA * ( (X-pi)*(X-pi) + (Y-pi)*(Y-pi) + (Z-pi)*(Z-pi) ));
  float Xp=X-pi;
  float Yp=Y-pi;
  float Zp=Z-pi;
  //float fxx=-2*SIGMA*f+4*SIGMA2*Xp*Xp*f;
  //float fyy=-2*SIGMA*f+4*SIGMA2*Yp*Yp*f;
  //float fzz=-2*SIGMA*f+4*SIGMA2*Zp*Zp*f;
  float fxxxx=12*SIGMA2*f-48*SIGMA3*Xp*Xp*f+16*SIGMA4*Xp*Xp*Xp*Xp*f;
  float fyyyy=12*SIGMA2*f-48*SIGMA3*Yp*Yp*f+16*SIGMA4*Yp*Yp*Yp*Yp*f;
  float fzzzz=12*SIGMA2*f-48*SIGMA3*Zp*Zp*f+16*SIGMA4*Zp*Zp*Zp*Zp*f;
  float fxxyy=4*SIGMA2*f-8*SIGMA3*Yp*Yp*f-8*SIGMA3*Xp*Xp*f+16*SIGMA4*Xp*Xp*Yp*Yp*f;
  float fxxzz=4*SIGMA2*f-8*SIGMA3*Zp*Zp*f-8*SIGMA3*Xp*Xp*f+16*SIGMA4*Xp*Xp*Zp*Zp*f;
  float fyyzz=4*SIGMA2*f-8*SIGMA3*Zp*Zp*f-8*SIGMA3*Yp*Yp*f+16*SIGMA4*Yp*Yp*Zp*Zp*f;

  analytic+=C0*9*sin(X)*sin(Y)*sin(Z)+C1*(fxxxx+2*fxxyy+2*fxxzz+fyyyy+2*fyyzz+fzzzz);
  if(analytic!=analytic) analytic=0;
  return analytic;
} // end solution_biharmonic
Example #16
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);
}
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
//--------------------------------------------------------------
void testApp::updateLines() {
    
    // Set the color back to default.
    dancerList[ 0 ].c.set( ofColor( 255, 0, 0 ) );
    dancerList[ 1 ].c.set( ofColor( 0, 255, 0 ) );
    dancerList[ 2 ].c.set( ofColor( 0, 0, 255 ) );
    
    // Change the color upon collision. This isn't working right now and it also makes the app run really slowly, so I'm turning it off.
    /*for ( int i = 0; i < dancerList.size(); i++ ) {
     for ( int j = 0; j < dancerList.size(); j++ ) {
     for ( int k = 0; k < dancerList[ i ].lineList.size(); k++ ) {
     for ( int l = 0; l < dancerList[ j ].lineList.size(); l++ ) {
     if ( dancerList[ i ].lineList[ k ].pos == dancerList[ j ].lineList[ l ].pos ) {
     dancerList[ i ].lineList[ k ].c = ofColor(255);
     dancerList[ j ].lineList[ l ].c = ofColor(255);
     }
     }
     }
     }
     }*/
    
    // When we have real breathing data we won't need this, but for the time being, let's fake it.
    float waveSpeed = 1;
    if ( !noiseBreath ) {
        breath = sin( ofGetElapsedTimef() * waveSpeed ) * breathRad;
    } else {
        breath = ofNoise( sin( ofGetElapsedTimef() * waveSpeed ) ) * breathRad;
    }
    
    // Pass in the data and update the Dancers.
    for ( int i = 0; i < dancerList.size(); i++ ) {
        dancerList[ i ].update( breath );
    }
}
Example #19
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 #20
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 );
}
Example #21
0
void cosft1(float y[], int n)
{
	void realft(float data[], unsigned long n, int isign);
	int j,n2;
	float sum,y1,y2;
	double theta,wi=0.0,wpi,wpr,wr=1.0,wtemp;

	theta=PI/n;
	wtemp=sin(0.5*theta);
	wpr = -2.0*wtemp*wtemp;
	wpi=sin(theta);
	sum=0.5*(y[1]-y[n+1]);
	y[1]=0.5*(y[1]+y[n+1]);
	n2=n+2;
	for (j=2;j<=(n>>1);j++) {
		wr=(wtemp=wr)*wpr-wi*wpi+wr;
		wi=wi*wpr+wtemp*wpi+wi;
		y1=0.5*(y[j]+y[n2-j]);
		y2=(y[j]-y[n2-j]);
		y[j]=y1-wi*y2;
		y[n2-j]=y1+wi*y2;
		sum += wr*y2;
	}
	realft(y,n,1);
	y[n+1]=y[2];
	y[2]=sum;
	for (j=4;j<=n;j+=2) {
		sum += y[j];
		y[j]=sum;
	}
}
Example #22
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 #23
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);
    }
}
	//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();
		}
	}
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 #26
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 #27
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);
}
static void quaternion_from_axis_angle(struct quaternion *q, float ax, float ay, float az, float angle)
{
	q->x = ax * sin(angle/2);
	q->y = ay * sin(angle/2);
	q->z = az * sin(angle/2);
	q->w = cos(angle/2);
}
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
Eigen::Matrix<T,4,1> blend(T t, Eigen::Matrix<T,4,1> p00, Eigen::Matrix<T,4,1> pNorm,
		   Eigen::Matrix<T,4,1> p10){
  T r0 = p00.mag();
  T r1 = p10.mag();
  T rs = linear(t,r0,r1);
  T dotp = dot(p00,p10);
  dotp = abs(dotp);
  T theta = acos(dotp/r0/r1);
  Eigen::Quaternion<T> dq0;
  p00.normalize();
  p10.normalize();
  Eigen::Quaternion<T> q00(p00);
  Eigen::Quaternion<T> q10(p10);	
  if (theta < 0.01) {
    //just give it a little nudge in the right direction
    T o = 0.10;
    Eigen::Quaternion<T> dq(cos(o/2.), sin(o/2.)*pNorm[0], sin(o/2.)*pNorm[1], sin(o/2.)*pNorm[2]);	
    Eigen::Matrix<T,4,1> p01 = dq.rotate(p00);
    Eigen::Quaternion<T> q01(p01);		
    dq0 = Eigen::Quaternion<T>::slerp(q01,q10,t);	
  } 
  else {
    dq0 = Eigen::Quaternion<T>::slerp(q00,q10,t);	
  }	
  T dott = p00.dot(p10);
  T dotq0 = q00.dot(q10);	
  T dotq1 = dq0.dot(q10);
  return Eigen::Matrix<T,4,1>(rs*dq0[1],rs*dq0[2],rs*dq0[3]);	
  //return Eigen::Matrix<T,4,1>(p01);
}