Slerp interpolation of angle results in -nan(ind) - c++

I'm trying to interpolate a 2D angle and it works 99.9% of the time. For some reason I'm getting -nan(ind) for some values, like:
lastAngle = -0.0613451
currentAngle = -0.061421
alpha = 0.218813
This is the code:
inline float slerpRotation(const float& angle1, const float& angle2, const float& alpha)
{
auto v1 = b2Vec2{std::cos(angle1), std::sin(angle1)};
auto v2 = b2Vec2{std::cos(angle2), std::sin(angle2)};
auto v = this->slerp(v1, v2, alpha);
return std::atan2(v.y, v.x);
}
inline b2Vec2 slerp(const b2Vec2& v1, const b2Vec2& v2, const float& alpha)
{
auto cosAngle = v1.x * v2.x + v1.y * v2.y;
auto angle = std::acos(cosAngle);
auto angleAlpha = angle * alpha;
auto v3 = (v2 - (cosAngle * v1)).Normalize();
auto x = v1.x * std::cos(angleAlpha) + v3 * std::sin(angleAlpha);
auto y = v1.y * std::cos(angleAlpha) + v3 * std::sin(angleAlpha);
return b2Vec2{x, y};
}
All this examples results in inf num:
slerpRotation(-0.0613451f, -0.061421f, 0.218813f);
slerpRotation(-1.63139f, -1.63139f, 0.723703f);
slerpRotation(-0.0614404f, -0.0614034f, 0.199831f);
slerpRotation(0.0194162f, 0.0194164f, 0.259074f);
I've tried to solve this problem for a while now without knowing what causes this problem, do you guys happened to know how to solve this?

In the end you are computing
angle1+alpha*(angle2-angle1)
or if you want to exclude some fringe cases,
angle1+alpha*reduce2pi(angle2-angle1)
where
reduce2pi(phi) = fmod( 3*pi + fmod(phi, 2*pi), 2*pi)-pi
Note that these formulas are completely singularity free, as there is no division. It is not necessary to switch forth and back between angles and their point on the unit circle.
In code, that would be
inline float slerpRotation(const float& angle1, const float& angle2, const float& alpha)
{
auto angleDiff = angle2-angle1;
angleDiff = std::fmod(angleDiff, 2*std::M_PI);
angleDiff = std::fmod(angleDiff + 3*std::M_PI, 2*std::M_PI)-std::M_PI;
return angle1+alpha*angleDiff;
}
(12/13/2016) combining several comments: If you insist on using exactly this interface structure, then you can get a singularity free method as follows:
inline b2Vec2 slerp(const b2Vec2& v1, const b2Vec2& v2, const float& alpha)
{
auto angle = std::atan2(v1.x*v2.y - v1.y*v2.x, v1.x*v2.x + v1.y*v2.y);
auto angleAlpha = angle * alpha;
auto v3=b2Vec2{-v1.y, v1.x}; // rotation by 90°
return std::cos(angleAlpha)*v1 + std::sin(angleAlpha)*v3;
}

Related

Using neon/simd to optimize Vector3 class

I'd like to know if it is worth it optimizing my Vector3 class' operations with neon/simd like I did to my Vector2 class.
As far as I know, simd can only handle two or four floats at the same time, so to my Vector3 we would need something like this:
Vector3 Vector3::operator * (const Vector3& v) const
{
#if defined(__ARM_NEON__)
// extra step: allocate a fourth float
const float v4A[4] = {x, y, z, 0};
const float v4B[4] = {v.x, v.y, v.z, 0};
float32x4_t r = vmul_f32(*(float32x4_t*)v4A, *(float32x4_t*)v4B);
return *(Vector3*)&r;
#else
return Vector3(x * v.x, y * v.y, z * v.z);
#endif
}
Is this safe? Would this extra step still be faster than a non-simd code on most scenarios (say arm64 for instance)?

How to apply Affine3d to Vector3d

I have a Vector3d containing a list of vertices and I'm trying to apply a rotation on it, but can't find a way to do so:
rotationArbitraryAxis(const Vector3d axis, const double angle) {
Vector3d normAxis = axis.normalized();
double radians = angle * M_PI / 180.0f;
AngleAxisd aa(radians, normAxis);
Affine3d fTransform = Translation3d(normAxis) * aa * Translation3d(-normAxis);
const double* data = reinterpret_cast<const double*>(m_mesh.points().data());
Vector3d verts(m_mesh.number_of_vertices());
std::copy(data, data + m_mesh.number_of_vertices() * 3, verts.data());
verts *= ???
}
Any ideas?
As i said in my comment, convert the Affine3d to a matrix, and use that to transform the vertices.
const double* data = reinterpret_cast<const double*>(m_mesh.points().data());
// I'm assuming you want this as an array of vector3's ???
std::vector<Vector3d> verts(m_mesh.number_of_vertices());
std::copy(data, data + m_mesh.number_of_vertices() * 3, verts.data());
// convert the transform to a matrix
auto matrix = fTransform.matrix();
for(auto& vert : verts)
{
// now multiply
vert = matrix * vert;
}

Determine affine transformation that transform one plane into a parallel plane to another

How i can determine the CGAL affine transformation (Aff_transformation_3) that transform one plane (plane1) into a parallel plane to another (plane2)?
Suppose that i have two object planes:
Plane_3 pl1;
Plane_3 pl2;
and they are not parallels, how determine this kind of affine transformation?
Aff_transformation_3 t3 = ??? (pl1, pl2);
I consulted this question and your answer: CGAL: Transformation Matrix for Rotation given two lines/vectors/directions, but i don't know how it may helpme. I have two planes, but in 3d dimensions.
Thanks.
I don't know how a 2d affine transformation (Aff_transformation_2) may helpme to apply a 3d affine transformation (Aff_transformation_3).
However, i found the solution to my question. This is may bit code that i hope to help someone.
typedef CGAL::Cartesian<double> KC;
typedef KC::Line_3 Line3;
typedef KC::Vector_3 Vector3;
typedef KC::Plane_3 Plane3;
typedef CGAL::Aff_transformation_3<KC> Transform3;
// forwards
struct axis_angle;
typedef boost::shared_ptr<axis_angle> RAxisAngle;
struct axis_angle
{
axis_angle()
{
angle = 0;
axis = Vector3(0.0, 0.0, 0.0);
}
double angle;
Vector3 axis;
};
Vector3 normalize(const Vector3 &v)
{
ldouble len = ::sqrt(v.squared_length());
if (len == 0.0)
return v;
return v / len;
}
// return the angle and axis from two planes that there are not parallels
RAxisAngle axis_angle_from_planes(const Plane3 &pln1, const Plane3 &pln2)
{
RAxisAngle result = RAxisAngle(new axis_angle());
Vector3 norm1 = pln1.orthogonal_vector();
Vector3 norm2 = pln2.orthogonal_vector();
double dot_r = norm1 * norm2;
double len_r = ::sqrt(norm1.squared_length() * norm2.squared_length());
if (len_r)
result->angle = ::acos(dot_r / len_r);
else
result->angle = 0.0;
Line3 l1;
CGAL::Object obj_cgal = CGAL::intersection(pln1, pln2);
if (CGAL::assign(l1, obj_cgal))
{
result->axis = normalize(l1.to_vector());
}
else
{
// when planes are parallels, then use some basic axis
result->axis = Vector3(1.0, 0.0, 0.0);
}
return result;
}
// return a CGAL affine transformation that is builded from a 3x3 matrix
// this transformation is for rotate an object from axis and angle
// http://en.wikipedia.org/wiki/Transformation_matrix
// http://en.wikipedia.org/wiki/Rotation_matrix
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
Transform3 axis_angle_to_matrix(const RAxisAngle &aa)
{
double tmp1, tmp2;
double c = ::cos(aa->angle);
double s = ::sin(aa->angle);
double t = 1.0 - c;
double m00 = c + aa->axis.x() * aa->axis.x() * t;
double m11 = c + aa->axis.y() * aa->axis.y() * t;
double m22 = c + aa->axis.z() * aa->axis.z() * t;
tmp1 = aa->axis.x() * aa->axis.y() * t;
tmp2 = aa->axis.z() * s;
double m10 = tmp1 + tmp2;
double m01 = tmp1 - tmp2;
tmp1 = aa->axis.x() * aa->axis.z() * t;
tmp2 = aa->axis.y() * s;
double m20 = tmp1 - tmp2;
double m02 = tmp1 + tmp2;
tmp1 = aa->axis.y() * aa->axis.z() * t;
tmp2 = aa->axis.x() * s;
double m21 = tmp1 + tmp2;
double m12 = tmp1 - tmp2;
return Transform3(m00, m01, m02, m10, m11, m12, m20, m21, m22);
}
Then, i can use there as this:
RAxisAngle aa = axis_angle_from_planes(plane1, plane2);
Transform3 t3 = axis_angle_to_matrix(aa);
Plane2 new_transform_plane = plane1.transform(t3);
or maybe a point of this plane:
Point3 new_transform_point = point_of_plane1.transform(t3);
Thanks for giveme the posibility to post my little solution.

Build Circle from 3 Points in 3D space implementation in C or C++

We have 3(three) xyz points that define a circle in 3D space, this circle needs to be converted into a polyline(for further rendering). I'm looking for a ready C or C++ function or free library that can do the job.
Don't understand why this was closed. And I can't even answer my own question there. Shame on you guys. But you will not stop the knowledge spreading!
There's a much simpler solution to find the circle parameters in real 3D, just take a look at the "barycentric coordinates" section in http://en.wikipedia.org/wiki/Circumscribed_circle .
You can extract the following optimized code from that:
// triangle "edges"
const Vector3d t = p2-p1;
const Vector3d u = p3-p1;
const Vector3d v = p3-p2;
// triangle normal
const Vector3d w = t.crossProduct(u);
const double wsl = w.getSqrLength();
if (wsl<10e-14) return false; // area of the triangle is too small (you may additionally check the points for colinearity if you are paranoid)
// helpers
const double iwsl2 = 1.0 / (2.0*wsl);
const double tt = t*t;
const double uu = u*u;
// result circle
Vector3d circCenter = p1 + (u*tt*(u*v) - t*uu*(t*v)) * iwsl2;
double circRadius = sqrt(tt * uu * (v*v) * iwsl2*0.5);
Vector3d circAxis = w / sqrt(wsl);
You can then calculate the points on the circle in real 3D too and e.g. draw them using GL_LINE_STRIP in OpenGL. This should be much faster than using the 2D sin/cos approach.
// find orthogonal vector to the circle axis
const Vector3d an = circAxis.getNormalized();
const Vector3d ao = Vector3d(4.0+an[0], 4.0+an[0]+an[1], 4.0+an[0]+an[1]+an[2]).crossProduct(an).getNormalized();
// 4x4 rotation matrix around the circle axis
const int steps = 360; // maybe adjust according to circle size on screen
Matrix4d R = makeRotMatrix4d(circCenter, circAxis, 2.0*M_PI/double(steps));
// one point on the circle
Vector3d cp = circCenter + ao*circRadius;
// rotate point on the circle
for (int i=0; i<steps; ++i)
{
circlePoints.push_back(cp);
cp = transformPoint(cp, R); // apply the matrix
}
For the creation of the transformation matrix (i.e. makeRotMatrix4d()) see http://paulbourke.net/geometry/rotate/ for example.
Please note that I did not test if the above code really compiles, but it should give you enough hints.
There is a nice article and a code sample on how to build a circle by 3 points in 2D, XY plane.
http://paulbourke.net/geometry/circlesphere/
http://paulbourke.net/geometry/circlesphere/Circle.cpp
To build a 3D circle we'll have to:
rotate our 3 points into XY plane
Calculate circle center
build a circle in XY plane using the code in the article
rotate it back into it's original plane
For rotations it is best to use quaternions.
To find a correct quaternion I looked at Ogre3d source code:
void Quaternion::FromAngleAxis (const Radian& rfAngle, const Vector3& rkAxis)
There is one more useful function there:
Quaternion getRotationTo(const Vector3& dest, const Vector3& fallbackAxis = Vector3::ZERO) const
But I didn't use it.
For quaterions and vectors I used our own classes. Here is the full source code of the function that does the job:
bool IsPerpendicular(Point3d *pt1, Point3d *pt2, Point3d *pt3);
double CalcCircleCenter(Point3d *pt1, Point3d *pt2, Point3d *pt3, Point3d *center);
void FindCircleCenter(const Point3d *V1, const Point3d *V2, const Point3d *V3, Point3d *center)
{
Point3d *pt1=new Point3d(*V1);
Point3d *pt2=new Point3d(*V2);
Point3d *pt3=new Point3d(*V3);
if (!IsPerpendicular(pt1, pt2, pt3) ) CalcCircleCenter(pt1, pt2, pt3, center);
else if (!IsPerpendicular(pt1, pt3, pt2) ) CalcCircleCenter(pt1, pt3, pt2, center);
else if (!IsPerpendicular(pt2, pt1, pt3) ) CalcCircleCenter(pt2, pt1, pt3, center);
else if (!IsPerpendicular(pt2, pt3, pt1) ) CalcCircleCenter(pt2, pt3, pt1, center);
else if (!IsPerpendicular(pt3, pt2, pt1) ) CalcCircleCenter(pt3, pt2, pt1, center);
else if (!IsPerpendicular(pt3, pt1, pt2) ) CalcCircleCenter(pt3, pt1, pt2, center);
else {
delete pt1;
delete pt2;
delete pt3;
return;
}
delete pt1;
delete pt2;
delete pt3;
}
bool IsPerpendicular(Point3d *pt1, Point3d *pt2, Point3d *pt3)
// Check the given point are perpendicular to x or y axis
{
double yDelta_a= pt2->y - pt1->y;
double xDelta_a= pt2->x - pt1->x;
double yDelta_b= pt3->y - pt2->y;
double xDelta_b= pt3->x - pt2->x;
// checking whether the line of the two pts are vertical
if (fabs(xDelta_a) <= 0.000000001 && fabs(yDelta_b) <= 0.000000001){
return false;
}
if (fabs(yDelta_a) <= 0.0000001){
return true;
}
else if (fabs(yDelta_b) <= 0.0000001){
return true;
}
else if (fabs(xDelta_a)<= 0.000000001){
return true;
}
else if (fabs(xDelta_b)<= 0.000000001){
return true;
}
else
return false ;
}
double CalcCircleCenter(Point3d *pt1, Point3d *pt2, Point3d *pt3, Point3d *center)
{
double yDelta_a = pt2->y - pt1->y;
double xDelta_a = pt2->x - pt1->x;
double yDelta_b = pt3->y - pt2->y;
double xDelta_b = pt3->x - pt2->x;
if (fabs(xDelta_a) <= 0.000000001 && fabs(yDelta_b) <= 0.000000001){
center->x= 0.5*(pt2->x + pt3->x);
center->y= 0.5*(pt1->y + pt2->y);
center->z= pt1->z;
return 1;
}
// IsPerpendicular() assure that xDelta(s) are not zero
double aSlope=yDelta_a/xDelta_a; //
double bSlope=yDelta_b/xDelta_b;
if (fabs(aSlope-bSlope) <= 0.000000001){ // checking whether the given points are colinear.
return -1;
}
// calc center
center->x= (aSlope*bSlope*(pt1->y - pt3->y) + bSlope*(pt1->x + pt2 ->x)
- aSlope*(pt2->x+pt3->x) )/(2* (bSlope-aSlope) );
center->y = -1*(center->x - (pt1->x+pt2->x)/2)/aSlope + (pt1->y+pt2->y)/2;
return 1;
}
//! Builds a circle in 3D space by 3 points on it and an optional center
void buildCircleBy3Pt(const float *pt1,
const float *pt2,
const float *pt3,
const float *c, // center, can be NULL
std::vector<float> *circle)
{
/* Get the normal vector to the triangle formed by 3 points
Calc a rotation quaternion from that normal to the 0,0,1 axis
Rotate 3 points using quaternion. Points will be in XY plane
Build a circle by 3 points on XY plane
Rotate a circle back into original plane using quaternion
*/
Point3d p1(pt1[0], pt1[1], pt1[2]);
Point3d p2(pt2[0], pt2[1], pt2[2]);
Point3d p3(pt3[0], pt3[1], pt3[2]);
Point3d center;
if (c)
{
center.set(c[0], c[1], c[2]);
}
const Vector3d p2top1 = p1 - p2;
const Vector3d p2top3 = p3 - p2;
const Vector3d circle_normal = p2top1.crossProduct(p2top3).normalize();
const Vector3d xy_normal(0, 0, 1);
Quaternion rot_quat;
// building rotation quaternion
{
// Rotation axis around which we will rotate our circle into XY plane
Vector3d rot_axis = xy_normal.crossProduct(circle_normal).normalize();
const double rot_angle = xy_normal.angleTo(circle_normal); // radians
const double w = cos(rot_angle * 0.5);
rot_axis *= sin(rot_angle * 0.5);
rot_quat.set(w, rot_axis.x, rot_axis.y, rot_axis.z);
}
Quaternion rot_back_quat;
// building backward rotation quaternion, same as prev. but -angle
{
const double rot_angle = -(xy_normal.angleTo(circle_normal)); // radians
const double w_back = cos(rot_angle * 0.5);
Vector3d rot_back_axis = xy_normal.crossProduct(circle_normal).normalize();
rot_back_axis *= sin(rot_angle * 0.5);
rot_back_quat.set(w_back, rot_back_axis.x, rot_back_axis.y, rot_back_axis.z);
}
rot_quat.rotate(p1);
rot_quat.rotate(p2);
rot_quat.rotate(p3);
rot_quat.rotate(center);
if (!c)
{
// calculate 2D center
FindCircleCenter(&p1, &p2, &p3, &center);
}
// calc radius
const double radius = center.distanceTo(p1);
const float DEG2RAD = 3.14159f / 180.0f;
// build circle
for (int i = 0; i < 360; ++i)
{
float degInRad = i * DEG2RAD;
Point3d pt(cos(degInRad) * radius + center.x, sin(degInRad) * radius + center.y, 0);
// rotate the point back into original plane
rot_back_quat.rotate(pt);
circle->push_back(pt.x);
circle->push_back(pt.y);
circle->push_back(pt.z);
}
}
The following is the C#/Unity port of Mark's answer. It uses types and utility functions from Unity's scripting API.
// triangle "edges"
var t = p2 - p1;
var u = p3 - p1;
var v = p3 - p2;
// triangle normal
var w = Vector3.Cross(t, u);
var wsl = Vector3.Dot(w, w);
// TODO: if (wsl<10e-14) return false; // area of the triangle is too small (you may additionally check the points for colinearity if you are paranoid)
// helpers
var iwsl2 = 1f / (2f * wsl);
var tt = Vector3.Dot(t, t);
var uu = Vector3.Dot(u, u);
// result circle
Vector3 circCenter = p1 + (u * tt * (Vector3.Dot(u, v)) - t * uu * (Vector3.Dot(t, v))) * iwsl2;
var circRadius = Mathf.Sqrt(tt * uu * (Vector3.Dot(v, v)) * iwsl2 * 0.5f);
Vector3 circAxis = w / Mathf.Sqrt(wsl);
Using Unity's Gizmos, the circle can be drawn as follows (using 30 line segments to approximate it in this case):
// Draw the circle:
Gizmos.color = Color.white;
for (int i = 0; i < 30; ++i)
{
Gizmos.DrawLine(
circCenter + Quaternion.AngleAxis(360f / 30f * i , circAxis) * (p1 - circCenter),
circCenter + Quaternion.AngleAxis(360f / 30f * (i + 1), circAxis) * (p1 - circCenter)
);
}
The result looks like follows for vertex positions var p1 = new Vector3(0f, 1.44f, 0f); var p2 = new Vector3(0f, 0.73f, 0.65f); var p3 = new Vector3(0f, -1.04f, 0f);:

how do I translate of lon/lat coordinate by some N-E meters distance on earth surface?

how do I get a new coordinate in geodetic (Lat/Lon) from a reference point (which is in geodetic) after some translation (in meters) on earth surface, and also I need to do the calculation using true earth ellipsoid model such as WGS84.
for example:
suppose I have reference point of 10.32E, -4.31N
then I do translation of (3000,-2000) meters ( which is move the point 3000 meters to east and 2000 meters to south on earth surface.
then I need the coordinate of new point in geodetic.
thank you
Have a look at the open-source library PROJ.4 which you can use to accurately translate geographic coordinates (lat/long) to projected coordinates (metres), and back again. In your case you can project into WGS 84 / World Mercator (EPSG:3395), perform the translation in metres, then un-project back to geographic.
found the answer :
http://www.movable-type.co.uk/scripts/latlong-vincenty-direct.html
from:
Vincenty direct formula - T Vincenty, "Direct and Inverse Solutions of Geodesics on the
Ellipsoid with application of nested equations", Survey Review, vol XXII no 176, 1975
http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
This code calculates the distance (N and E) between two points given lat/lon coordinates. You can easily reverse it for your purposes.
Take a look at function
u8 GPS_CalculateDeviation()
in
http://svn.mikrokopter.de/filedetails.php?repname=NaviCtrl&path=/tags/V0.15c/GPS.c
You either find some geo-library, or do the trigonometry yourself.
In any case you should formulate your question more exactly. In particular you say:
then I do translation of (3000,-2000) meters ( which is move the point 3000 meters to east and 2000 meters to south on earth surface.
You should note that moving by 3km to east and then 2km to south differs from moving 2km to south and then 3km to east. Those are not commutative actions. So that calling this by offsetting (3000, -2000) is incorrect.
Below is C++ code slighly modified from original version from ETH Zurich. The file only has dependency on Eigen library (which can be eliminated with some trivial work if required by writing matrix multiplication function yourself). You can use geodetic2ned() function to convert latitude, longitude, altitude to NED frame.
//GeodeticConverter.hpp
#ifndef air_GeodeticConverter_hpp
#define air_GeodeticConverter_hpp
#include <math>
#include <eigen3/Eigen/Dense>
class GeodeticConverter
{
public:
GeodeticConverter(double home_latitude = 0, double home_longitude = 0, double home_altitude = 0)
: home_latitude_(home_latitude), home_longitude_(home_longitude)
{
// Save NED origin
home_latitude_rad_ = deg2Rad(latitude);
home_longitude_rad_ = deg2Rad(longitude);
home_altitude_ = altitude;
// Compute ECEF of NED origin
geodetic2Ecef(latitude, longitude, altitude, &home_ecef_x_, &home_ecef_y_, &home_ecef_z_);
// Compute ECEF to NED and NED to ECEF matrices
double phiP = atan2(home_ecef_z_, sqrt(pow(home_ecef_x_, 2) + pow(home_ecef_y_, 2)));
ecef_to_ned_matrix_ = nRe(phiP, home_longitude_rad_);
ned_to_ecef_matrix_ = nRe(home_latitude_rad_, home_longitude_rad_).transpose();
}
void getHome(double* latitude, double* longitude, double* altitude)
{
*latitude = home_latitude_;
*longitude = home_longitude_;
*altitude = home_altitude_;
}
void geodetic2Ecef(const double latitude, const double longitude, const double altitude, double* x,
double* y, double* z)
{
// Convert geodetic coordinates to ECEF.
// http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
double lat_rad = deg2Rad(latitude);
double lon_rad = deg2Rad(longitude);
double xi = sqrt(1 - kFirstEccentricitySquared * sin(lat_rad) * sin(lat_rad));
*x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad);
*y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad);
*z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) * sin(lat_rad);
}
void ecef2Geodetic(const double x, const double y, const double z, double* latitude,
double* longitude, double* altitude)
{
// Convert ECEF coordinates to geodetic coordinates.
// J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
// to geodetic coordinates," IEEE Transactions on Aerospace and
// Electronic Systems, vol. 30, pp. 957-961, 1994.
double r = sqrt(x * x + y * y);
double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
double F = 54 * kSemiminorAxis * kSemiminorAxis * z * z;
double G = r * r + (1 - kFirstEccentricitySquared) * z * z - kFirstEccentricitySquared * Esq;
double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
double S = cbrt(1 + C + sqrt(C * C + 2 * C));
double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
+ sqrt(
0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
- P * (1 - kFirstEccentricitySquared) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
double U = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + z * z);
double V = sqrt(
pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * z * z);
double Z_0 = kSemiminorAxis * kSemiminorAxis * z / (kSemimajorAxis * V);
*altitude = U * (1 - kSemiminorAxis * kSemiminorAxis / (kSemimajorAxis * V));
*latitude = rad2Deg(atan((z + kSecondEccentricitySquared * Z_0) / r));
*longitude = rad2Deg(atan2(y, x));
}
void ecef2Ned(const double x, const double y, const double z, double* north, double* east,
double* down)
{
// Converts ECEF coordinate position into local-tangent-plane NED.
// Coordinates relative to given ECEF coordinate frame.
Vector3d vect, ret;
vect(0) = x - home_ecef_x_;
vect(1) = y - home_ecef_y_;
vect(2) = z - home_ecef_z_;
ret = ecef_to_ned_matrix_ * vect;
*north = ret(0);
*east = ret(1);
*down = -ret(2);
}
void ned2Ecef(const double north, const double east, const double down, double* x, double* y,
double* z)
{
// NED (north/east/down) to ECEF coordinates
Vector3d ned, ret;
ned(0) = north;
ned(1) = east;
ned(2) = -down;
ret = ned_to_ecef_matrix_ * ned;
*x = ret(0) + home_ecef_x_;
*y = ret(1) + home_ecef_y_;
*z = ret(2) + home_ecef_z_;
}
void geodetic2Ned(const double latitude, const double longitude, const double altitude,
double* north, double* east, double* down)
{
// Geodetic position to local NED frame
double x, y, z;
geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
ecef2Ned(x, y, z, north, east, down);
}
void ned2Geodetic(const double north, const double east, const double down, double* latitude,
double* longitude, double* altitude)
{
// Local NED position to geodetic coordinates
double x, y, z;
ned2Ecef(north, east, down, &x, &y, &z);
ecef2Geodetic(x, y, z, latitude, longitude, altitude);
}
void geodetic2Enu(const double latitude, const double longitude, const double altitude,
double* east, double* north, double* up)
{
// Geodetic position to local ENU frame
double x, y, z;
geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
double aux_north, aux_east, aux_down;
ecef2Ned(x, y, z, &aux_north, &aux_east, &aux_down);
*east = aux_east;
*north = aux_north;
*up = -aux_down;
}
void enu2Geodetic(const double east, const double north, const double up, double* latitude,
double* longitude, double* altitude)
{
// Local ENU position to geodetic coordinates
const double aux_north = north;
const double aux_east = east;
const double aux_down = -up;
double x, y, z;
ned2Ecef(aux_north, aux_east, aux_down, &x, &y, &z);
ecef2Geodetic(x, y, z, latitude, longitude, altitude);
}
private:
// Geodetic system parameters
static double kSemimajorAxis = 6378137;
static double kSemiminorAxis = 6356752.3142;
static double kFirstEccentricitySquared = 6.69437999014 * 0.001;
static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
static double kFlattening = 1 / 298.257223563;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::Matrix<double, 3, 3> Matrix3x3d;
inline Matrix3x3d nRe(const double lat_radians, const double lon_radians)
{
const double sLat = sin(lat_radians);
const double sLon = sin(lon_radians);
const double cLat = cos(lat_radians);
const double cLon = cos(lon_radians);
Matrix3x3d ret;
ret(0, 0) = -sLat * cLon;
ret(0, 1) = -sLat * sLon;
ret(0, 2) = cLat;
ret(1, 0) = -sLon;
ret(1, 1) = cLon;
ret(1, 2) = 0.0;
ret(2, 0) = cLat * cLon;
ret(2, 1) = cLat * sLon;
ret(2, 2) = sLat;
return ret;
}
inline double rad2Deg(const double radians)
{
return (radians / M_PI) * 180.0;
}
inline double deg2Rad(const double degrees)
{
return (degrees / 180.0) * M_PI;
}
double home_latitude_rad_, home_latitude_;
double home_longitude_rad_, home_longitude_;
double home_altitude_;
double home_ecef_x_;
double home_ecef_y_;
double home_ecef_z_;
Matrix3x3d ecef_to_ned_matrix_;
Matrix3x3d ned_to_ecef_matrix_;
}; // class GeodeticConverter
#endif