/// accel_to_lean_angles - horizontal desired acceleration to lean angles
///    converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
{
    float accel_right, accel_forward;
    float lean_angle_max = _attitude_control.lean_angle_max();

    // reset accel to current desired acceleration
    if (_flags.reset_accel_to_lean_xy) {
        _accel_target_jerk_limited.x = _accel_target.x;
        _accel_target_jerk_limited.y = _accel_target.y;
        _accel_target_filter.reset(Vector2f(_accel_target.x, _accel_target.y));
        _flags.reset_accel_to_lean_xy = false;
    }

    // apply jerk limit of 17 m/s^3 - equates to a worst case of about 100 deg/sec/sec
    float max_delta_accel = dt * POSCONTROL_JERK_LIMIT_CMSSS;

    Vector2f accel_in(_accel_target.x, _accel_target.y);
    Vector2f accel_change = accel_in-_accel_target_jerk_limited;
    float accel_change_length = accel_change.length();

    if(accel_change_length > max_delta_accel) {
        accel_change *= max_delta_accel/accel_change_length;
    }
    _accel_target_jerk_limited += accel_change;

    // lowpass filter on NE accel
    _accel_target_filter.set_cutoff_frequency(min(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler));
    Vector2f accel_target_filtered = _accel_target_filter.apply(_accel_target_jerk_limited, dt);

    // rotate accelerations into body forward-right frame
    accel_forward = accel_target_filtered.x*_ahrs.cos_yaw() + accel_target_filtered.y*_ahrs.sin_yaw();
    accel_right = -accel_target_filtered.x*_ahrs.sin_yaw() + accel_target_filtered.y*_ahrs.cos_yaw();

    // update angle targets that will be passed to stabilize controller
    _pitch_target = constrain_float(atanf(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI_F),-lean_angle_max, lean_angle_max);
    float cos_pitch_target = cosf(_pitch_target*M_PI_F/18000);
    _roll_target = constrain_float(atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI_F), -lean_angle_max, lean_angle_max);
}
Example #2
0
void Stars::render(float fovY, float aspect, float nearZ, float alpha) {
    // determine length of screen diagonal from quadrant height and aspect ratio
    float quadrantHeight = nearZ * tanf(RADIANS_PER_DEGREE * fovY * 0.5f);
    float halfDiagonal = sqrt(quadrantHeight * quadrantHeight * (1.0f + aspect * aspect));

    // determine fov angle in respect to the diagonal
    float fovDiagonal = atanf(halfDiagonal / nearZ) * 2.0f;

    // pull the modelview matrix off the GL stack
    glm::mat4 view;
    glGetFloatv(GL_MODELVIEW_MATRIX, glm::value_ptr(view));

    _controller->render(fovDiagonal, aspect, glm::affineInverse(view), alpha);
}
Example #3
0
//----------------------------------------------------------------------
//	放物線(出力、開始点、目標地点、初速、かかる負荷(重さ))
//----------------------------------------------------------------------
	bool Parabola(Vector3& out, Vector3 start, Vector3 end, float speed, float mass)
	{
		float angleTan;
		float elevation;	//仰角
		float height = start.y - end.y;	//高低差
		float tx = end.x - start.x;	// X座標差
		float tz = end.z - start.z;	// Z座標差

		float txz = sqrtf(tx*tx + tz*tz);

		// y = v0 * sinθ*t + 0.5f*G*t*t
		// x = v0 * cosθ*t
		// 2次方程式 a*T*T + b*T + c = 0
		float A = (mass*txz*txz) / (2.0f*speed*speed);

		// Tの係数(aT*T + b*T + c = 0)	
		float a, b, c;
		a = 1.0f;
		if (A != 0){
			b = txz / A;
			c = 1.0f + height / A;
		}
		// 解の公式(平方根)
		float D = b * b - 4 * a * c;
		if (D < 0)
		{
			out = Vector3(0, 0, 0);
			angleTan = 0;
			return false;
		}

		// 解の公式
		elevation = (-b + sqrtf(D)) / (2.0f*a);

		// 仰角
		angleTan = atanf(elevation);

		// 移動距離
		D3DXVECTOR3 tt;
		tt.x = end.x - start.x;
		tt.y = 0;
		tt.z = end.z - start.z;
		D3DXVec3Normalize(&tt, &tt);

		out.x = speed * cosf(angleTan)*tt.x;
		out.y = speed * sinf(angleTan);
		out.z = speed * cosf(angleTan)*tt.z;

		return true;
	}
Example #4
0
void	setup_projection_matrix(int w, int h, float horizontal_fov_degrees, float nearz, float farz)
// Set up viewport & projection matrix.
// w and h are the window dimensions in pixels.
// horizontal_fov is in degrees.
{
	glViewport(0, 0, w, h);

	//
	// Set up projection matrix.
	//

	float	aspect_ratio = float(h) / float(w);
	float	horizontal_fov = (float) (horizontal_fov_degrees * M_PI / 180.f);
	float	vertical_fov = atanf(tanf(horizontal_fov / 2.f) * aspect_ratio) * 2;
	
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();

	float	m[16];
	int	i;
	for (i = 0; i < 16; i++) m[i] = 0;
	m[0] = -1.0f / tanf(vertical_fov / 2.0f);
	m[5] = -m[0] / aspect_ratio;
	m[10] = (farz + nearz) / (farz - nearz);
	m[11] = 1;
	m[14] = - 2 * farz * nearz / (farz - nearz);
	
	glMultMatrixf(m);

	glMatrixMode(GL_MODELVIEW);

//#define TEST_FRUSTUM 1	// define to shrink the frustum, to see culling at work...
#if TEST_FRUSTUM
	// Compute values for the frustum planes.  Normals point inwards towards the visible volume.
	frustum_plane[0].set(0, 0, 1, nearz * 10);	// near.
	frustum_plane[1].set(0, 0, -1, -farz * 0.1);	// far.
	frustum_plane[2].set(-cosf(horizontal_fov/2), 0, sinf(horizontal_fov/4), 0);	// left.
	frustum_plane[3].set(cosf(horizontal_fov/2), 0, sinf(horizontal_fov/4), 0);	// right.
	frustum_plane[4].set(0, -cosf(vertical_fov/2), sinf(vertical_fov/4), 0);	// top.
	frustum_plane[5].set(0, cosf(vertical_fov/2), sinf(vertical_fov/4), 0);	// bottom.
#else
	// Compute values for the frustum planes.  Normals point inwards towards the visible volume.
	frustum_plane[0].set(0, 0, 1, nearz);	// near.
	frustum_plane[1].set(0, 0, -1, -farz);	// far.
	frustum_plane[2].set(-cosf(horizontal_fov/2), 0, sinf(horizontal_fov/2), 0);	// left.
	frustum_plane[3].set(cosf(horizontal_fov/2), 0, sinf(horizontal_fov/2), 0);	// right.
	frustum_plane[4].set(0, -cosf(vertical_fov/2), sinf(vertical_fov/2), 0);	// top.
	frustum_plane[5].set(0, cosf(vertical_fov/2), sinf(vertical_fov/2), 0);	// bottom.
#endif
}
Example #5
0
static int GM_CDECL gmfATan(gmThread * a_thread)
{
  GM_CHECK_NUM_PARAMS(1);

  float floatValue;

  if(a_thread->ParamType(0) == GM_INT) { floatValue = (float) a_thread->Param(0).m_value.m_int; }
  else if(a_thread->ParamType(0) == GM_FLOAT) { floatValue = a_thread->Param(0).m_value.m_float; }
  else { return GM_EXCEPTION; }

  a_thread->PushFloat(atanf(floatValue));

  return GM_OK;
}
Example #6
0
bool InverseKinematics::calcLegJoints(const Pose3D& position, Joints jointAngles, float joint0, bool left, const RobotDimensions& robotDimensions) {
  Pose3D target(position);
  Joint firstJoint(left ? LHipYawPitch : RHipYawPitch);
  const int sign(left ? -1 : 1);
  target.translation.y += robotDimensions.values_[RobotDimensions::lengthBetweenLegs] / 2 * sign; // translate to origin of leg
  target = Pose3D().rotateZ(joint0 * -sign).rotateX(M_PI_4 * sign).conc(target); // compute residual transformation with fixed joint0

  // use cosine theorem and arctan to compute first three joints
  float length = target.translation.abs();
  float sqrLength = length * length;
  float upperLeg = robotDimensions.values_[RobotDimensions::upperLegLength];
  float sqrUpperLeg = upperLeg * upperLeg;
  float lowerLeg = robotDimensions.values_[RobotDimensions::lowerLegLength];
  float sqrLowerLeg = lowerLeg * lowerLeg;
  float cosUpperLeg = (sqrUpperLeg + sqrLength - sqrLowerLeg) / (2 * upperLeg * length);
  float cosKnee = (sqrUpperLeg + sqrLowerLeg - sqrLength) / (2 * upperLeg * lowerLeg);
  // clip for the case that target position is not reachable
  const Range<float> clipping(-1.0f, 1.0f);
  bool reachable = true;
  if(!clipping.isInside(cosKnee) || clipping.isInside(upperLeg))
  {
    cosKnee = clipping.limit(cosKnee);
    cosUpperLeg = clipping.limit(cosUpperLeg);
    reachable = false;
  }
  float joint1 = target.translation.z == 0.0f ? 0.0f : atanf(target.translation.y / -target.translation.z) * sign;
  float joint2 = -acos(cosUpperLeg);
  joint2 -= atan2(target.translation.x, Vector2<float>(target.translation.y, target.translation.z).abs() * -sgn(target.translation.z));
  float joint3 = M_PI - acos(cosKnee);
  RotationMatrix beforeFoot = RotationMatrix().rotateX(joint1 * sign).rotateY(joint2 + joint3);
  joint1 -= M_PI_4; // because of the strange hip of Nao

  // compute joints from rotation matrix using theorem of euler angles
  // see http://www.geometrictools.com/Documentation/EulerAngles.pdf
  // this is possible because of the known order of joints (y, x, z) where z is left open and is seen as failure
  RotationMatrix foot = beforeFoot.invert() * target.rotation;
  float joint5 = asin(-foot[2].y) * -sign * -1;
  float joint4 = -atan2(foot[2].x, foot[2].z) * -1;
  //float failure = atan2(foot[0].y, foot[1].y) * sign;

  // set computed joints in jointAngles
  jointAngles[firstJoint + 0] = joint0;
  jointAngles[firstJoint + 1] = joint1;
  jointAngles[firstJoint + 2] = joint2;
  jointAngles[firstJoint + 3] = joint3;
  jointAngles[firstJoint + 4] = joint4;
  jointAngles[firstJoint + 5] = joint5;

  return reachable;
}
Example #7
0
void pendulum_menu() {
    cleardevice();
    outtext((char*)"Click and drag mouse to define anchor point and starting point for pendulum");
    do {
        mouseInput(x1, y1, x2, y2);
        if (y2 < y1)
            outtextxy(0, 20, (char*)"Please choose an anchor point higher than the starting point of pendulum");
    }while(y2 < y1);
    int l = (int) (sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2)) + 0.5);
    Point2D a(x1, y1);
    float ang = atanf(((float)(x2-x1))/(y2-y1));
    cleardevice();
    pendulum(ang, l, a, 25);
}
Example #8
0
const Entity EntityBuilder::CreateHealingLight(const XMFLOAT3 & pos, const XMFLOAT3 & rot, const DirectX::XMFLOAT3 & color, float intensity, float outerAngle, float innerAngle, float range)
{
	Entity ent = _entity.Create();
	_transform->CreateTransform(ent);
	_light->BindSpotLight(ent, color, intensity, outerAngle, innerAngle, range);
	_light->SetAsVolumetric(ent, true);
	_transform->SetPosition(ent, pos);
	_transform->SetRotation(ent, rot);

	float s = range*atanf(outerAngle);
	_transform->SetScale(ent, XMFLOAT3(s, s, range));

	return ent;
}
inline
void CompressedRectilinearPortraitProjector::mapBackward(float u, float v, float &x, float &y)
{
	u /= - scale;
    v /= scale;

	float aatg = a * atanf(u / a);
	float u_ = aatg;
	float v_ = atanf(v * cosf( aatg ) / b);

    float cosv = cosf(v_);
    float y_ = cosv * sinf(u_);
    float x_ = sinf(v_);
    float z_ = cosv * cosf(u_);

    float z;
    x = k_rinv[0] * x_ + k_rinv[1] * y_ + k_rinv[2] * z_;
    y = k_rinv[3] * x_ + k_rinv[4] * y_ + k_rinv[5] * z_;
    z = k_rinv[6] * x_ + k_rinv[7] * y_ + k_rinv[8] * z_;

    if (z > 0) { x /= z; y /= z; }
    else x = y = -1;
}
Example #10
0
float VectorGetYaw(vector_t *arg){

  float yaw;
  /* figure out what quadrant we're in, react accordingly */
  if(arg->z > 0){
    if(arg->x > 0){ /* quadrant 1 */
	  yaw = atanf(arg->z/arg->x) + M_PI;
	}
    else{ /* quadrant 2 */
	  yaw = (atanf(arg->z/arg->x) + M_PI_2) + M_PI;
	}
  }
  else{
    if(arg->x < 0){ /* quadrant 3 */
	  yaw = (atanf(arg->z/arg->x) + M_PI) + M_PI;
	}
    else{ /* quadrant 4 */
	  yaw = (M_2PI - atanf(arg->z/arg->x)) + M_PI;
	}
  }

  return yaw;
}
Example #11
0
float3	EffectTranslucency::ComputePhase( const float3& _Anisotropy, int _PixelDistance, int _SamplesCount, float _TexelSize, float _SliceThickness )
{
	// Imagine receiving light from a point above you offset by a distance d
	//
	//                    0      Source
	// -------------------*--------*-------------
	//                    |      /
	//                    |    /
	//                    |  /
	//                    |/
	// -------------------*----------------------
	//                 Receiver
	//
	// The source and receiver make an angle with the vertical, diffusion direction.
	// This is that angle we compute the phase for.
	//
	float		Theta = atanf( _PixelDistance * _TexelSize / _SliceThickness );
	float		CosTheta = cosf( Theta );

	float3	P;
	P.x = (1.0f - _Anisotropy.x*_Anisotropy.x) * powf( 1.0f + _Anisotropy.x*_Anisotropy.x - 2.0f*_Anisotropy.x*CosTheta, -1.5f );
	P.y = (1.0f - _Anisotropy.y*_Anisotropy.y) * powf( 1.0f + _Anisotropy.y*_Anisotropy.y - 2.0f*_Anisotropy.y*CosTheta, -1.5f );
	P.z = (1.0f - _Anisotropy.z*_Anisotropy.z) * powf( 1.0f + _Anisotropy.z*_Anisotropy.z - 2.0f*_Anisotropy.z*CosTheta, -1.5f );
	P = INV4PI * P;	// Normalize over hemisphere

	// Now, compute the weight to give this phase function
	// The weight is simply the solid angle covered by all the texels using the phase function
	// This solid angle is a spherical band between current and next pixel angles, divided by the amount of samples taken in that band
	//
	float		NextTheta = atanf( (1+_PixelDistance) * _TexelSize / _SliceThickness );
	float		CosNextTheta = cosf( NextTheta );

	float		SolidAngle = TWOPI * (CosTheta - CosNextTheta);	// Solid angle covered by the spherical band in [Theta,NextTheta]
	SolidAngle /= _SamplesCount;								// Solid angle of a single sample in that band

	return SolidAngle * P;
}
Example #12
0
/**
 * lw6sys_math_angle_360
 *
 * @sys_context: global system context
 * @x: x coordinate
 * @y: y coordinate
 *
 * This is a wrapper over the standard @atan function which will
 * handle internally the special x == 0 case and the various positive/negative
 * values of @x and @y.
 *
 * Return value: the angle, in degrees
 */
float
lw6sys_math_angle_360 (lw6sys_context_t * sys_context, int x, int y)
{
  float ret = 0.0f;

  if (x == 0)
    {
      ret = (y == 0) ? 0.0f : (y > 0) ? 90.0f : 270.0f;
    }
  else
    {
      if (x > 0)
	{
	  if (y >= 0)
	    {
	      ret = lw6sys_math_rad2deg (atanf (((float) y) / ((float) x)));
	    }
	  else
	    {
	      ret = 360.0f - lw6sys_math_rad2deg (atanf (((float) -y) / ((float) x)));
	    }
	}
      else
	{
	  if (y >= 0)
	    {
	      ret = 180.0f - lw6sys_math_rad2deg (atanf (((float) y) / ((float) -x)));
	    }
	  else
	    {
	      ret = 180.0f + lw6sys_math_rad2deg (atanf (((float) -y) / ((float) -x)));
	    }
	}
    }

  return ret;
}
Example #13
0
void listenToCommands() {
	initCOMPort();
	initUSBRadio();
	lastSpeedSentTimer.start();
	HANDLE waitHandles[3] = { startSignal, osStatusDongle.hEvent };

	stateGoal = goalOnRight;
	stateBallSeen = ballOnRight;
	while (true) {
		float floorX, floorY;
		int currentx, currenty;
		findNearestFloorBall(floorX, floorY, currentx, currenty);
		float angle = atanf(floorY / floorX) * 180.0 / PI;

		swprintf_s(buffer, L"NX %.2f \n NY %.2f\n ang %.2f", floorX, floorY, angle);
		SetWindowText(infoGUI, buffer);

		swprintf_s(buffer, L"greencount %d", fieldGreenPixelCountShare);
		SetWindowText(infoGUI3, buffer);

		if (hCOMDongle != INVALID_HANDLE_VALUE) {
			if (lastSpeedSentTimer.time() > 0.5) { //if we send the speeds too often we get communication timeouts
				setSpeedBoth(currentDrivingState);
				lastSpeedSentTimer.start();
			}
		}
		switch (WaitForMultipleObjects(2, waitHandles, FALSE, 50)) {
		//case WAIT_OBJECT_0:
		//	prints(L"radio event %X\n", dwCommEvent);
		//	//receiveCommand();
		//	//WaitCommEvent(hCOMRadio, &dwCommEvent, &osStatus);
		//	break;
		case WAIT_OBJECT_0:
			prints(L"Start signal arrived.\n");
			SetWindowText(stateStatusGUI, L"started");
			dribblerON();
			play();
			dribblerOFF();
			//discharge();
			break;
		case WAIT_OBJECT_0 + 1: //info from the main board controller
			//prints(L"main board COM event %X\n", dwCommEventDongle);
			handleMainBoardCommunication();
			WaitCommEvent(hCOMDongle, &dwCommEventDongle, &osStatusDongle); //listen to new events, ie beginning character
			break;
		}
		receiveCommand();
	}
}
Example #14
0
void Stage04::StagePass()
{
	this->unschedule(SEL_SCHEDULE(&Stage04::createCarWithTimer));
	bus = Sprite::create("bus.png");
	bus->setPosition(winSize.width + 400, 150);
	this->addChild(bus);
	DelayTime* busdelaytime1 = DelayTime::create(1);
	MoveTo* busmoveto1 = MoveTo::create(3,ccp(winSize.width / 2, 150));
	DelayTime* busdelaytime2 = DelayTime::create(2);
	MoveTo* busmoveto2 = MoveTo::create(3,ccp(-400, 150));
	auto busaction = Sequence::create(busdelaytime1,busmoveto1, busdelaytime2, busmoveto2, NULL);
	bus->runAction(busaction);

	auto s	= getChildByTag(1);
	float o = winSize.width / 2 - s->getPosition().x;
	float a = 100 - s->getPosition().y;
	float at = (float) CC_RADIANS_TO_DEGREES( atanf( o/a) );

	if( a < 0 ) 
	{
		if(  o < 0 )
			at = 180 + fabs(at);
		else
			at = 180 - fabs(at);    
	}
	RotateTo* rotateto =  RotateTo::create(0.5, at);
	DelayTime* herodelaytime = DelayTime::create(4);
	MoveTo* heromoveto = MoveTo::create(1,ccp(winSize.width / 2, 150));
	FadeOut* herofadeout = FadeOut::create(0.5f);
	auto heroaction = Sequence::create(rotateto,herodelaytime, heromoveto, herofadeout, NULL);
	s->runAction(heroaction);

	winboard = Sprite::create("winboard.png");
	winboard->setPosition(winSize.width / 2, winSize.height + 250);
	this->addChild(winboard);
	LabelTTF* playTTF1		=	LabelTTF::create("next stage","ARNPRIOR",32);
	MenuItemLabel* item1		=	MenuItemLabel::create(playTTF1,CC_CALLBACK_1(Stage04::WinBoardNextCallBack,this));
	LabelTTF* playTTF2		=	LabelTTF::create("main menu","ARNPRIOR",32);
	MenuItemLabel* item2		=	MenuItemLabel::create(playTTF2,CC_CALLBACK_1(Stage04::WinBoardMainMenuCallBack,this));
	item1->setPositionY(item1->getPositionY() + 20);
	item2->setPositionY(item2->getPositionY() - 20);
	Menu* playMenu			=	Menu::create(item1, item2, NULL);
	playMenu->setPosition(270,130);
	winboard->addChild(playMenu);
	DelayTime* boarddelaytime = DelayTime::create(7);
	MoveTo* boardmoveto = MoveTo::create(1,ccp(winSize.width / 2, winSize.height / 2));
	auto boardaction = Sequence::create(boarddelaytime, boardmoveto, NULL);
	winboard->runAction(boardaction);
}
Example #15
0
//---------------------------
//
//---------------------------
Matrix44 Matrix44::GetBillboardX() const
{
	static Matrix44	matWorld;
	static Vector3	vDir;

	vDir.x = _13;
	vDir.y = _23;
	vDir.z = _33;

	if( vDir.x == 0.0F )
	{
		matWorld.SetIdentity();
	}
	else	if( vDir.x > 0.0F )
	{
		matWorld.SetRotationY( -atanf( vDir.z / vDir.x ) + MATH_PI / 2 );
	}
	else
	{
		matWorld.SetRotationY( -atanf( vDir.z / vDir.x ) - MATH_PI / 2 );
	} //if..else if..else..

	return matWorld;
} //Matrix44::GetBillboardX
Example #16
0
crlb_qi_run(const vector2 *anchor, const size_t count, const vector2 *location)
{
    const float pi = (float) M_PI;
    // speed of light in su / s, assumes su = 1dm
    const float c = 299792458.0f / crlb_qi_su;
    // Constant alpha of the paper
    const float alpha =
         (c * c) / (8.0f * pi * pi * crlb_qi_beta * crlb_qi_beta);

    // Calculate the CRLB
    float numer = 0.0f;
    float denom = 0.0f;
    for (size_t i = 0; i < count; i++) {
	numer += distance_v(&(anchor[i]), location);
	for (size_t j = 0; j < i; j++) {
	    float phi_i = atanf((location->y - anchor[i].y) / (location->x - anchor[i].x));
	    float phi_j = atanf((location->y - anchor[j].y) / (location->x - anchor[j].x));
	    float a = sinf(phi_i - phi_j);
	    denom += distance_v(location, &(anchor[i])) *
                       distance_v(location, &(anchor[j])) * a * a;
	}
    }
    return crlb_qi_scale * alpha * numer / denom;
}
Example #17
0
void Dart::setAttributes(position3D startPos, position3D destPos, float power) {
	this->x = startPos.x;
	this->y = startPos.y;
	this->z = startPos.z;

	this->destx = destPos.x;
	this->desty = destPos.y;
	this->destz = destPos.z;

	this->dx = (destx - x)/30;
	this->dy = (desty - y)/30;
	this->dz = (destz - z)/30;

	this->angleX = 0.0;
	this->angleY = 0.0;
	this->angleZ = 0.0;

	draw = false;
	flies = true;
	justHit = false;

	angleX = atanf(dx/dz)*radToDec;
	angleY = atanf(dy/dz)*radToDec;
}
Example #18
0
float complex
catanhf(float complex z)
{
	float x, y, ax, ay, rx, ry;

	x = crealf(z);
	y = cimagf(z);
	ax = fabsf(x);
	ay = fabsf(y);

	if (y == 0 && ax <= 1)
		return (CMPLXF(atanhf(x), y));

	if (x == 0)
		return (CMPLXF(x, atanf(y)));

	if (isnan(x) || isnan(y)) {
		if (isinf(x))
			return (CMPLXF(copysignf(0, x), y + y));
		if (isinf(y))
			return (CMPLXF(copysignf(0, x),
			    copysignf(pio2_hi + pio2_lo, y)));
		return (CMPLXF(x + 0.0L + (y + 0), x + 0.0L + (y + 0)));
	}

	if (ax > RECIP_EPSILON || ay > RECIP_EPSILON)
		return (CMPLXF(real_part_reciprocal(x, y),
		    copysignf(pio2_hi + pio2_lo, y)));

	if (ax < SQRT_3_EPSILON / 2 && ay < SQRT_3_EPSILON / 2) {
		raise_inexact();
		return (z);
	}

	if (ax == 1 && ay < FLT_EPSILON)
		rx = (m_ln2 - logf(ay)) / 2;
	else
		rx = log1pf(4 * ax / sum_squares(ax - 1, ay)) / 4;

	if (ax == 1)
		ry = atan2f(2, -ay) / 2;
	else if (ay < FLT_EPSILON)
		ry = atan2f(2 * ay, (1 - ax) * (1 + ax)) / 2;
	else
		ry = atan2f(2 * ay, (1 - ax) * (1 + ax) - ay * ay) / 2;

	return (CMPLXF(copysignf(rx, x), copysignf(ry, y)));
}
Example #19
0
/*
====================
CalcFov
====================
*/
float CalcFov (float fov_x, float width, float height)
{
        float   a;
        float   x;

        if (fov_x < 1 || fov_x > 179)
                Sys_Error ("Bad fov: %f", fov_x);

        x = width/tanf(fov_x/360*Q_PI);

        a = atanf(height/x);

        a = a*360/Q_PI;

        return a;
}
Example #20
0
static void makect_32(void) {
  float *c = rdft_w + 32;
  const int nc = 32;
  int j, nch;
  float delta;

  ip[1] = nc;
  nch = nc >> 1;
  delta = atanf(1.0f) / nch;
  c[0] = cosf(delta * nch);
  c[nch] = 0.5f * c[0];
  for (j = 1; j < nch; j++) {
    c[j] = 0.5f * cosf(delta * j);
    c[nc - j] = 0.5f * sinf(delta * j);
  }
}
Example #21
0
void quat_logdif(AD_Quaternion *q1, AD_Quaternion *q2, AD_Quaternion *q3)
{
  AD_Quaternion inv, dif;
  float len, len1, s;

  quat_inverse (q1, &inv);
  quat_mul (&inv, q2, &dif);
  len = sqrtf (dif.x*dif.x + dif.y*dif.y + dif.z*dif.z);
  s = quat_dot (q1, q2);
  if (s != 0.0) len1 = atanf (len / s); else len1 = (float)(M_PI/2.0f);
  if (len != 0.0) len1 /= len;
  q3->w = 0.0;
  q3->x = dif.x * len1;
  q3->y = dif.y * len1;
  q3->z = dif.z * len1;
}
Example #22
0
	void rotation()
	{
		pmath::Vec2f angVec = m_rigidBody->GetPosition() - prevPos;

		if (m_direction * m_rigidBody->GetPosition().x > 0)
		{
			m_sliding = 1.8;
		}
		else
		{
			m_sliding = 1;
		}

		angle = m_sliding * atanf(angVec.y / angVec.x);
		parent->transform.SetRotation(pmath::radiansToDegrees(angle));
	}
Example #23
0
    OSPTexture2D getOSPDepthTextureFromOpenGLPerspective()
    {
      // compute fovy, aspect, zNear, zFar from OpenGL projection matrix
      GLdouble glProjectionMatrix[16];
      glGetDoublev(GL_PROJECTION_MATRIX, glProjectionMatrix);

      const double m0  = glProjectionMatrix[0];
      const double m5  = glProjectionMatrix[5];
      const double m10 = glProjectionMatrix[10];
      const double m14 = glProjectionMatrix[14];
      const double k = (m10 - 1.0f) / (m10 + 1.0f);

      const double fovy = 2. * atanf(1.0f / m5) * 180./M_PI;
      const double aspect = m5 / m0;
      const double zNear = (m14 * (1.0f - k)) / (2.0f * k);
      const double zFar = k * zNear;

      // get camera direction and up vectors from model view matrix
      GLdouble glModelViewMatrix[16];
      glGetDoublev(GL_MODELVIEW_MATRIX, glModelViewMatrix);

      const ospray::vec3f  cameraUp( glModelViewMatrix[1],  glModelViewMatrix[5],  glModelViewMatrix[9]);
      const ospray::vec3f cameraDir(-glModelViewMatrix[2], -glModelViewMatrix[6], -glModelViewMatrix[10]);

      // get window dimensions from OpenGL viewport
      GLint glViewport[4];
      glGetIntegerv(GL_VIEWPORT, glViewport);

      const size_t width = glViewport[2];
      const size_t height = glViewport[3];

      // read OpenGL depth buffer
      float *glDepthBuffer = new float[width * height];
      glReadPixels(0, 0, width, height, GL_DEPTH_COMPONENT, GL_FLOAT, (GLvoid *)glDepthBuffer);

      // get an OSPRay depth texture from the OpenGL depth buffer
      OSPTexture2D depthTexture
        = getOSPDepthTextureFromOpenGLPerspective(fovy, aspect, zNear, zFar, 
                                                  (osp::vec3f&)cameraDir, (osp::vec3f&)cameraUp, 
                                                  glDepthBuffer, width, height);

      // free allocated depth buffer
      delete[] glDepthBuffer;

      // return OSPRay depth texture
      return depthTexture;
    }
void BWJumpBy::update(float t)
{
    // parabolic jump (since v0.8.2)
    if (m_pTarget)
    {
        float frac = fmodf( t * m_nJumps, 1.0f );
        float y = m_height * 4 * frac * (1 - frac);
        
       // float fA = m_height * 4 ;
       // float y =   - frac*frac * fA + fA * frac + 5;
        

        y += m_delta.y * t;
        float x = m_delta.x * t;
#if CC_ENABLE_STACKABLE_ACTIONS
        CCPoint currentPos = m_pTarget->getPosition();

        CCPoint diff = ccpSub( currentPos, m_previousPos );
        m_startPosition = ccpAdd( diff, m_startPosition);

        CCPoint newPos = ccpAdd( m_startPosition, ccp(x,y));
        m_pTarget->setPosition(newPos);
        m_previousPos = newPos;
        
        if(m_bRotHead)
        {
            CCPoint dirPoint = ccpSub( currentPos,newPos );
            float fRadian = atanf(dirPoint.y/dirPoint.x);
            if((dirPoint.y > 0 && dirPoint.x > 0)
               || (dirPoint.y < 0 && dirPoint.x > 0))
            {
                fRadian = M_PI_2 - fRadian;
            }
            else if((dirPoint.y < 0 && dirPoint.x < 0)||
                    (dirPoint.y > 0 && dirPoint.x < 0))
            {
                fRadian = -M_PI_2 - fRadian;
            }
            float m_fRotation = fRadian*180.0/M_PI;
            m_pTarget->setRotation(m_fRotation);
        }
       
#else
        m_pTarget->setPosition(ccpAdd( m_startPosition, ccp(x,y)));
#endif // !CC_ENABLE_STACKABLE_ACTIONS
    }
}
Example #25
0
/** Navigates around (x, y). Clockwise iff radius > 0 */
void nav_circle_XY(float x, float y, float radius)
{
  struct EnuCoor_f *pos = stateGetPositionEnu_f();
  float last_trigo_qdr = nav_circle_trigo_qdr;
  nav_circle_trigo_qdr = atan2f(pos->y - y, pos->x - x);
  float sign_radius = radius > 0 ? 1 : -1;

  if (nav_in_circle) {
    float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr;
    NormRadAngle(trigo_diff);
    nav_circle_radians += trigo_diff;
    trigo_diff *= - sign_radius;
    if (trigo_diff > 0) { // do not rewind if the change in angle is in the opposite sense than nav_radius
      nav_circle_radians_no_rewind += trigo_diff;
    }
  }

  float dist2_center = DistanceSquare(pos->x, pos->y, x, y);
  float dist_carrot = CARROT * NOMINAL_AIRSPEED;

  radius += -nav_shift;

  float abs_radius = fabs(radius);

  /** Computes a prebank. Go straight if inside or outside the circle */
  circle_bank =
    (dist2_center > Square(abs_radius + dist_carrot)
     || dist2_center < Square(abs_radius - dist_carrot)) ?
    0 :
    atanf(stateGetHorizontalSpeedNorm_f() * stateGetHorizontalSpeedNorm_f() / (NAV_GRAVITY * radius));

  float carrot_angle = dist_carrot / abs_radius;
  carrot_angle = Min(carrot_angle, M_PI / 4);
  carrot_angle = Max(carrot_angle, M_PI / 16);
  float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle;
  horizontal_mode = HORIZONTAL_MODE_CIRCLE;
  float radius_carrot = abs_radius;
  if (nav_mode == NAV_MODE_COURSE) {
    radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius);
  }
  fly_to_xy(x + cosf(alpha_carrot)*radius_carrot,
            y + sinf(alpha_carrot)*radius_carrot);
  nav_in_circle = true;
  nav_circle_x = x;
  nav_circle_y = y;
  nav_circle_radius = radius;
}
Example #26
0
// inverse kinematics
// helper functions, calculates angle theta1 (for YZ-pane)
int RotaryDeltaSolution::delta_calcAngleYZ(float x0, float y0, float z0, float &theta)
{
    float y1 = -0.5F * tan30 * delta_f; // f/2 * tan 30
    y0      -=  0.5F * tan30 * delta_e; // shift center to edge
    // z = a + b*y
    float a = (x0 * x0 + y0 * y0 + z0 * z0 + delta_rf * delta_rf - delta_re * delta_re - y1 * y1) / (2.0F * z0);
    float b = (y1 - y0) / z0;

    float d = -(a + b * y1) * (a + b * y1) + delta_rf * (b * b * delta_rf + delta_rf); // discriminant
    if (d < 0.0F) return -1;                                            // non-existing point

    float yj = (y1 - a * b - sqrtf(d)) / (b * b + 1.0F);               // choosing outer point
    float zj = a + b * yj;

    theta = 180.0F * atanf(-zj / (y1 - yj)) / pi + ((yj > y1) ? 180.0F : 0.0F);
    return 0;
}
Example #27
0
/** Used in \ref determineOvertakePosition to adjust the overtake position
 *  which is calculated by slope of line if it's too close.
 *  \param old_slope Old slope calculated.
 *  \param rotate_up If adjust the slope upwards.
 *  \return A newly calculated slope.
 */
float SoccerAI::rotateSlope(float old_slope, bool rotate_up)
{
    const float theta = atanf(old_slope) + (old_slope < 0 ? M_PI : 0);
    float new_theta = theta + (rotate_up ? M_PI / 6 : -M_PI /6);
    if (new_theta > ((M_PI / 2) - 0.02f) && new_theta < ((M_PI / 2) + 0.02f))
    {
        // Avoid almost tan 90
        new_theta = (M_PI / 2) - 0.02f;
    }
    // Check if over-rotated
    if (new_theta > M_PI)
        new_theta = M_PI - 0.1f;
    else if (new_theta < 0)
        new_theta = 0.1f;

    return tanf(new_theta);
}   // rotateSlope
Example #28
0
void Helm::goTo( xyz location ) {
  
  xyz delta;
  float distance;
  float theta;
  
  delta.x = location.x - currentPos.x;
  delta.y = location.y - currentPos.y;
  delta.z = location.z - currentPos.z;
  
  distance = sqrt( pow(delta.x, 2) + pow(delta.y, 2) );
  theta = atanf( delta.x / delta.y ) - this->rotation;
  
  rotate( theta );
  goDistance( distance );
  
}
Example #29
0
/*
====================
AdaptFovx

Adapts a 4:3 horizontal FOV to the current screen size (Hor+).
====================
*/
static float AdaptFovx (float fov43, float w, float h)
{
	static const float pi = M_PI; /* float instead of double. */

	if (w <= 0 || h <= 0)
		return fov43;

	/*
	 * Formula:
	 *
	 * fov = 2.0 * atan(width / height * 3.0 / 4.0 * tan(fov43 / 2.0))
	 *
	 * The code below is equivalent but precalculates a few values and
	 * converts between degrees and radians when needed.
	 */
	return (atanf(tanf(fov43 / 360.0f * pi) * (w / h) * 0.75f) / pi * 360.0f);
}
 void ani_subobject_state::set_affine_transform(const ani_affine_transform& affine_transform)
 {
     m_scale.x = sqrtf((affine_transform.a * affine_transform.a) + (affine_transform.c * affine_transform.c));
     m_scale.y = sqrtf((affine_transform.b * affine_transform.b) + (affine_transform.d * affine_transform.d));
     
     m_position.x = affine_transform.tx;
     m_position.y = affine_transform.ty;
     
     f32 sign = atanf(-affine_transform.c / affine_transform.a);
     f32 radians  = acosf(affine_transform.a / m_scale.x);
     m_rotation  = glm::degrees(radians);
     
     if ((m_rotation > 90.f && sign > 0.f) || (m_rotation < 90.f && sign < 0.f))
     {
         m_rotation = 360.f - m_rotation;
     }
 }