void RotateY(Vec3d &v, double degree){
    double c = cos(Deg2Rad(degree));
      double s = sin(Deg2Rad(degree));
        double v0 = v[0] * c + v[2] * s;
          double v2 = -v[0] * s + v[2] * c;
            v[0] = v0; v[2] = v2; 
}
Exemple #2
0
static int SetAndCloseMap(
       MAP *m,
       const ATTRIBUTES *a)
{
       double angle;

       if (! (IS_MV_REAL8(&(a->angle))))
       {
        if (a->angle < 0)
           angle = -Deg2Rad(-a->angle);
        else
           angle = Deg2Rad(a->angle);
       }
       if (RuseAs(m, CR_REAL8))
              goto error2;

       if (a->projection != PT_UNDEFINED)
        MputProjection(m,a->projection);
       if (! (IS_MV_REAL8(&(a->xUL))))
        RputXUL(m, a->xUL);
       if (! (IS_MV_REAL8(&(a->yUL))))
        RputYUL(m, a->yUL);
       if (! (IS_MV_REAL8(&(a->angle))))
        RputAngle(m, angle);
       if (! (IS_MV_REAL8(&(a->cellSize))))
        RputCellSize(m, a->cellSize);
       if(a->gisFileId != MV_UINT4)
        MputGisFileId(m,a->gisFileId);

       if (Merrno)
              goto error2;
       return 0;
error2:
       return       RetError(1,"Can not write to '%s': %s",MgetFileName(m),MstrError());
}
Exemple #3
0
inline void AngleMatrix(const Angle &angles, matrix3x4 &matrix)
{
	float sp, sy, sr, cp, cy, cr;

	SinCos(Deg2Rad(angles.x), sp, cp);
	SinCos(Deg2Rad(angles.y), sy, cy);
	SinCos(Deg2Rad(angles.z), sr, cr);

	matrix[0][0] = cp * cy;
	matrix[1][0] = cp * sy;
	matrix[2][0] = -sp;

	float crcy = cr * cy;
	float crsy = cr * sy;
	float srcy = sr * cy;
	float srsy = sr * sy;

	matrix[0][1] = sp * srcy - crsy;
	matrix[1][1] = sp * srsy + crcy;
	matrix[2][1] = sr * cp;

	matrix[0][2] = sp * crcy + srsy;
	matrix[1][2] = sp * crsy - srcy;
	matrix[2][2] = cr * cp;

	matrix[0][3] = 0.0f;
	matrix[1][3] = 0.0f;
	matrix[2][3] = 0.0f;
}
void RotateX(Vec3d &v, double degree){
  double c = cos(Deg2Rad(degree));
  double s = sin(Deg2Rad(degree));
  double v1 = v[1] * c - v[2] * s;
  double v2 = v[1] * s + v[2] * c;
  v[1] = v1; v[2] = v2; 
  return;
}
Exemple #5
0
YOCTO_PROGRAM_START()
{
#include "main-core.cpp"

    double       zeta      = L.Get<double>("zeta");
    double       alpha_deg = L.Get<double>("alpha");
    double       alpha     = Deg2Rad(alpha_deg);
    std::cerr << "alpha=" << alpha_deg << std::endl;

    B.SaveLens("lens.dat", zeta);


    {
        ios::wcstream fp("profile.dat");
        ios::wcstream rp("results.dat");

        for(double theta_deg = 1; theta_deg <= 179; theta_deg += 1)
        {
            const double theta = Deg2Rad(theta_deg);
            const double ans   = B.profile(alpha,theta,zeta,&fp,false);
            fp << "\n";
            rp("%g %g %g\n", theta_deg, ans, sin(B.param[BRIDGE_A]));
        }

    }


    bool isFlat = false;
    {
        ios::wcstream fp("theta_opt.dat");

        const double theta = B.find_theta(alpha,zeta,isFlat);
        std::cerr << "THETA=" << Rad2Deg(theta) << std::endl;
        if(theta<=0)
        {
            std::cerr << "No Bridge" << std::endl;
            fp("%g %g\n", B.start_u, B.start_v);
        }
        else
        {
            if(isFlat)
            {
                for(double xx=0;xx<=1.0;xx+=0.1)
                {
                    fp("%g %g\n", B.start_u+xx, B.start_v);
                }
            }
            else
            {
                (void) B.profile(alpha, theta,zeta,&fp);
            }
            const double shift = B.compute_shift(alpha, theta, zeta);
            std::cerr << "shift=" << shift << std::endl;
        }
        
    }

}
Exemple #6
0
void Robot_Arm::DH2TransforMatrix(double a, double alpha, double d, double theta, Eigen::Matrix4d& T)
{
	alpha = Deg2Rad(alpha);
    theta = Deg2Rad(theta);
    
   T << cos(theta), -sin(theta)*cos(alpha),  sin(theta)*sin(alpha),   a*cos(theta),
        sin(theta),  cos(theta)*cos(alpha),  -cos(theta)*sin(alpha),  a*sin(theta),
        0,           sin(alpha),             cos(alpha),              d,
        0,           0,                      0,                       1;

}
Exemple #7
0
inline void AngleVectors(const Angle &angles, Vector &forward)
{
	float sp, sy, cp, cy;

	SinCos(Deg2Rad(angles.x), sp, cp);
	SinCos(Deg2Rad(angles.y), sy, cy);

	forward.x = cp * cy;
	forward.y = cp * sy;
	forward.z = -sp;
	forward.Normalize();
}
Exemple #8
0
YOCTO_PROGRAM_START()
{
#include "main-core.cpp"

#if 0
    double       zeta      = Lua::Config::Get<lua_Number>(L, "zeta" );
    double       theta_deg = Lua::Config::Get<lua_Number>(L,"theta");
    double       theta     = Deg2Rad(theta_deg);
    std::cerr << "theta=" << theta_deg << std::endl;

    B.SaveLens("lens.dat", zeta);

    {
        ios::wcstream fp("profile.dat");
        ios::wcstream rp("results.dat");
        for(double alpha_deg=1;alpha_deg<=90; alpha_deg += 0.2)
        {
            const double ans = B.profile(Deg2Rad(alpha_deg), theta, zeta, &fp);
            rp("%g %g\n",alpha_deg,ans);
            fp << "\n";
        }
    }

    bool isFlat = false;
    {
        ios::wcstream fp("alpha_opt.dat");

        const double alpha = B.find_alpha(theta,zeta,isFlat);
        std::cerr << "ALPHA=" << Rad2Deg(alpha) << std::endl;
        if(alpha<=0)
        {
            std::cerr << "No Bridge" << std::endl;
            fp("%g %g\n", B.start_u, B.start_v);
        }
        else
        {
            if(isFlat)
            {
                for(double xx=0;xx<=1.0;xx+=0.1)
                {
                    fp("%g %g\n", B.start_u+xx, B.start_v);
                }
            }
            else
            {
                (void) B.profile(alpha, theta,zeta,&fp);
            }
        }
        
    }
#endif

}
Exemple #9
0
static int CreateMap(
       const char *name,
       const ATTRIBUTES *a)
{
       UINT1 *buf;
       size_t i;
       double angle;
       MAP *m;

  size_t CELLMAX = (long int)(pow(2,30) - 1);
  if(((size_t)a->nrRows * (size_t)a->nrCols) > CELLMAX){
    printf("WARNING:\n  The specified amount of cells exceeds 2^30 - 1.\n  Not all PCRaster applications accept maps of this size.\n");
  }

       if (a->angle < 0)
           angle = -Deg2Rad(-a->angle);
       else
           angle = Deg2Rad(a->angle);
       m = Rcreate(name,(size_t)a->nrRows,(size_t)a->nrCols, a->cellRepr, a->valueScale,
                     a->projection, a->xUL, a->yUL, angle, a->cellSize);
       if (m == NULL)
        goto error1;

       PRECOND(a->gisFileId != MV_UINT4);
       if (MputGisFileId(m,a->gisFileId) == MV_UINT4)
              goto error2;

       if (RuseAs(m, CR_UINT1))
              goto error2;
       buf = (UINT1 *)Rmalloc(m, (size_t)a->nrCols);
       if (buf == NULL)
       {
        Mclose(m);
        remove(name);
        return 1;
       }
       for(i=0; i < a->nrRows; i++)
       {
              memset(buf,1,(size_t)a->nrCols);
              RputRow(m,i,buf);
       }
       Free(buf);
       Mclose(m);
       return 0;
error2:
       Mclose(m);
       remove(name);
error1:
       return       RetError(1,"Can not create '%s': %s",name,MstrError());
}
Exemple #10
0
std::tuple<float, float> CreateUnitVector(float dir)
{
    dir = BoundDirection(dir);
    if (IsDiagonalDirection(dir)) {
        std::tuple<float, float> result{ 1.0, 1.0 };
        if ((dir > 50.0) && (dir < 310.0))
            std::get<0>(result) = -std::get<0>(result);
        if (dir < 180.0)
            std::get<1>(result) = -std::get<0>(result);
        return result;
    } else {
        return std::tuple<float, float>{ cos(Deg2Rad(dir)), -sin(Deg2Rad(dir)) };
    }
}
Exemple #11
0
TEST(TestSuite, testCaseRpy) {
    //std::cout << "Check row versus column major matrice\n";
    // TestRpy(0.1, 0, 0);
    double r = 90, p = 0, y = 90;
    printf(" IN: %f %f %f\n", r, p, y);
    tf::Matrix3x3 m = GetTfRotationMatrix(Deg2Rad(r), Deg2Rad(p), Deg2Rad(y));
    GetRPY(m, r, p, y);
    printf("OUT: %f %f %f\n", Rad2Deg(r), Rad2Deg(p), Rad2Deg(y));
    tf::Vector3 xaxis, zaxis;
    GetHighland(m, xaxis, zaxis);
    printf("%f %f %f\n", xaxis.getX(), xaxis.getY(), xaxis.getZ());
    printf("%f %f %f\n", zaxis.getX(), zaxis.getY(), zaxis.getZ());

}
Exemple #12
0
void
GoToPoint::call()
{
	double  dis_to_point;
	double  dir_to_point;
	Vector2D agent_p = m_world->me().pos;
	double  ang_permisible;
	double  turn_parameter;
	Vector2D velocidad;
	double  dash_parameter;
	double const dash_power_rate = 0.006, effort = 0.8;
	double const inertia_moment = 5.0;

	dis_to_point = (m_target - agent_p).mag();
	dir_to_point = Rad2Deg( atan2( m_target.y - agent_p.y , m_target.x - agent_p.x) );
	if( dis_to_point > m_radius )  // El agente no ha llegado al punto
	{
		ang_permisible = Rad2Deg( atan2( m_radius , dis_to_point ) );
		turn_parameter = dir_to_point - m_world->me().angleDeg();
		turn_parameter = entre180( turn_parameter );
		velocidad.x	  = m_world->me().speed_amount;
		velocidad.y   = m_world->me().speed_dir;
		if( ang_permisible < turn_parameter * 0.1  ||  dis_to_point > 25.0 )
			ang_permisible = 25.0;

		if( fabs(turn_parameter) > ang_permisible )  // el agente no esta bien alineado al punto
		{
			velocidad     = Vector2D::fromPolar( velocidad.x, Deg2Rad(velocidad.y) );
			turn_parameter =  turn_parameter *(1.0 + inertia_moment*velocidad.mag() );
			turn_parameter = entre180( turn_parameter );
			m_command->append_turn( turn_parameter );
		}
		else
		{
			velocidad     = Vector2D::fromPolar( velocidad.x, Deg2Rad(velocidad.y - m_world->me().angleDeg()) );
			dash_parameter = ( dis_to_point - velocidad.x )  / ( dash_power_rate * effort );
			if( dash_parameter > 100.0)
				dash_parameter = 100.0;
			if( m_dash_override )
				m_command->append_dash( m_dash_power );
			else
				m_command->append_dash( dash_parameter );
		}
	}
	else
	{
		// El agente llegó al punto
	}
}
Exemple #13
0
CalChart::Coord CreateVector(float dir, float mag)
{
    dir = BoundDirection(dir);
    if (IsDiagonalDirection(dir)) {
        CalChart::Coord r{ Float2CoordUnits(mag), Float2CoordUnits(mag) };
        if ((dir > 50.0) && (dir < 310.0))
            r.x = -r.x;
        if (dir < 180.0)
            r.y = -r.y;
        return r;
    } else {
        return { Float2CoordUnits(mag * cos(Deg2Rad(dir))),
            Float2CoordUnits(mag * -sin(Deg2Rad(dir))) };
    }
}
Exemple #14
0
void
RunWithBall::call()
{
	double ang_to_point;
	Vector2D aux;
	Vector2D pos  = m_world->me().pos;
	Vector2D ball = m_world->estBallPosition();
	double disBall = m_world->bitacoraBalon.begin()->dis;
	if ( disBall <= m_radius )
	{
		ang_to_point = Rad2Deg( atan2(  m_target.y- pos.y, m_target.x - pos.x) );

		// Este m_pass_distance es similar a lo que queremos que adelante la pelota,
		// para valores grandes la adelanta mucho.
		aux = Vector2D::fromPolar( m_pass_distance, Deg2Rad( ang_to_point ) );

		m_pass_to_point->setDesiredVel( m_ball_final_vel );
		m_pass_to_point->setTarget( pos + aux );
		m_pass_to_point->call();
	}
	else
	{
		m_go_to_point->setDashOverride( false );
		m_go_to_point->setRadius( m_radius );
		m_go_to_point->setTarget( ball );
		m_go_to_point->call();
	}
}
Exemple #15
0
static double CalcRad(double lat)
/* earth's radius of curvature in meters at specified latitude.*/
{
    const double a = 6378.137;
    const double e2 = 0.081082 * 0.081082;
    // the radius of curvature of an ellipsoidal Earth in the plane of a
    // meridian of latitude is given by
    //
    // R' = a * (1 - e^2) / (1 - e^2 * (sin(lat))^2)^(3/2)
    //
    // where a is the equatorial radius,
    // b is the polar radius, and
    // e is the eccentricity of the ellipsoid = sqrt(1 - b^2/a^2)
    //
    // a = 6378 km (3963 mi) Equatorial radius (surface to center distance)
    // b = 6356.752 km (3950 mi) Polar radius (surface to center distance)
    // e = 0.081082 Eccentricity
    double sc = sin(Deg2Rad(lat));
    double x = a * (1.0 - e2);
    double z = 1.0 - e2 * sc * sc;
    double y = pow(z, 1.5);
    double r = x / y;

    return r * 1000.0;	// Convert to meters
}
Exemple #16
0
void Player::Update(float dt)
{
    const Uint8* keys = System::GetKeyStates();

    //
    // apply rotations
    //
    if (keys[SDL_SCANCODE_D]) {
        mAngle += mRotSpeed * dt;
    }
    if (keys[SDL_SCANCODE_A]) {
        mAngle -= mRotSpeed * dt;
    }

    // keep the angle in standard range (-180, 180]
    mAngle = StandardizeAngle(mAngle);

    //std::cout << (int)mAngle << std::endl;

    float radAngle = Deg2Rad(mAngle);

    Vec2 dir = GetDirectionR(radAngle);

    if (keys[SDL_SCANCODE_W]) {
        //mCenter.x += mSpeed * dt * std::cos(radAngle);
        //mCenter.y += mSpeed * dt * std::sin(radAngle);
        mCenter += mSpeed * dt * dir;
    }
    if (keys[SDL_SCANCODE_S]) {
        //mCenter.x -= mSpeed * dt * std::cos(radAngle);
        //mCenter.y -= mSpeed * dt * std::sin(radAngle);
        mCenter -= mSpeed * dt * dir;
    }
}
Exemple #17
0
bool
Monitor::dispplayer( const char * command )
{
    // a player is given new position by the monitor
    int side, unum;
    int x, y, a;
    if ( std::sscanf( command,
                      " ( dispplayer %d %d %d %d %d ) ",
                      &side, &unum, &x, &y, &a ) != 5 )
    {
        sendMsg( MSG_BOARD, "(error illegal_command_form)" );
        return false;
    }

    double real_x = x / SHOWINFO_SCALE;
    double real_y = y / SHOWINFO_SCALE;
    double angle = Deg2Rad( a );
    PVector vel( 0.0, 0.0 );

    return M_stadium.movePlayer( static_cast< Side >( side ),
                                 unum,
                                 PVector( real_x, real_y ),
                                 &angle,
                                 &vel );
}
Exemple #18
0
template<> const ClassAccessors& GetAccessors<PlatformRig::DefaultShadowFrustumSettings>()
{
    using Obj = PlatformRig::DefaultShadowFrustumSettings;
    static ClassAccessors props(typeid(Obj).hash_code());
    static bool init = false;
    if (!init) {
        props.Add(u("FrustumCount"), DefaultGet(Obj, _frustumCount),  
            [](Obj& obj, unsigned value) { obj._frustumCount = Clamp(value, 1u, SceneEngine::MaxShadowTexturesPerLight); });
        props.Add(u("MaxDistanceFromCamera"),  DefaultGet(Obj, _maxDistanceFromCamera),   DefaultSet(Obj, _maxDistanceFromCamera));
        props.Add(u("FrustumSizeFactor"),   DefaultGet(Obj, _frustumSizeFactor),    DefaultSet(Obj, _frustumSizeFactor));
        props.Add(u("FocusDistance"),   DefaultGet(Obj, _focusDistance),    DefaultSet(Obj, _focusDistance));
        props.Add(u("Flags"),   DefaultGet(Obj, _flags),    DefaultSet(Obj, _flags));
        props.Add(u("TextureSize"),   DefaultGet(Obj, _textureSize),    
            [](Obj& obj, unsigned value) { obj._textureSize = 1<<(IntegerLog2(value-1)+1); });  // ceil to a power of two
        props.Add(u("SingleSidedSlopeScaledBias"),   DefaultGet(Obj, _slopeScaledBias),    DefaultSet(Obj, _slopeScaledBias));
        props.Add(u("SingleSidedDepthBiasClamp"),   DefaultGet(Obj, _depthBiasClamp),    DefaultSet(Obj, _depthBiasClamp));
        props.Add(u("SingleSidedRasterDepthBias"),   DefaultGet(Obj, _rasterDepthBias),    DefaultSet(Obj, _rasterDepthBias));
        props.Add(u("DoubleSidedSlopeScaledBias"),   DefaultGet(Obj, _dsSlopeScaledBias),    DefaultSet(Obj, _dsSlopeScaledBias));
        props.Add(u("DoubleSidedDepthBiasClamp"),   DefaultGet(Obj, _dsDepthBiasClamp),    DefaultSet(Obj, _dsDepthBiasClamp));
        props.Add(u("DoubleSidedRasterDepthBias"),   DefaultGet(Obj, _dsRasterDepthBias),    DefaultSet(Obj, _dsRasterDepthBias));
        props.Add(u("WorldSpaceResolveBias"),   DefaultGet(Obj, _worldSpaceResolveBias),    DefaultSet(Obj, _worldSpaceResolveBias));
        props.Add(u("BlurAngleDegrees"),   
            [](const Obj& obj) { return Rad2Deg(XlATan(obj._tanBlurAngle)); },
            [](Obj& obj, float value) { obj._tanBlurAngle = XlTan(Deg2Rad(value)); } );
        props.Add(u("MinBlurSearch"),   DefaultGet(Obj, _minBlurSearch),    DefaultSet(Obj, _minBlurSearch));
        props.Add(u("MaxBlurSearch"),   DefaultGet(Obj, _maxBlurSearch),    DefaultSet(Obj, _maxBlurSearch));
        init = true;
    }
    return props;
}
Exemple #19
0
real CalcAngularDistanceInRadians(real angle1InDegrees, real angle2InDegrees)
{
    real angle = CalcAngularDistanceInDegrees(
            Rad2Deg(angle1InDegrees),
            Rad2Deg(angle2InDegrees)
    );
    return Deg2Rad(angle);
}
Exemple #20
0
void Kinematics::InitialAngle(float qAngle[])
{
	mJointU[0].q = 0;
	for (unsigned int i=1; i<mJointU.size(); i++)
	{
		mJointU[i].q = Deg2Rad(qAngle[i-1]);
	}
}
Exemple #21
0
bool TransformComponent::VInit(XMLElement* pCompData)
{
	assert(pCompData);

	Vector3 position = Vector3(0.f, 0.f, 0.f);
	Vector3 rotation = Vector3(0.f, 0.f, 0.f);
	Vector3 scale = Vector3(1.f, 1.f, 1.f);

	XMLElement* pPositionElement = pCompData->FirstChildElement("Position");
	if (pPositionElement)
	{
		position.x = pPositionElement->FloatAttribute("x");
		position.y = pPositionElement->FloatAttribute("y");
		position.z = pPositionElement->FloatAttribute("z");
	}

	XMLElement* pRotationElement = pCompData->FirstChildElement("Rotation");
	if (pRotationElement)
	{
		rotation.x = Deg2Rad(pRotationElement->FloatAttribute("x"));
		rotation.y = Deg2Rad(pRotationElement->FloatAttribute("y"));
		rotation.z = Deg2Rad(pRotationElement->FloatAttribute("z"));
	}

	XMLElement* pScaleElement = pCompData->FirstChildElement("Scale");
	if (pScaleElement)
	{
		scale.x = pScaleElement->FloatAttribute("x");
		scale.y = pScaleElement->FloatAttribute("y");
		scale.z = pScaleElement->FloatAttribute("z");
	}


	Matrix trans;
	trans= glm::translate(trans, position);

	Matrix rot;
	rot = glm::yawPitchRoll(rotation.x, rotation.y, rotation.z);

	Matrix scaleM;
	scaleM = glm::scale(scaleM, scale);

	m_transform = scaleM * rot * trans;
	return true;
}
void ArbitraryRotate(Vec3d U, Vec3d V, Vec3d W,
                     double degreeX, double degreeY,
                     Vec3d& point, Vec3d aim) {
  double cx = cos(Deg2Rad(degreeX));
  double sx = sin(Deg2Rad(degreeX));
  double cy = cos(Deg2Rad(degreeY));
  double sy = sin(Deg2Rad(degreeY));
  
  Matrixd trans = { {1, 0, 0, -aim[0]},
    {0, 1, 0, -aim[1]},
    {0, 0, 1, -aim[2]},
    {0, 0, 0, 1}};
  
  Matrixd mat = { {U[0], U[1], U[2], 0},
    {V[0], V[1], V[2], 0},
    {W[0], W[1], W[2], 0},
    {0, 0, 0, 1}};
  
  Matrixd rot(4, 4);
  Matrixd pos =  {{point[0]}, {point[1]}, {point[2]}, {1}};
  
  pos = trans * pos;
  
  pos = mat*pos;
  
  rot = {{1,   0,  0, 0},
    {0,  cx, sx, 0},
    {0, -sx, cx, 0},
    {0,   0,  0, 1}};
  
  pos = rot*pos;
  
  rot = {{ cy, 0, sy, 0},
    {0, 1,  0, 0},
    {-sy, 0, cy, 0},
    {0, 0,  0, 1}};
  
  pos = rot * pos;
  
  pos = mat.inv()*pos;
  
  pos = trans.inv()*pos;
  
  point = {pos.get(0, 0), pos.get(1, 0), pos.get(2, 0)};
}
double DoCos(double Angle)
{
    double CoSine;
#ifdef DEG
    Angle = Deg2Rad(Angle);
#endif
    CoSine = (double)cosf(Angle);
    return CoSine;
}
double DoSin(double Angle)
{
    double Sine;
#ifdef DEG
    Angle = Deg2Rad(Angle);
#endif
    Sine = (double)sinf(Angle);
    return Sine;
}
double DoTan(double Angle)
{
    double Tan;
#ifdef DEG
    Angle = Deg2Rad(Angle);
#endif
    Tan = (double)tanf(Angle);
    return Tan;
}
/**
 * Update data used by tackle.
 * \param agent.
 */
void Tackler::UpdateTackleData(const Agent & agent)
{
    if (mAgentID == agent.GetAgentID())  {
        return; /** no need to update */
    }

    mAgentID = agent.GetAgentID();

    const BallState & ball_state     = agent.GetWorldState().GetBall();
    const PlayerState & player_state = agent.GetSelf();
    Vector ball_2_player = (ball_state.GetPos() - player_state.GetPos()).Rotate(-player_state.GetBodyDir());

    Vector ball_vel;
    mMaxTackleSpeed = -1.0;
    mDirMap.clear();

    const double max_tackle_power = ServerParam::instance().maxTacklePower();
    const double min_back_tackle_power = ServerParam::instance().maxBackTacklePower();

    double factor = 1.0 - 0.5 * (fabs(Deg2Rad(ball_2_player.Dir())) / M_PI);

    for (AngleDeg tackle_angle = -180.0 + FLOAT_EPS; tackle_angle <= 180.0 + FLOAT_EPS; tackle_angle += 1.0) {
    	Vector ball_vel(ball_state.GetVel());
        double eff_power = (min_back_tackle_power + ((max_tackle_power - min_back_tackle_power) * (1.0 - fabs(Deg2Rad(tackle_angle)) / M_PI))) * ServerParam::instance().tacklePowerRate();
        eff_power *= factor;
        ball_vel += Polar2Vector(eff_power, tackle_angle + player_state.GetBodyDir());
        ball_vel = ball_vel.SetLength(Min(ball_vel.Mod(), ServerParam::instance().ballSpeedMax()));

        int angle_idx = ang2idx(tackle_angle);
        int dir_idx = dir2idx(ball_vel.Dir());

        mTackleAngle[angle_idx] = tackle_angle;
        mBallVelAfterTackle[angle_idx] = ball_vel;
        mDirMap[dir_idx].push_back(std::make_pair(angle_idx, ang2idx(tackle_angle + 1.0)));

        if (ball_vel.Mod() > mMaxTackleSpeed){
            mMaxTackleSpeed = ball_vel.Mod();
        }

        if (ball_vel.Mod() * ServerParam::instance().ballDecay() < FLOAT_EPS) {
        	mCanTackleStopBall = true;
        	mTackleStopBallAngle = tackle_angle;
        }
    }
//
//
//    for (AngleDeg tackle_angle = -180.0 + 0.3; tackle_angle <= 180.0 + FLOAT_EPS; tackle_angle += 1.0) {
//    	Vector ball_vel = GetBallVelAfterTackle(agent, tackle_angle);
//    	AngleDeg tackle_angle2 = 0.0;
//    	GetTackleAngleToDir(agent, ball_vel.Dir(), & tackle_angle2);
//
//    	std::cout << tackle_angle << " " << tackle_angle2 << std::endl;
//    }
//
//    exit(0);
}
Exemple #27
0
POINT2D Rotate(POINT2D rot_point, POINT2D obj_point, double theta)
{
	POINT2D ret_point;
	double rad = Deg2Rad(theta);

	ret_point.x = (obj_point.x - rot_point.x)*cos(rad) - (obj_point.y - rot_point.y)*sin(rad) + rot_point.x;
	ret_point.y = (obj_point.x - rot_point.x)*sin(rad) + (obj_point.y - rot_point.y)*cos(rad) + rot_point.y;

	return (ret_point);
}
Exemple #28
0
double earth_distance(double lat1, double lon1, double lat2, double lon2)
/* distance in meters between two points specified in degrees. */
{
    double x1 = CalcRad(lat1) * cos(Deg2Rad(lon1)) * sin(Deg2Rad(90-lat1));
    double x2 = CalcRad(lat2) * cos(Deg2Rad(lon2)) * sin(Deg2Rad(90-lat2));
    double y1 = CalcRad(lat1) * sin(Deg2Rad(lon1)) * sin(Deg2Rad(90-lat1));
    double y2 = CalcRad(lat2) * sin(Deg2Rad(lon2)) * sin(Deg2Rad(90-lat2));
    double z1 = CalcRad(lat1) * cos(Deg2Rad(90-lat1));
    double z2 = CalcRad(lat2) * cos(Deg2Rad(90-lat2));
    double a = (x1*x2 + y1*y2 + z1*z2)/pow(CalcRad((lat1+lat2)/2),2);
    // a should be in [1, -1] but can sometimes fall outside it by
    // a very small amount due to rounding errors in the preceding
    // calculations.  This is prone to happen when the argument points
    // are very close together.  Return NaN so calculations trying
    // to use this will also blow up.
    if (fabs(a) > 1) 
	return NAN;
    else
	return CalcRad((lat1+lat2) / 2) * acos(a);
}
Exemple #29
0
void
Controller::getDesiredTailState(float* ret)
{//요기
	ret[0] = realWingbeat.getTailBend(currentFrame, currentWeight); + Deg2Rad(-5);
	ret[1] = realWingbeat.getTailSpread(currentFrame, currentWeight);

#ifdef PEACOCK
	ret[1] = AngToRad(-35);
#endif
	
}
Exemple #30
0
POINT2D Rotate(POINT2D point, double theta)
{
	POINT2D ret_point;
	double rad = Deg2Rad(theta);

	ret_point.x = (point.x)*cos(rad) - (point.y)*sin(rad);
	ret_point.y = (point.x)*sin(rad) + (point.y)*cos(rad);


	return (ret_point);
}