示例#1
0
void COLLI_SetMode( int mode)
{
  int intMode = (int) mode;
  
  if (intMode >= NUMBER_OF_MODES || intMode < 0) {
    fprintf(stderr, "Wrong mode number (%d). Use default mode.\n", intMode);
    ACTUAL_MODE = mode_structure_array[DEFAULT_MODE];
    mode_number = DEFAULT_MODE;
  }
  else {
    ACTUAL_MODE = mode_structure_array[intMode];
    mode_number = intMode;
    if ( mode_number == ARM_OUT_MODE) {
      armState = OUTSIDE;
/*       COLLI_GoBackward(); */
    }
  }

  fprintf(stderr, "\n\nmode: %d\n", intMode);
 
  fprintf (stderr, "tv      : %f  ", (ACTUAL_MODE->target_max_trans_speed));
  fprintf (stderr, "ta      : %f\n", (ACTUAL_MODE->target_trans_acceleration));
  fprintf (stderr, "rv      : %f  ", (RAD_TO_DEG(ACTUAL_MODE->target_max_rot_speed)));
  fprintf (stderr, "ra      : %f\n", (RAD_TO_DEG(ACTUAL_MODE->target_rot_acceleration)));
  fprintf (stderr, "velocity: %f \n", (ACTUAL_MODE->velocity_factor));
  fprintf (stderr, "angle   : %f\n", (ACTUAL_MODE->angle_factor));
  fprintf (stderr, "distance: %f\n", (ACTUAL_MODE->distance_factor));

  /* Set the corresponding values. */
  BASE_TranslateVelocity(             ACTUAL_MODE->target_max_trans_speed);
  BASE_RotateVelocity( RAD_TO_DEG(    ACTUAL_MODE->target_max_rot_speed));
  BASE_TranslateAcceleration(         ACTUAL_MODE->target_trans_acceleration);
  BASE_RotateAcceleration(RAD_TO_DEG( ACTUAL_MODE->target_rot_acceleration));
}
示例#2
0
void moon::display(const vector3 &moon_pos, const vector3 &sun_pos, double max_view_dist) const
{
	vector3 moon_dir = moon_pos.normal();
	double moon_size = max_view_dist/20;
	float moon_azimuth = atan2(-moon_dir.y, moon_dir.x);
	float moon_elevation = asin(moon_dir.z);

	glsl_moon->use();
	glsl_moon->set_gl_texture(*map_diffuse, loc_diffcol, 0);
	glsl_moon->set_gl_texture(*map_normal, loc_nrml, 1);
	//	transform light into object space
	matrix4 roth = matrix4::rot_z(-RAD_TO_DEG(moon_azimuth));
	matrix4 rotv = matrix4::rot_y(-RAD_TO_DEG(moon_elevation));
	matrix4 model_mat = roth*rotv;
	vector3 l = model_mat.inverse() * sun_pos;
	vector3 nl = vector3(-l.y, l.z, -l.x).normal();	//	OpenGL coordinates
	glsl_moon->set_uniform(loc_lightdir, nl);

	//	render moon
	glPushMatrix();
	model_mat.multiply_gl();
	glTranslated(0.95*max_view_dist, 0, 0);

	primitives::textured_quad(vector3f( 0,  moon_size,  moon_size),
				  vector3f( 0, -moon_size,  moon_size),
				  vector3f( 0, -moon_size, -moon_size),
				  vector3f( 0,  moon_size, -moon_size),
				  *map_diffuse).render_plain();
	glPopMatrix();
}
示例#3
0
void COLLI_get_parameters(COLLI_parameter_ptr parameters)
{
  
  if (dumpInfo)
    fprintf( dumpFile, "Received new parameters: VELOCITY_FACTOR : %f   ANGLE_FACTOR: %f\n",
	    parameters->velocity_factor, parameters->angle_factor);
  fprintf(stderr, "Received new parameters: VELOCITY_FACTOR : %f   ANGLE_FACTOR: %f\n",
	  parameters->velocity_factor, parameters->angle_factor);
  
  if (parameters->velocity_factor >= 0.0) 
    ACTUAL_MODE->velocity_factor = parameters->velocity_factor;
  if (parameters->angle_factor >= 0.0) 
    ACTUAL_MODE->angle_factor = parameters->angle_factor;
  if (parameters->distance_factor >= 0.0) 
    ACTUAL_MODE->distance_factor = parameters->angle_factor;
  if (parameters->target_max_trans_speed >= 0.0) {
    ACTUAL_MODE->target_max_trans_speed = parameters->target_max_trans_speed;
    BASE_TranslateVelocity(ACTUAL_MODE->target_max_trans_speed);
  }
  if (parameters->target_max_rot_speed >= 0.0)  {
    ACTUAL_MODE->target_max_rot_speed =  DEG_TO_RAD(parameters->target_max_rot_speed);
    BASE_RotateVelocity(RAD_TO_DEG(ACTUAL_MODE->target_max_rot_speed));
  }
  if (parameters->target_trans_acceleration >= 0.0)  {
    ACTUAL_MODE->target_trans_acceleration = parameters->target_trans_acceleration;
    BASE_TranslateCollisionAcceleration(ACTUAL_MODE->target_trans_acceleration);
  }
  if (parameters->target_rot_acceleration >= 0.0) {
    ACTUAL_MODE->target_rot_acceleration = DEG_TO_RAD(parameters->target_rot_acceleration);
    BASE_RotateCollisionAcceleration(RAD_TO_DEG(ACTUAL_MODE->target_rot_acceleration));
  } 
}
示例#4
0
void quaternion_to_yaw_pitch_roll(Quaternion *q, float *ypr) {
/*
#if 0
    float gx = 2.f * (q->q1*q->q3 - q->q0*q->q2);
    float gy = 2.f * (q->q0*q->q1 + q->q2*q->q3);
    float gz = q->q0*q->q0 - q->q1*q->q1 - q->q2*q->q2 + q->q3*q->q3;
    ypr[0] = atan2f(2.f * q->q1 * q->q2 - 2.f * q->q0 * q->q3, 2.f * q->q0*q->q0 + 2.f * q->q1 * q->q1 - 1.f) * 180.f / PI_F;
    ypr[1] = atanf(gx / sqrtf(gy*gy + gz*gz)) * 180.f / PI_F;
    ypr[2] = atanf(gy / sqrtf(gx*gx + gz*gz)) * 180.f / PI_F;
#else
    // algorythm by http://www.x-io.co.uk/res/doc/quaternions.pdf
    
    float q1q3q0q2 = (2.f*q->q1*q->q3 + 2.f*q->q0*q->q2) * (2.f*q->q1*q->q3 + 2.f*q->q0*q->q2);
    float yaw = atan2f(2.f*(q->q2*q->q3 - q->q0*q->q1), 2.f*q->q0*q->q0 - 1.f + 2.f*q->q3*q->q3);
    float pitch = -atanf(2.f*(q->q1*q->q3 + q->q0*q->q2) / sqrtf(1.f - q1q3q0q2));
    float roll = atan2f(2.f*(q->q1*q->q2 - q->q0*q->q3), 2.f*q->q0*q->q0 - 1.f + 2.f*q->q1*q->q1);
    
    ypr[0] = RAD_TO_DEG(yaw);
    ypr[1] = RAD_TO_DEG(pitch);
    ypr[2] = RAD_TO_DEG(roll);
#endif
*/
		// Inagawa, 2014-04-21 ジャイロセンサと地磁気センサの統合
		float yaw =   atan2f(2.f*q->q1*q->q2 - 2.f*q->q0*q->q3, 2.f*SQ(q->q0) + 2.f*SQ(q->q1) - 1.f);
		float pitch = -asinf(2.f*q->q0*q->q2 + 2.f*q->q1*q->q3);
		float roll =  atan2f(2.f*q->q2*q->q3 - 2.f*q->q0*q->q1, 2.f*SQ(q->q0) + 2.f*SQ(q->q3) - 1.f);
    ypr[0] = RAD_TO_DEG(yaw);
    ypr[1] = RAD_TO_DEG(pitch);
    ypr[2] = RAD_TO_DEG(roll);
}
void vXYZ_To_AzElDist(double dX,    double dY,    double dZ,
                      double *pdAz, double *pdEl, double *pdDist)
    {

    *pdDist = sqrt(dX*dX + dY*dY + dZ*dZ);
    *pdAz   = 90.0 - RAD_TO_DEG(atan2(dY, dX));
    *pdEl   = RAD_TO_DEG(asin(dZ/(*pdDist)));

    return;
    }
示例#6
0
文件: Data.cpp 项目: kzahedi/NMODE
string Data::__toXml(Module *m)
{
  stringstream sst;
  sst << "      <module name=\"" << m->name() << "\">" << endl;
  if(m->isCopy())
  {
    sst << "        <copy      name=\"" << m->ref() << "\"/>" << endl;
    MirrorAxes ma = m->mirrorAxes();
    Quaternion q = m->rotation();
    P3D        r;
    r << q;
    P3D        p = m->translation();
    sst << "        <mirror    x=\"" << XML_BOOL(ma.x)
                     << "\" y=\"" << XML_BOOL(ma.y)
                     << "\" z=\"" << XML_BOOL(ma.z)
                     << "\"/>" << endl;
    sst << "        <rotate    x=\"" << RAD_TO_DEG(r.x)
                     << "\" y=\"" << RAD_TO_DEG(r.y)
                     << "\" z=\"" << RAD_TO_DEG(r.z)
                     << "\"/>" << endl;
    sst << "        <translate x=\"" << p.x
                        << "\" y=\"" << p.y
                        << "\" z=\"" << p.z
                        << "\"/>" << endl;
  }
  else
  {
    for(Nodes::const_iterator n = m->n_begin(); n != m->n_end(); n++)
    {
      sst << "        <node type=\"" << (*n)->type()
        << "\" label=\"" << (*n)->label() << "\"";
      if((*n)->isInactive() == true)
      {
        sst << " inactive=\"true\"";
      }
      sst << ">" << endl;
      sst << "          <position x=\"" << (*n)->position().x
        << "\" y=\"" << (*n)->position().y
        << "\" z=\"" << (*n)->position().z
        << "\"/>" << endl;
      sst << "          <transferfunction name=\"" << (*n)->transferfunction()
        << "\"/>" << endl;
      sst << "          <bias value=\"" << ZERO((*n)->bias()) << "\"/>" << endl;
      sst << "        </node>" << endl;
    }
    for(Edges::const_iterator e = m->e_begin(); e != m->e_end(); e++)
    {
      sst << "        <edge source=\"" << (*e)->sourceNode()->label()
        << "\" destination=\"" << (*e)->destinationNode()->label() << "\" weight=\""
        << (*e)->weight()<< "\"/>" << endl;
    }
  }
  sst << "      </module>" << endl;
  return sst.str();
}
示例#7
0
//=============================================================================
//	ポリゴンの初期化処理
//=============================================================================
void CFireWork::setVtxBuff()
{
    VERTEX_2D* temporaryPointer;

    //	ポリゴンのサイズを変更する前に、対角線の長さと成す角度を再計算する
    diagonalAngle = RAD_TO_DEG(atan2f(size.y, size.x));
    diagonalLength = sqrtf(SQUARE(size.x * 0.5f) + SQUARE(size.y * 0.5f));

    vtxBuff->Lock(0,
                  0,
                  reinterpret_cast<void **>(&temporaryPointer),
                  0);

    temporaryPointer[0].vtx =
        D3DXVECTOR3(
            pos.x - (CRadianTable::myCosf(-diagonalAngle + rot.z)) * diagonalLength,
            pos.y - (CRadianTable::mySinf(-diagonalAngle + rot.z)) * diagonalLength,
            0.0f);
    temporaryPointer[0].rhw = 1.0f;
    temporaryPointer[0].diffuse = color;
    temporaryPointer[0].tex = D3DXVECTOR2(texUV.left, texUV.bottom);



    temporaryPointer[1].vtx =
        D3DXVECTOR3(
            pos.x - (CRadianTable::myCosf(diagonalAngle + rot.z)) * diagonalLength,
            pos.y - (CRadianTable::mySinf(diagonalAngle + rot.z)) * diagonalLength,
            0.0f);
    temporaryPointer[1].rhw = 1.0f;
    temporaryPointer[1].diffuse = color;
    temporaryPointer[1].tex = D3DXVECTOR2(texUV.left, texUV.top);



    temporaryPointer[2].vtx =
        D3DXVECTOR3(
            pos.x + (CRadianTable::myCosf(diagonalAngle + rot.z)) * diagonalLength,
            pos.y + (CRadianTable::mySinf(diagonalAngle + rot.z)) * diagonalLength,
            0.0f);
    temporaryPointer[2].rhw = 1.0f;
    temporaryPointer[2].diffuse = color;
    temporaryPointer[2].tex = D3DXVECTOR2(texUV.right, texUV.bottom);




    temporaryPointer[3].vtx =
        D3DXVECTOR3(
            pos.x + (CRadianTable::myCosf(-diagonalAngle + rot.z)) * diagonalLength,
            pos.y + (CRadianTable::mySinf(-diagonalAngle + rot.z)) * diagonalLength,
            0.0f);
    temporaryPointer[3].rhw = 1.0f;
    temporaryPointer[3].diffuse = color;
    temporaryPointer[3].tex = D3DXVECTOR2(texUV.right, texUV.top);



    vtxBuff->Unlock();
}
void DrivenFeature::Update(float now, FaceAPIData* data, float adaptiveSmooth)
{
	bool dataExists = 
			data != NULL && 
			data->h_confidence > 0 && 
			data->h_data.find(m_fapiIndex) != data->h_data.end();

	float value;

	if(!dataExists)
	{
		value = FadeOut(now);
	}
	else
	{
		value = data->h_data[m_fapiIndex] * (m_reverse ? -1 : 1);
		Neutralise(value, data->h_frameNum, data->h_frameDuration);
		Smooth(value, now, adaptiveSmooth);
		Scale(value);
		value = FadeIn(value, now);
	}

	if(m_isPose)
	{
		m_actor->SetPoseParameter(m_poseIndex, RAD_TO_DEG(value));
	}
	else
	{
		value = clamp(value, 0, 1);
		for(int i = 0; i < m_flexors.size(); i++)
			m_actor->SetFlexWeight(m_flexors[i], value);
	}
}
示例#9
0
void update_object_orientation(void)
{
	static int mx = 0, my = 0, last_mouse_x = 0, last_mouse_y = 0;
	static int arcball = 0;
	
	IF_FAILED(init);
	
	if(!disable_mouse) {
		
		if(!arcball && input_get_mousebtn_state(MOUSE_LEFTBTN)) {
			arcball = 1;
			
			input_get_mouse_position(&mx, &my);
			
			last_mouse_x = mx;
			last_mouse_y = my;
		} else if(arcball && !input_get_mousebtn_state(MOUSE_LEFTBTN)) {
			arcball = 0;
			return;
		}
		
		if(arcball) {
			input_get_mouse_position(&mx, &my);
			
			if(mx < 0)
				mx = 0;
			else if(mx > window_width)
				mx = window_width;
			if(my < 0)
				my = 0;
			else if(my > window_height)
				my = window_height;
			
			if(last_mouse_x != mx || last_mouse_y != my) {
				// получаем вектора вращения виртуальной сферы
				vector3f v1 = compute_sphere_vector(last_mouse_x, last_mouse_y);
				vector3f v2 = compute_sphere_vector(mx, my);
				
				// угол вращения
				rot_angle = RAD_TO_DEG(math_acosf(math_min(1.0f, vec3f_dot(v1, v2))));
				
				matrix3f rotmat3, model3, rotmodel3;
				mat4_submat(rotmat3, 3, 3, rotmat);
				mat4_submat(model3, 3, 3, modelmat);
				
				mat3_mult2(rotmodel3, rotmat3, model3);
				
				// получаем ось вращения (переводим её в систему координат объекта)
				rot_axis = mat3_mult_vec3(rotmodel3, vec3f_norm(vec3f_cross(v1, v2)));
				
				// домножаем матрицу вращения
				mat4_rotate_axis_mult(rotmat, rot_angle, rot_axis);
				
				last_mouse_x = mx;
				last_mouse_y = my;
			}
		}
	}
}
示例#10
0
/////////////
// ExpAtan
void ExpAtan::Evaluate(ExpReturn* er)
{
	p->Evaluate(er);

	switch (er->eType) {
	case EXPTYPE_INTEGER:
		er->eData.fVal = RAD_TO_DEG(atan((double)er->eData.iVal));
		er->eType = EXPTYPE_FLOAT;
		return;
	case EXPTYPE_FLOAT:
		er->eData.fVal = RAD_TO_DEG(atan((double)er->eData.fVal)) ;
		return;
	}

	// Types invalid: return without modifying er (!)
	TYPEMISMATCH1(er, "atan");
}
示例#11
0
void RotatePanorama::rotatePano(PanoramaData& panorama, const Matrix3& transformMat)
{
    for (unsigned int i = 0; i < panorama.getNrOfImages(); i++)
    {
        const SrcPanoImage & image = panorama.getImage(i);
        SrcPanoImage copy = image;
        double y = image.getYaw();
        double p = image.getPitch();
        double r = image.getRoll();
        Matrix3 mat;
        mat.SetRotationPT(DEG_TO_RAD(y), DEG_TO_RAD(p), DEG_TO_RAD(r));
        DEBUG_DEBUG("rotation matrix (PT) for img " << i << " << ypr:" << y << " " << p << " " << r << std::endl << mat);
        Matrix3 rotated;
        rotated = transformMat * mat;
        DEBUG_DEBUG("rotation matrix after transform: " << rotated);
        rotated.GetRotationPT(y,p,r);
        y = RAD_TO_DEG(y);
        p = RAD_TO_DEG(p);
        r = RAD_TO_DEG(r);
        DEBUG_DEBUG("rotated angles of img " << i << ": " << y << " " << p << " " << r);
        
        // Don't update a variable linked to a variable we already updated.
        conditional_set(Yaw, y);
        conditional_set(Pitch, p);
        conditional_set(Roll, r);
        if(image.getX()!=0.0 || image.getY()!=0.0 || image.getZ()!=0.0)
        {
            // rotate translation vector
            Vector3 vecRot=transformMat.Inverse().TransformVector(Vector3(image.getZ(), image.getX(), image.getY()));
            conditional_set(X, vecRot.y);
            conditional_set(Y, vecRot.z);
            conditional_set(Z, vecRot.x);
            // rotate translation plane
            mat.SetRotationPT(DEG_TO_RAD(image.getTranslationPlaneYaw()), DEG_TO_RAD(image.getTranslationPlanePitch()), 0.0);
            rotated = transformMat * mat;
            rotated.GetRotationPT(y,p,r);
            conditional_set(TranslationPlaneYaw, RAD_TO_DEG(y));
            conditional_set(TranslationPlanePitch, RAD_TO_DEG(p));
        };
        panorama.setImage(i, copy);
        panorama.imageChanged(i);
    }
}
示例#12
0
/* This function is called when rotate-away is chosen. It sends the commands
   that are nessessary to turn the robot by the angle calculated by rotateAway
   ModeIsPossible */
void
startRotatingAway(Point rpos, float rrot,float angle)
{
  rotatingAwayFlag = TRUE;
  absoluteAngle = normed_angle(rrot-angle); 
  if (angle >= 0.0)
     rightRot = TRUE;
  else
     rightRot = FALSE;
  BASE_TranslateCollisionAcceleration(ACTUAL_MODE->exception_trans_acceleration);
  BASE_RotateCollisionAcceleration( RAD_TO_DEG( ACTUAL_MODE->exception_rot_acceleration));
  target_flag = TRUE;
  BASE_TranslateVelocity (0.0);
  BASE_RotateVelocity ( RAD_TO_DEG( ACTUAL_MODE->exception_rot_velocity));
  BASE_TranslateCollisionVelocity (0.0);
  BASE_RotateCollisionVelocity ( RAD_TO_DEG( ACTUAL_MODE->exception_rot_velocity));
  
  BASE_Rotate((double)RAD_TO_DEG(angle));
  if (dumpInfo)
     fprintf( dumpFile, " (%f) ", RAD_TO_DEG(angle));
}
示例#13
0
void object::paintTexture(Point& isect, char r, char g, char b) {
	int width = blendTexture->getWidth();
	int height = blendTexture->getHeight();

	float u = RAD_TO_DEG(atan2(isect[2], isect[0]));
	float v = RAD_TO_DEG(atan2(isect[2], isect[1]));

	//std::cout << "u: " << u << " v: " << v << std::endl;

	u = u / 180.0f;
	u = 1.0f - u;
	v = v / 180.0f;

	u = 0.25 + (u * 0.5);

	int drawX = u * width;
	int drawY = v * height;
	for (int xx = drawX - 5; xx < drawX + 5; xx++){
		for (int yy = drawY - 5; yy < drawY + 5; yy++){
			if ((xx > 0) && (xx < width) && (yy > 0) && (yy < height)) {
				blendTexture->setPixel(xx, yy, r, g, b);
			}
		}
	}
	glGenTextures(1, &blendTextureID);
	// Now we begin to edit the texture
	//std::cout << "binding texture" << std::endl;
	glBindTexture(GL_TEXTURE_2D, blendTextureID);
	//  Now map the actual ppm image to the texture
	//std::cout << "store texture in memory " << std::endl;
	glTexImage2D(GL_TEXTURE_2D,
		0,
		GL_RGB,
		blendTexture->getWidth(),
		blendTexture->getHeight(),
		0,
		GL_RGB,
		GL_UNSIGNED_BYTE,
		blendTexture->getPixels());
}
示例#14
0
/* This function sends drive-on commands to the base after keepOnHalting has *
 * received the message that the way is free again */   
void driveOn()
{

  SOUND_talk_text("free again");
  SOUND_play_message(1);

  if (dumpInfo)
    fprintf( dumpFile, "free again.\n");
  fprintf(stderr, "DO : free again.\n");
  BASE_TranslateCollisionAcceleration(ACTUAL_MODE->target_trans_acceleration);
  BASE_RotateCollisionAcceleration(RAD_TO_DEG(ACTUAL_MODE->target_rot_acceleration));
  return;
}
/**
  * @brief  Get roll and pitch from measured acceleration components.
  * @param  ax, ay, az: float components of acceleration vector (in g)
  * @param  pitch, roll: pointers to floats where to store pitch and roll values
  * @retval None
  */
static void Accelerometer_GetRollPitch(float ax, float ay, float az, float *pitch, float *roll)
{
	/* Axes: the positive y-axis points toward the USB port,
			 the positive x-axis points toward the right (PA7 pin),
			 the positive z-axis points toward the bottom of the board.
	
		Pitch angle is the angle between our y-axis and the horizontal.
		Roll angle is the angle between our x-axis and the horizontal.
	*/
	
	*pitch = RAD_TO_DEG(atan2f(ay, sqrtf(ax * ax + az * az)));
			
	/* When az < 0, the board is past the point where the y-axis points downwards --> angle goes past 90 degrees.
	   But atan2 returns angles between -90 and 90, so we need to get the 180 - value. */
	if (az < 0)
		*pitch = ((float)180.0) - *pitch;
	
	if (*pitch < 0)
		*pitch += (float)360.0;	/* Return angles in the range 0 to 360 instead of -90 to 270 */
					
	*roll = RAD_TO_DEG(atan2f(ax, sqrtf(ay * ay + az * az)));
}
void get_angle_vec(int *yy,int from_ind,int to_ind,int col_width, int **angle_vec)
{
  int x_val;

  *angle_vec = allocate_ivector(from_ind,to_ind);

  for (x_val = from_ind; x_val <= to_ind; x_val++) {

    (*angle_vec)[x_val] = 
      (int) round(RAD_TO_DEG( atan ((yy[x_val+col_width] - yy[x_val-col_width] + 0.)/
				    (2*col_width + 1))));
  } 
}
示例#17
0
文件: Car.cpp 项目: JannesMeyer/rac0r
void Car::updateGhosts() {
    static sf::Clock clock = sf::Clock();
    
    sf::Time time = clock.getElapsedTime();
    
    sf::Vector2f lastLocation = this->mLastLocation;
    if (this->mCarGhostDrawables.size() > 0) {
        lastLocation = this->mCarGhostDrawables.back().ghost.getPosition();
    }
    
    // add new ghost
    float ghostDistance = length(this->mCurrentLocation - lastLocation);
    if (ghostDistance >= Car::MAX_GHOSTS_DISTANCE && this->mVelocity > 0.0f) {
        sf::RectangleShape ghost(sf::Vector2f(Car::CAR_WIDTH, Car::CAR_HEIGHT));
        ghost.setOrigin(Car::CAR_WIDTH / 2.0f, Car::CAR_HEIGHT / 2.0f);
        
        sf::Vector2f dir = normalize(this->mCurrentDirection);
        ghost.setPosition(this->mCurrentLocation); // - (dir * (Car::MAX_GHOSTS_DISTANCE + 20.0f)));
        ghost.setRotation(RAD_TO_DEG(heading(dir)));
        
        ghost.setFillColor(this->mCarDrawable.getFillColor());
        
        this->mCarGhostDrawables.push_back(Ghost(ghost, time.asMilliseconds()));
        
        if (this->mCarGhostDrawables.size() >= Car::MAX_GHOSTS) {
            this->mCarGhostDrawables.erase(this->mCarGhostDrawables.begin());
        }
    }
    
    // do we need to remove some ghosts?
    std::list<Ghost>::iterator it = this->mCarGhostDrawables.begin();
    while (it != this->mCarGhostDrawables.end()) {
        if ((time.asMilliseconds() - (*it).age) >= Car::MAX_GHOSTS_AGE) {
            it = this->mCarGhostDrawables.erase(it);
        } else {
            ++it;
        }
    }
       
    // adjust alphas
    int i = this->mCarGhostDrawables.size()+1;
    //float steps = (80.0f - (80.0f * (MAX_VELOCITY / this->mVelocity))) / static_cast<float>(i);
    float steps = 80.0f / static_cast<float>(i);
    sf::Color color = this->mCarDrawable.getFillColor();
    for (std::list<Ghost>::reverse_iterator it = this->mCarGhostDrawables.rbegin(); it != this->mCarGhostDrawables.rend(); ++it) {
        color.a = steps * i--;
        (*it).ghost.setFillColor(color);
    }

}
示例#18
0
Vector3 Vector3::ConvertFromQuat(const Vector4 & q1)
{
    double test = q1.x * q1.y + q1.z * q1.w;

    double sqx = q1.x * q1.x;
    double sqy = q1.y * q1.y;
    double sqz = q1.z * q1.z;

    Vector3 eulerAngles;
    eulerAngles.y = (atan2(2 * q1.y * q1.w - 2 * q1.x * q1.z, 1 - 2 * sqy - 2 * sqz)) * RAD_TO_DEG();
    eulerAngles.z = (asin(2 * test)) * RAD_TO_DEG();
    eulerAngles.x = (atan2(2 * q1.x * q1.w - 2 * q1.y * q1.z, 1 - 2 * sqx - 2 * sqz)) * RAD_TO_DEG();
    return eulerAngles;
}
示例#19
0
/*  This procedure is called when an exception-handling is necessary.      *
 *  It sends a stop-command to the base and the haltingFlag is set to TRUE */
void
startHalting( Point rpos,
	     float rrot)		      
{
   BASE_TranslateCollisionAcceleration(ACTUAL_MODE->exception_trans_acceleration);
   BASE_RotateCollisionAcceleration( RAD_TO_DEG( ACTUAL_MODE->exception_rot_acceleration));
   BASE_TranslateHalt();
   BASE_RotateHalt();
   
   target_flag = TRUE;
   haltingFlag = TRUE;

   return;
}
示例#20
0
static gboolean
gth_image_rotator_motion_notify (GthImageViewerTool *base,
				 GdkEventMotion     *event)
{
	GthImageRotator *self = GTH_IMAGE_ROTATOR (base);

	if (! self->priv->dragging
	    && gtk_drag_check_threshold (GTK_WIDGET (self->priv->viewer),
			    	    	 self->priv->drag_p1.x,
			    	    	 self->priv->drag_p1.y,
			    	    	 self->priv->drag_p2.x,
			    	    	 self->priv->drag_p2.y))
	{
		GdkCursor *cursor;

		self->priv->angle_before_dragging = self->priv->angle;
		self->priv->dragging = TRUE;

		cursor = gdk_cursor_new_from_name (gtk_widget_get_display (GTK_WIDGET (self->priv->viewer)), "grabbing");
		gth_image_viewer_set_cursor (self->priv->viewer, cursor);
		if (cursor != NULL)
			g_object_unref (cursor);
	}

	if (self->priv->dragging) {
		GdkPoint center;
		double   angle1;
		double   angle2;
		double   angle;

		self->priv->drag_p2.x = event->x;
		self->priv->drag_p2.y = event->y;

		center.x = self->priv->center.x * self->priv->preview_zoom + self->priv->preview_image_area.x;
		center.y = self->priv->center.y * self->priv->preview_zoom + self->priv->preview_image_area.y;

		angle1 = get_angle (&center, &self->priv->drag_p1);
		angle2 = get_angle (&center, &self->priv->drag_p2);
		angle = self->priv->angle_before_dragging + (angle2 - angle1);
		if (angle <  - G_PI)
			angle = G_2_PI + angle;
		if (angle >  + G_PI)
			angle = angle - G_2_PI;

		g_signal_emit (self, signals[ANGLE_CHANGED], 0, CLAMP (RAD_TO_DEG (angle), -180.0, 180));
	}

	return FALSE;
}
示例#21
0
文件: Car.cpp 项目: JannesMeyer/rac0r
void Car::update(const sf::Time & _time) {
    
    // keep location as last valid
    if (!this->mDriftedOffTrack) {
        this->mCurrentPassedDistance += length(this->mCurrentLocation - this->mLastLocation);
        this->mLastLocation = this->mCurrentLocation;
        
       // std::cout << "Passed Distance: " << this->mCurrentPassedDistance << std::endl;
    }
    
    // compute cars offset to the track segment and adjust it
    sf::Vector2f proj = project(this->mCurrentLocation, this->mTrack[this->mSegmentStart], this->mTrack[this->mSegmentEnd]);
    sf::Vector2f positionAdjust = proj - this->mCurrentLocation;
   
    // apply drag friction
    if (this->mVelocity > 0.0f) {
       // this->mForce -= abs(Car::FRICTION_FORCE * (this->mVelocity) * (this->mDriftedOffTrack ? Car::DRAG_FRICTION_OFF_TRACK : Car::DRAG_FRICTION_ON_TRACK));
        this->mForce -= abs(Car::FRICTION_FORCE * (this->mVelocity));
        this->mForce -= abs(this->mIsAccelerating ? 0.0f : Car::BREAK_FRICTION * this->mVelocity);
        this->mForce -= abs(!this->mDriftedOffTrack ? 0.0f : Car::DRAG_FRICTION_OFF_TRACK * this->mVelocity);
        
        this->mIsAccelerating = false;
    }
    
    // compute acceleration A = F / M
    float acceleration = this->mForce / Car::DEFAULT_MASS;
    
    // compute velocity
    this->mVelocity += acceleration * _time.asSeconds();
    this->mVelocity = fmax(fmin(this->mVelocity, Car::MAX_VELOCITY), 0.0f);
    //std::cout << "Velocity: " << this->mVelocity << std::endl;
        
    // compute new position
    this->mCurrentLocation += this->mCurrentDirection * (this->mVelocity * _time.asSeconds());
    this->mCurrentLocation += positionAdjust;
    
    this->keepOnTrack();
    this->updateGhosts();
    
    // compute rotation
    float angle = heading(this->mCurrentDirection);
    
    this->mCarDrawable.setRotation(RAD_TO_DEG(angle));
    this->mCarDrawable.setPosition(this->mCurrentLocation);
    
    // debug stuff
    this->mLocationPoint.setPosition(this->mCurrentLocation);
    this->mDirectionShape.setPosition(this->mCurrentLocation);
}
示例#22
0
// Calculates a new position based on the current heading and distance
// traveled from the previous position
static void calc_position(float* new_lat, float* new_long, float ref_lat, float ref_long, float distance, float heading) {
  float lat_rad = DEG_TO_RAD(ref_lat);
  float long_rad = DEG_TO_RAD(ref_long);
  float heading_rad = DEG_TO_RAD(heading);

  float est_lat = asin( sin(lat_rad) *
    cos(distance / EARTH_RADIUS_M) +
    cos(lat_rad) *
    sin(distance / EARTH_RADIUS_M) *
    cos(heading_rad) );

  float est_long = long_rad +
    atan2( sin(heading_rad) *
    sin(distance / EARTH_RADIUS_M) *
    cos(lat_rad),
    cos(distance / EARTH_RADIUS_M) -
    sin(lat_rad) *
    sin(est_lat) );

  *new_lat = RAD_TO_DEG(est_lat);
  *new_long = RAD_TO_DEG(est_long);

  return;
}
示例#23
0
void Vector4::ToAxisAngle(Vector3 & axis, float & angle)
{
    float scale = (float)sqrt(x * x + y * y + z * z);
    if (scale != 0)
    {
        axis.x = x / scale;
        axis.y = y / scale;
        axis.z = z / scale;
    }
    else
    {
        axis = Vector3::Zero();
    }

    angle = ((float)acos(w) * 2.0f) * RAD_TO_DEG();
}
示例#24
0
// --[ Method ]---------------------------------------------------------------
//
//  - Class     : CVector3
//  - Prototype : float Angle(const CVector3& vector)
//
//  - Purpose   : Returns the angle between two vectors.
//
// ---------------------------------------------------------------------------
float CVector3::Angle(const CVector3& vector) const
{
	float fDotProduct = m_fX * vector.m_fX + m_fY * vector.m_fY + m_fZ * vector.m_fZ;
	float fArg        = fDotProduct / (Length() * vector.Length());

#ifdef _DEBUG

	if(fArg < -1.0f || fArg > 1.0f)
	{
		CLogger::ErrorWindow("acosf arg %f (%f, %f, %f)*(%f, %f, %f)", fArg, m_fX, m_fY, m_fZ, vector.m_fX, vector.m_fY, vector.m_fZ);
	}

#endif

	return RAD_TO_DEG(acosf(fArg));
}
示例#25
0
// return the currently used FOV ----------------------------------------------
//
int VID_GetFOV()
{
	if ( AUX_ENABLE_FOV_CONTROL && vid_set_fov != -1 ) {

		return vid_set_fov;

	} else {
		double hypotenuse = hypot( Screen_XOfs, D_Value ); 

		if ( hypotenuse < 1e-7 ) 
			return -1;

		double fov_rad = asin( ( (double)Screen_XOfs ) / hypotenuse ) * 2;
		//double fov = fov_rad * 360.0 / ( 2 * 3.14159265359 );

		return (int) RAD_TO_DEG( fov_rad );
	}
}
示例#26
0
/* Move and rotate the enemy according to its state */
void Enemy::update(vec3 destination) {
   vec3 moveVec = normalize(destination - tPosition);

   if (state == LIVE) {
      if (moveVec.z == 0)
         moveVec.z = 0.001;
      tRotYaw = RAD_TO_DEG(atan(moveVec.x / moveVec.z));
      if (type == BIG && sin(age) < 0) {
         tPosition.y = tScale.y / 2.0;
      }
      else {
         tPosition.x += moveVec.x * moveSpeed;
         tPosition.y = tScale.y / 2.0 + abs(sin(age)) * jumpHeight;
         tPosition.z += moveVec.z * moveSpeed;
      }
      age += jumpSpeed;
   }
   else if (state == SPAWN) {
      tScale *= 1.05;
      tPosition.y = size / 2.0;
      tRotPitch = tRotRoll = tRotYaw = age;
      age += 20;

      if (tScale.x >= size) {
         tScale = vec3(size);
         tRotPitch = tRotRoll = tRotYaw = 0;
         state = LIVE;
         age = 0;
      }
   }
   else if (state == DIE) {
      tScale *= 0.95;
      tRotPitch = tRotRoll = tRotYaw = age;
      age += 20;

      if (tScale.x <= 0.01)
         state = DEAD;
   }
}
示例#27
0
		void LightningBolt::draw(sf::RenderTexture* _Render)
		{
			const float ImageThickness = 8.0f;
			float fThicknessScale = m_fThickness / ImageThickness;

			m_Sprite->setColor(m_Color);

			for( int i(0); i < m_iPointNumber; i++ )
			{
				sf::Vector2f Vector(m_PointArray[i + 1] - m_PointArray[i]);
				float fNorm = math::Calc_Norm(Vector);
				float fRotation = RAD_TO_DEG(atan2(Vector.y, Vector.x));
				sf::Vector2f Position(0.0f, m_Sprite->getTextureRect().width * 0.5f);
				sf::Vector2f Scale(fNorm, fThicknessScale);

				m_Sprite->setRotation(fRotation);
				m_Sprite->setScale(Scale);
				m_Sprite->setPosition(m_PointArray[i]);
				_Render->draw(*m_Sprite);

				m_iDrawNumber++;
			}
		}
示例#28
0
void Shotgun::fire(const sf::Vector2f&pos, const sf::Vector2f&dirVec, float angle, float power, b2World*world, ObjectDetroyer*destroyer, std::list<GameObject*>*mainGameObjectVec)
{

	if(m_Burst > 0){
		m_fireSound.play();
		float sprade = RAD_TO_DEG(angle) - 15.f;
		for (int i = 0; i < 5; i++)
		{
			mainGameObjectVec->push_back(new Bullet(world,destroyer,mainGameObjectVec,200.f));


			mainGameObjectVec->back()->getBody()->SetTransform(b2Vec2(pos.x,pos.y),DEG_TO_RAD(sprade));

			mainGameObjectVec->back()->getBody()->SetLinearVelocity(mainGameObjectVec->back()->getBody()->GetWorldVector(b2Vec2(20.f,0.f)));

			sprade += 6.f;
		}

		--m_Burst;
		m_loaded = false;
	}

}
示例#29
0
//===========================================
// Box2dPhysics::update
//===========================================
void Box2dPhysics::update() {
   if (!m_opts.dynamic) return;

   Vec2f pos(m_body->GetPosition()(0) * m_worldUnitsPerMetre,
      m_body->GetPosition()(1) * m_worldUnitsPerMetre);

   float32_t a = RAD_TO_DEG(m_body->GetAngle());

   if (pos != m_entity->getTranslation_abs() || a != m_entity->getRotation_abs()) {
      Range oldBounds = m_entity->getBoundary();
      Vec2f oldTransl = m_entity->getTranslation();
      Vec2f oldTransl_abs = m_entity->getTranslation_abs();
      float32_t oldRot = m_entity->getRotation();
      float32_t oldRot_abs = m_entity->getRotation_abs();

      m_entity->setSilent(true);
      m_entity->setTranslation_abs(pos);
      m_entity->setRotation_abs(a);
      m_entity->setSilent(false);

      EEvent* event1 = new EEntityBoundingBox(m_entity->getSharedPtr(), oldBounds, m_entity->getBoundary());
      EEvent* event2 = new EEntityTranslation(m_entity->getSharedPtr(), oldTransl, oldTransl_abs, m_entity->getTranslation(), m_entity->getTranslation_abs());
      EEvent* event3 = new EEntityRotation(m_entity->getSharedPtr(), oldRot, oldRot_abs, m_entity->getRotation(), m_entity->getRotation_abs());

      m_ignore.insert(event1->getId());
      m_ignore.insert(event2->getId());
      m_ignore.insert(event3->getId());

      m_entity->onEvent(event1);
      m_entity->onEvent(event2);
      m_entity->onEvent(event3);

      m_eventManager.queueEvent(event1);
      m_eventManager.queueEvent(event2);
      m_eventManager.queueEvent(event3);
   }
}
示例#30
0
// Calculates the true bearing between two gps coordinates in degrees
// "I'd have to change my heading to this value to point to that coordinate"
static float calc_true_bearing(float start_lat, float start_long, float dest_lat, float dest_long) {
  float start_lat_rad = DEG_TO_RAD(start_lat);
  float start_long_rad = DEG_TO_RAD(start_long);
  float dest_lat_rad = DEG_TO_RAD(dest_lat);
  float dest_long_rad = DEG_TO_RAD(dest_long);

  float y = sin(dest_long_rad - start_long_rad) *
    cos(dest_lat_rad);
  float x = cos(start_lat_rad) *
    sin(dest_lat_rad) -
    sin(start_lat_rad) *
    cos(dest_lat_rad) *
    cos(dest_long_rad - start_long_rad);

  float bearing_rad = atan2(y, x);
  float bearing_deg = RAD_TO_DEG(bearing_rad);

  // Shift the values from the range [-180,180] to [0,360)
  if (bearing_deg < 0.0) {
    return bearing_deg + 360.0;
  }

  return bearing_deg;
}