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;
}
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());
}
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;
}
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;
}
}
}
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;
}
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();
}
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
}
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());
}
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)) };
}
}
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());
}
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
}
}
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))) };
}
}
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();
}
}
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
}
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;
}
}
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 );
}
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;
}
real CalcAngularDistanceInRadians(real angle1InDegrees, real angle2InDegrees)
{
real angle = CalcAngularDistanceInDegrees(
Rad2Deg(angle1InDegrees),
Rad2Deg(angle2InDegrees)
);
return Deg2Rad(angle);
}
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]);
}
}
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);
}
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);
}
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);
}
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
}
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);
}
