glm::vec3 quatToEuler(glm::quat quat){
glm::vec3 angle(0,0,0);
angle.x = asin(2*quat.x*quat.y + 2*quat.z*quat.w);
angle.y = atan2(2*quat.y*quat.w-2*quat.x*quat.z,
1-2*quat.y*quat.y-2*quat.z*quat.z);
angle.z = atan2(2*quat.x*quat.w-2*quat.y*quat.z,
1-2*quat.x*quat.x-2*quat.z*quat.z);
angle.x = toDegrees(angle.x);
angle.y = toDegrees(angle.y);
angle.z = toDegrees(angle.z);
//angle.x += 180.0f;
angle.y -= 180.0f;
angle.z -= 180.0f;
return angle;
}
void
Airport::checkLandings()
{
if(flights.empty()) return;
//pthread_mutex_lock (&mutex);
std::list<Flight*>::iterator it;
it = flights.begin();
while(it != flights.end())
{
if((final_pos.distance((*it)->getPosition()) < LANDING_DIST) &&
(toDegrees(normalizePi(fabs((*it)->getBearing() - toRadians(LANDING_BEAR))))<LANDING_BEAR_MAX_ERROR) &&
((*it)->getSpeed()<LANDING_SPEED))
{
std::cerr<<"Flight "<<(*it)->getId()<<" landed"<<std::endl;
points += (int)(*it)->getPoints();
it = removeFlight((*it)->getId());
std::cerr<<"*";
return;
}else
it++;
}
//pthread_mutex_unlock (&mutex);
}
void
Airport::checkCrashes()
{
if(flights.empty()) return;
std::list<Flight*>::iterator it;
it = flights.begin();
while(it != flights.end())
{
if((*it)->getPosition().get_z()<CRASH_Z)
{
std::cerr<<"[PoZ]Crash of "<<(*it)->getId()<<std::endl;
it=removeFlight((*it)->getId());
points += CRASH_HEIGHT_POINTS;
}else if(toDegrees(fabs((*it)->getInclination())) > CRASH_INC)
{
std::cerr<<"[Inc] Crash of "<<(*it)->getId()<<std::endl;
it = removeFlight((*it)->getId());
points += CRASH_INC_POINTS;
}else if( (*it)->getSpeed()<CRASH_SPEED)
{
std::cerr<<"[Spd] Crash of "<<(*it)->getId()<<std::endl;
it = removeFlight((*it)->getId());
points += CRASH_SPEED_POINTS;
}else
it++;
}
}
float angleDiffDeg(float a, float b)
{
/*float diff = fmod(b - a + 180.f, 360.f);
if(diff < 0.f) diff += 380.f;
return diff - 180.f;*/
return toDegrees(angleDiff(toRadians(a), toRadians(b)));
}
QString Histogram::toString() const
{
QStringList l;
for (size_t i = 0; i < _bins.size(); ++i)
{
l << QString::fromUtf8("%1°: %2").arg(toDegrees(getBinCenter(i))).arg(_bins[i]);
}
return l.join(", ");
}
void Vehicle::update( const Vec2f& mousePosition )
{
arrive( mousePosition );
velocity += acceleration;
velocity.limit( maxSpeed );
position += velocity;
acceleration.set( Vec2f::zero() );
angle = toDegrees( atan2f( velocity.y, velocity.x ) );
}
float BH2011GoalSymbols::getOpeningAngleOfFreePart()
{
float angleToLeftEnd = freePartOfOpponentGoalModel.leftEnd.angle();
float angleToRightEnd = freePartOfOpponentGoalModel.rightEnd.angle();
if(angleToLeftEnd < angleToRightEnd)
{
angleToLeftEnd += pi2;
}
return toDegrees(abs(normalize(angleToLeftEnd - angleToRightEnd)));
}
void Vehicle::update()
{
wander();
velocity += acceleration;
velocity.limit( maxSpeed );
position += velocity;
acceleration.set( Vec2f::zero() );
borders();
angle = toDegrees( atan2f( velocity.y, velocity.x ) );
}
void Rocket::run( const vector< shared_ptr< Obstacle > >& obstacles )
{
if ( ( ! hit_obstacle ) && ( ! hit_target ) ) {
apply_force( dna->get_gene( gene_counter ) );
gene_counter = ( gene_counter + 1 ) % dna->get_genes_length();
update();
check_obstacles( obstacles );
theta = toDegrees( atan2f( velocity.y, velocity.x ) );
}
}
bool formatSensor(Type type, float inValue, bool filtered, char* value) const
{
switch(type)
{
case usLDistance: inValue = usDistances[filtered ? 1 : 0][SensorData::left]; break;
case usLtRDistance: inValue = usDistances[filtered ? 1 : 0][SensorData::leftToRight]; break;
case usRtLDistance: inValue = usDistances[filtered ? 1 : 0][SensorData::rightToLeft]; break;
case usRDistance: inValue = usDistances[filtered ? 1 : 0][SensorData::right]; break;
default: break;
}
if(inValue == SensorData::off)
{
strcpy(value, "?");
return true;
}
switch(type)
{
case SensorWidget::distance:
sprintf(value, "%.1fmm", inValue);
break;
case SensorWidget::pressure:
sprintf(value, "%.1fg", inValue * 1000);
break;
case SensorWidget::acceleration:
sprintf(value, "%.1fmg", inValue * 1000);
break;
case SensorWidget::batteryLevel:
sprintf(value, "%.1fV", inValue);
break;
case SensorWidget::angularSpeed:
sprintf(value, "%.1f°/s", inValue);
break;
case SensorWidget::ratio:
sprintf(value, "%.1f%%", inValue * 100);
break;
case SensorWidget::angle2:
sprintf(value, "%.1f°", toDegrees(inValue));
break;
case SensorWidget::usLDistance:
sprintf(value, "%.0fmm", inValue);
break;
case SensorWidget::usLtRDistance:
sprintf(value, "%.0fmm", inValue);
break;
case SensorWidget::usRtLDistance:
sprintf(value, "%.0fmm", inValue);
break;
case SensorWidget::usRDistance:
sprintf(value, "%.0fmm", inValue);
break;
default:
return false;
}
return true;
}
void FindIntersectionsVisitor::visit(const ConstElementPtr& e)
{
shared_ptr<NodeToWayMap> n2w = _map->getIndex().getNodeToWayMap();
long id = e->getId();
const set<long>& wids = n2w->getWaysByNode(id);
// find all ways that are highways (ie roads)
set<long> hwids;
for (set<long>::const_iterator it = wids.begin(); it != wids.end(); ++it)
{
shared_ptr<Way> w = _map->getWay(*it);
if (OsmSchema::getInstance().isLinearHighway(w->getTags(), w->getElementType()))
{
hwids.insert(*it);
}
}
if (hwids.size() >= 3) // two or more roads intersting
{
// keep it
_ids.push_back(id);
_map->getNode(id)->setTag("IntersectionWayCount", QString::number(hwids.size()));
vector<Radians> angles = NodeMatcher::calculateAngles(_map, id, hwids, 10);
vector<double> v;
for (uint i = 0; i < angles.size(); i++)
{
v.push_back(toDegrees(angles[i])+180);
}
sort(v.begin(), v.end());
double minAngle = 360.;
double maxAngle = 0.;
for (uint i = 0; i < v.size(); i++)
{
double a = (i == 0) ? (v[i] + 360 - v[v.size()-1]) : v[i] - v[i-1];
if (a < minAngle)
{
minAngle = a;
}
if (a > maxAngle)
{
maxAngle = a;
}
}
_map->getNode(id)->setTag("MinAngle", QString::number(minAngle));
_map->getNode(id)->setTag("MaxAngle", QString::number(maxAngle));
}
}
sf::Vector2f Planet::toProjection(sf::Vector2f point, float radius) {
float circumference = 2 * PI * radius;
float r = sqrt(pow(point.x, 2) + pow(point.y, 2));
float angle = toDegrees(atan2(point.y, point.x));
float r2 = (sqrt(r) * sqrt(circumference / 4)) / (circumference / 4) * radius;
sf::Vector2f pos = makeVector(angle, r2, sf::Vector2f(radius, radius));
return pos;
}
sf::Vector2f Planet::toPlane(sf::Vector2f point, float radius) {
float circumference = 2 * PI * radius;
float r = sqrt(pow(point.x, 2) + pow(point.y, 2));
float angle = toDegrees(atan2(point.y, point.x));
float r2 = (pow(r, 2) / radius) / radius * (circumference / 4);
sf::Vector2f pos = makeVector(angle, r2, sf::Vector2f(circumference / 4, circumference /4));
return pos;
}
void SfmlBoxInstanceInfo::render()
{
if (Settings::renderDebugBoxes)
{
rectangle.setPosition(getPosition().x, getPosition().y);
rectangle.setRotation(toDegrees(getAngle()));
rectangle.setScale(getScale().x, getScale().y);
rectangle.setOrigin(getPivot().x*getSize().x, getPivot().y*getSize().y);
renderWindow->draw(rectangle);
}
}
void rt::Ra2Parser::loadCamFile(const std::string& filename)
{
std::ifstream file(filename.c_str());
std::string line;
std::vector<std::string> tokens;
Point position;
Vector direction;
Vector up;
float depth;
float filmSizeY;
if (file.is_open()) {
while (!file.eof()) {
getline(file, line);
tokenize(line, tokens);
if (tokens[0] == "position") {
float x = atof(tokens[1].c_str());
float y = atof(tokens[2].c_str());
float z = atof(tokens[3].c_str());
position = Point(x,y,z);
}
else if (tokens[0] == "direction") {
float x = atof(tokens[1].c_str());
float y = atof(tokens[2].c_str());
float z = atof(tokens[3].c_str());
direction = Vector(x,y,z);
}
else if (tokens[0] == "up") {
float x = atof(tokens[1].c_str());
float y = atof(tokens[2].c_str());
float z = atof(tokens[3].c_str());
up = Vector(x,y,z);
}
else if (tokens[0] == "depth") {
depth = atof(tokens[1].c_str());
}
else if (tokens[0] == "filmSizeY") {
filmSizeY = atof(tokens[1].c_str());
}
}
float fovy = toDegrees(atan((filmSizeY / 2) / depth));
camera_ = Camera(position, direction, up, fovy, fovy, depth, 1024, 1024);
}
else
std::cerr << "Unabe to open file " << filename << std::endl;
file.close();
}
void BgrManager::update( const Timer& timer ) {
const Vec& curr_dir = _p_camera->getDirection();
_rot_factor = toDegrees( acosf( dot( _init_dir, curr_dir ) ) ) / 360.0f / _fov_factor;
if ( dot( _right_dir, curr_dir ) > 0.0f ) {
_rot_factor = -( 1.0f / _fov_factor ) + _rot_factor;
}
else {
_rot_factor = -_rot_factor;
}
_vis_offset = 1.0f / _fov_factor + _rot_factor;
}
/**
* Synchronizes the state of this body with the internal Box2D state.
*/
void Box2DBody::synchronize()
{
Q_ASSERT(mBody);
if (sync(mBodyDef.position, mBody->GetPosition())) {
if (mTarget)
mTarget->setPosition(mWorld->toPixels(mBodyDef.position));
emit positionChanged();
}
if (sync(mBodyDef.angle, mBody->GetAngle()))
if (mTarget)
mTarget->setRotation(toDegrees(mBodyDef.angle));
}
void KinematicsSolver::showSolutionSet(SolutionSet *set) {
cout << setprecision(4) << " [ " << set->theta[0] << ", " << set->theta[1]
<< ", " << set->theta[2] << ", " << set->theta[3] << ", "
<< set->theta[4] << ", " << set->theta[5] << "] [ "
<< toDegrees(set->theta[0]) << ", " << toDegrees(set->theta[1])
<< ", " << toDegrees(set->theta[2]) << ", "
<< toDegrees(set->theta[3]) << ", " << toDegrees(set->theta[4])
<< ", " << toDegrees(set->theta[5]) << "] " << endl;
}
void ArmContactModelProvider::checkArm(bool left, float factor)
{
Vector2f retVal;
/* Calculate arm diffs */
struct ArmAngles angles = angleBuffer[p.frameDelay];
retVal.x = left
? theFilteredJointData.angles[JointData::LShoulderPitch] - angles.leftX
: theFilteredJointData.angles[JointData::RShoulderPitch] - angles.rightX;
retVal.y = left
? theFilteredJointData.angles[JointData::LShoulderRoll] - angles.leftY
: theFilteredJointData.angles[JointData::RShoulderRoll] - angles.rightY;
retVal *= factor;
if(left)
{
ARROW("module:ArmContactModelProvider:armContact", 0, 0, -(toDegrees(retVal.y) * SCALE), toDegrees(retVal.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::blue);
leftErrorBuffer.add(retVal);
}
else
{
ARROW("module:ArmContactModelProvider:armContact", 0, 0, (toDegrees(retVal.y) * SCALE), toDegrees(retVal.x) * SCALE, 20, Drawings::ps_solid, ColorClasses::blue);
rightErrorBuffer.add(retVal);
}
}
void HodginDidItApp::setupCiCamera()
{
PXCPointF32 cFOV;
mPXC.QueryCapture()->QueryDevice()->QueryPropertyAsPoint(PXCCapture::Device::PROPERTY_DEPTH_FOCAL_LENGTH,&cFOV);
mFOV.x = math<float>::atan(mDepthW/(cFOV.x*2))*2;
mFOV.y = math<float>::atan(mDepthH/(cFOV.y*2))*2;
mNIFactors.x = math<float>::tan(mFOV.x/2.0f)*2.0f;
mNIFactors.y = math<float>::tan(mFOV.y/2.0f)*2.0f;
float cVFov = (float)toDegrees(mFOV.y);
float cAspect = getWindowAspectRatio();
mCamera.setPerspective(cVFov,cAspect,0.1f,100.0f);
mCamera.lookAt(Vec3f(0,0,0), Vec3f::zAxis(), Vec3f::yAxis());
}
void CameraIntrinsics::serialize(In* in, Out* out)
{
float upperOpeningAngleWidth = toDegrees(this->upperOpeningAngleWidth);
float upperOpeningAngleHeight = toDegrees(this->upperOpeningAngleHeight);
float lowerOpeningAngleWidth = toDegrees(this->lowerOpeningAngleWidth);
float lowerOpeningAngleHeight = toDegrees(this->lowerOpeningAngleHeight);
STREAM_REGISTER_BEGIN;
STREAM(upperOpeningAngleWidth);
STREAM(upperOpeningAngleHeight);
STREAM(upperOpticalCenter);
STREAM(lowerOpeningAngleWidth);
STREAM(lowerOpeningAngleHeight);
STREAM(lowerOpticalCenter);
STREAM_REGISTER_FINISH;
if(in)
{
this->upperOpeningAngleWidth = fromDegrees(upperOpeningAngleWidth);
this->upperOpeningAngleHeight = fromDegrees(upperOpeningAngleHeight);
this->lowerOpeningAngleWidth = fromDegrees(lowerOpeningAngleWidth);
this->lowerOpeningAngleHeight = fromDegrees(lowerOpeningAngleHeight);
}
}
double F16FlightModel::calculateYawMoment() const {
const double p = -m_AngularVelocityBody.y();
/**
* negative since yaw axis (and hence yaw rate) is opposite nasa 1979.
*/
const double r = -m_AngularVelocityBody.z();
/**
* cn_da = dcn,da + dcn,da,lef * (1 - dlef/25)
*/
double cn_da = m_Cn_da_a_b[m_Alpha][(m_Aileron > 0) ? m_Beta : -m_Beta];
cn_da += m_ScaleLEF * (m_Cn_da_lef_a_b[m_Alpha][(m_Aileron > 0) ? m_Beta : -m_Beta] - cn_da);
double cn_p = (m_Cn_p_a[m_Alpha] + m_Cn_p_lef_a[m_Alpha] * m_ScaleLEF) * p;
double cn_r = (m_Cn_r_a[m_Alpha] + m_Cn_r_lef_a[m_Alpha] * m_ScaleLEF) * r;
double cn_dr = m_Cn_dr_a_b[m_Alpha][(m_Rudder > 0) ? m_Beta : -m_Beta];
/**
* XXX hack (testing spin control in deep stall trim) without this "boost" the yaw rate in
* a deep stall is about 45 deg/sec, compared with about 20 deg/sec cited in the nasa paper.
* this could be due to differences between the nasa data and the data used in the fm. For
* convenience, the fm data was taken from the "non-linear f-16 simulation using simulink
* and matlab" package in predigitized form. This data should be identical to the nasa data,
* but in practice there are small variations.
*/
cn_dr *= clampTo((toDegrees(m_Alpha) - 40) * 0.8, 1.0, 20.0);
cn_dr = -fabs(cn_dr);
/**
* note that these offsets are in internal body coordinates, not fm coordinates.
*/
double offset_y = m_CenterOfMassBody.y() - m_CL_x_m[0.0]; /** @todo should this vary with mach? */
double offset_x = m_CenterOfMassBody.x();
double moment =
m_Cn_de_a_b[m_Elevator][m_Alpha][m_Beta] +
m_Cn_lef_a_b[m_Alpha][m_Beta] * m_ScaleLEF +
(cn_p + cn_r) * m_WingSpan * m_Inv2V +
cn_da * m_Aileron +
cn_dr * m_Rudder +
(offset_x * m_CX - offset_y * m_CY) / m_WingSpan;
/**
* negative since yaw axis (and hence yaw moment) is opposite nasa 1979.
*/
return -moment * m_qBarS * m_WingSpan;
}
float BH2011GoalSymbols::getAngleToLastSeen()
{
Vector2<int> position;
unsigned int maxLastSeen = 0;
for(int i = 0; i < GoalPercept::NUMBER_OF_GOAL_POSTS; ++i)
if(goalPercept.posts[i].timeWhenLastSeen > maxLastSeen)
{
maxLastSeen = goalPercept.posts[i].timeWhenLastSeen;
position = goalPercept.posts[i].positionOnField;
}
for(int i = 0; i < GoalPercept::NUMBER_OF_UNKNOWN_GOAL_POSTS; ++i)
if(goalPercept.posts[i].timeWhenLastSeen > maxLastSeen)
{
maxLastSeen = goalPercept.posts[i].timeWhenLastSeen;
position = goalPercept.posts[i].positionOnField;
}
return toDegrees(Vector2<>(float(position.x), float(position.y)).angle());
}
float BH2011GoalSymbols::getCenterAngleOfFreePart()
{
Pose2D poseForOppGoalAngle = robotPose;
Vector2<> leftOppGoalPostAbs = Vector2<>((float) fieldDimensions.xPosOpponentGroundline, (float) fieldDimensions.yPosLeftGoal);
Vector2<> rightOppGoalPostAbs = Vector2<>((float) fieldDimensions.xPosOpponentGroundline, (float) fieldDimensions.yPosRightGoal);
if(poseForOppGoalAngle.translation.x > float(fieldDimensions.xPosOpponentGroundline - 50))
{
poseForOppGoalAngle.translation.x = float(fieldDimensions.xPosOpponentGroundline - 50);
}
float leftOppGoalPostAngle = Geometry::angleTo(poseForOppGoalAngle, leftOppGoalPostAbs);
float rightOppGoalPostAngle = Geometry::angleTo(poseForOppGoalAngle, rightOppGoalPostAbs);
if(leftOppGoalPostAngle < rightOppGoalPostAngle)
{
leftOppGoalPostAngle += pi2;
}
Vector2<> centerOfFreePart = (freePartOfOpponentGoalModel.leftEnd + freePartOfOpponentGoalModel.rightEnd) / 2.0f;
float angleToCenterOfFreePart = Range<>(rightOppGoalPostAngle - getAngleToleranceToFreePart(), leftOppGoalPostAngle + getAngleToleranceToFreePart()).limit(centerOfFreePart.angle());
return toDegrees(normalize(angleToCenterOfFreePart));
}
/**
* Function calculates forward azimuth (initial bearing) from first provided coordinates to second.
* @param first - initial coordinates in tCoords structure
* @param second - final coordinates in tCoords structure
* @return initial bearing from first to second position in decimal degrees
*/
double getBearing(tCoords first, tCoords second)
{
double deltaLon = toRadians(second.lon) - toRadians(first.lon);
double dPhi = log(tan(toRadians(second.lat) / 2.0 + M_PI / 4.0) / tan(toRadians(first.lat) / 2.0 + M_PI / 4.0));
if (abs(deltaLon) > M_PI)
{
if (deltaLon > 0.0)
{
deltaLon = -(2.0 * M_PI - deltaLon);
}
else
{
deltaLon = (2.0 * M_PI - deltaLon);
}
}
return (std::fmod((toDegrees(atan2(deltaLon, dPhi)) + 360.0), 360.0));
}
void abb_file_suite::AbbMotionFtpDownloader::handleJointTrajectory(const trajectory_msgs::JointTrajectory &traj)
{
ROS_INFO_STREAM("Trajectory received");
// generate temporary file with appropriate rapid code
std::ofstream ofh ("/tmp/mGodel_blend.mod");
if (!ofh)
{
ROS_WARN_STREAM("Could not create file");
return;
}
std::vector<rapid_emitter::TrajectoryPt> pts;
pts.reserve(traj.points.size());
for (std::size_t i = 0; i < traj.points.size(); ++i)
{
std::vector<double> tmp = toDegrees(traj.points[i].positions);
if (j23_coupled_) linkageAdjust(tmp);
double duration = 0.0;
// Timing
if (i > 0)
{
duration = (traj.points[i].time_from_start - traj.points[i-1].time_from_start).toSec();
}
rapid_emitter::TrajectoryPt pt (tmp, duration);
pts.push_back(pt);
}
rapid_emitter::ProcessParams params;
params.wolf = false;
rapid_emitter::emitJointTrajectoryFile(ofh, pts, params);
ofh.close();
// send to controller
if (!uploadFile(ip_ + "/PARTMODULES", "/tmp/mGodel_blend.mod"))
{
ROS_WARN("Could not upload joint trajectory to remote ftp server");
}
}
//Uses law of sines to solve a triangle, with the user providing angle-side-side
double solveTriangle(double _a, double _A, double _B){
double c;
double b;
double a = _a;
double C;
double B = _B;
double A = _A;
double ae = sin(toRadians(a))/A;
double be;
double ce;
b = toDegrees(asin(ae*B));
be = sin(toRadians(b))/B;
c = 180-a-b;
C = sin(toRadians(c))/be;
//C*C is the area of the rectangle for whichever triangle was being solved
return C*C;
}
double Waypoint::bearingGCInitTo(Waypoint wp, int scale)
{
double ret = 0.0;
double dlatRad = toRadians(wp.lat - lat);
double dlonRad = toRadians(wp.lon - lon);
double latOrgRad = toRadians(lat);
double lonOrgRad = toRadians(lon);
double y = sin(dlonRad) * cos(toRadians(wp.lat));
double x = cos(latOrgRad) * sin(toRadians(wp.lat))
- sin(latOrgRad)
* cos(toRadians(wp.lat))
* cos(dlonRad);
ret = toDegrees(atan2(y, x));
ret = fmod((ret + 360.0), 360.0);
return ret;
}
/*
void TorsoMatrixProvider::update(FilteredOdometryOffset& odometryOffset)
{
Pose2DBH odometryOffset;
if(lastTorsoMatrix.translation.z != 0.)
{
Pose3DBH odometryOffset3D(lastTorsoMatrix);
odometryOffset3D.conc(theTorsoMatrixBH.offset);
odometryOffset3D.conc(theTorsoMatrixBH.invert());
odometryOffset.translation.x = odometryOffset3D.translation.x;
odometryOffset.translation.y = odometryOffset3D.translation.y;
odometryOffset.rotation = odometryOffset3D.rotation.getZAngle();
}
PLOT("module:TorsoMatrixProvider:odometryOffsetX", odometryOffset.translation.x);
PLOT("module:TorsoMatrixProvider:odometryOffsetY", odometryOffset.translation.y);
PLOT("module:TorsoMatrixProvider:odometryOffsetRotation", toDegrees(odometryOffset.rotation));
(Pose3DBH&)lastTorsoMatrix = theTorsoMatrixBH;
}
*/
void TorsoMatrixProvider::update(OdometryDataBH& odometryData)
{
Pose2DBH odometryOffset;
if(lastTorsoMatrix.translation.z != 0.)
{
Pose3DBH odometryOffset3D(lastTorsoMatrix);
odometryOffset3D.conc(theTorsoMatrixBH.offset);
odometryOffset3D.conc(theTorsoMatrixBH.invert());
odometryOffset.translation.x = odometryOffset3D.translation.x;
odometryOffset.translation.y = odometryOffset3D.translation.y;
odometryOffset.rotation = odometryOffset3D.rotation.getZAngle();
}
PLOT("module:TorsoMatrixProvider:odometryOffsetX", odometryOffset.translation.x);
PLOT("module:TorsoMatrixProvider:odometryOffsetY", odometryOffset.translation.y);
PLOT("module:TorsoMatrixProvider:odometryOffsetRotation", toDegrees(odometryOffset.rotation));
odometryData += odometryOffset;
(Pose3DBH&)lastTorsoMatrix = theTorsoMatrixBH;
}
double Waypoint::bearingRhumbTo(Waypoint wp, int scale)
{
double latDiff = toRadians(wp.lat - lat); // Δφ
double lonDiff = toRadians(wp.lon - lon); // Δλ
double lat1R = toRadians(lat); // Original latitude
double lat2R = toRadians(wp.lat); // Desitination latitude
double projectedLatDiff =
log(
tan(pi / 4 + lat2R / 2) / tan(pi / 4 + lat1R / 2)
); // Δψ
double q =
abs(projectedLatDiff) > 10 * exp(-12) ?
latDiff / projectedLatDiff : cos(lat1R);
// if dLon over 180° take shorter rhumb across anti-meridian:
if (abs(lonDiff) > pi)
lonDiff = lonDiff > 0 ? -(2 * pi - lonDiff) : (2 * pi + lonDiff);
double rhumbBearing = toDegrees(atan2(lonDiff, projectedLatDiff));
while ((rhumbBearing < 0) || (rhumbBearing > 360))
{
if (rhumbBearing < 0)
{
rhumbBearing = 360 + rhumbBearing;
}
else if (rhumbBearing > 360)
{
rhumbBearing = 360 - rhumbBearing;
}
}
return rhumbBearing;
}
