My systems consists of a coordinate frame and a quaternion in this system represents a roll (about x), pitch (about y) and yaw (about z) in the form of four values (qx, qy, qz, qw).
How can I separate this quaternion, into three independent quaternions, such that one represents the roll, the other represent pitch and the third represents yaw rotations?
Thank you.
It sounds like you should extract the Euler angles, then create 3 new quaternions and then re-combine them in the right order to arrive at the original quaternion again.
This PDF has formulas for converting between Euler and Quaternion manually. The formulas work in YPR format, rather than your RPY. Can you use them?
Here they are implemented in C:
/* euler_2_quaternion_ZYX */
quat->w = cos(az / 2.0f) * cos(ay / 2.0f) * cos(ax / 2.0f) + sin(az / 2.0f) * sin(ay / 2.0f) * sin(ax / 2.0f);
quat->x = cos(az / 2.0f) * cos(ay / 2.0f) * sin(ax / 2.0f) - sin(az / 2.0f) * sin(ay / 2.0f) * cos(ax / 2.0f);
quat->y = cos(az / 2.0f) * sin(ay / 2.0f) * cos(ax / 2.0f) + sin(az / 2.0f) * cos(ay / 2.0f) * sin(ax / 2.0f);
quat->z = sin(az / 2.0f) * cos(ay / 2.0f) * cos(ax / 2.0f) - cos(az / 2.0f) * sin(ay / 2.0f) * sin(ax / 2.0f);
/* quaternion_2_euler_ZYX */
float qx, qy, qz, qw;
qw = quat->w;
qx = quat->x;
qy = quat->y;
qz = quat->z;
*az = atan2(qy * qz + qw * qx, 0.5f - ((qx * qx) + (qy * qy)));
*ay = asin(-2.0f * ((qx * qz) - (qw * qy)));
*ax = atan2(((qx * qy) + (qw * qz)), 0.5f - ((qy * qy) + (qz * qz)));
Related
I am developing a app that use picking selection using color depth in OpenGL for Mac. The selection works great, but I observed that OpenGL not draw perfect circles (circunferences). I use GL_LINE_LOOP for render 360 segments for complete a close circunference. But if, I see the render in SELECTION mode I observe some kind of bites (triangles in white) over the circunference, 4 in total for each circle.
Is a OpenGL problem or I am doing something wrong?
This is the code for draw each circunference.
glLineWidth(10.0f); // Exaggerated for image.
glBegin(GL_LINE_LOOP);
for (int i = 0; i <= 360; i++) {
calcX = radio * cos((i * M_PI) / 180) + offset_x;
calcY = radio * sin((i * M_PI) / 180) + offset_y;
glVertex3f(calcX, calcY, offset_z);
}
glEnd();
Thanks "radical7, for the the suggest to use Triangles instead lines. I was able to obtain a perfect RING, works as smooth .
This is the code:
color = 0.00277778;
glBegin(GL_TRIANGLE_STRIP);
for (int i = 0; i <= 360; i++) {
glColor4f(color, 0, 0, 1.0f);
calcX = radio * cos((i * M_PI) / 180) + offset_x;
calcY = radio * sin((i * M_PI) / 180) + offset_y;
glVertex3f(calcX, calcY, offset_z);
radio +=20;
calcX = radio * cos((i * M_PI) / 180) + offset_x;
calcY = radio * sin((i * M_PI) / 180) + offset_y;
glVertex3f(calcX, calcY, offset_z);
radio -=20;
color += 0.00277778;
}
glEnd();
Result:
I have three CGpoint and I would like calculate the angle.
I drawn a little schema :
I tried with this code :
CGPoint u ;
u.x = 0;
u.y = - middleRectY;
CGPoint v ;
v.x = x1 - middelRectX;
v.y = y1 - middleRectY;
// formule = u.v / ( ||u|| * ||v||)
double cosa = (double)((u.x * v.x + u.y * v.y)) / sqrt(u.x * u.x + u.y * u.y) * sqrt(v.x * v.x + v.y * v.y);
// angle en degré
double angle = (180.0 / M_PI) * acos(cosa);
// Signe de l'angle
int sign = (u.x * v.y - u.y * v.x) > 0 ? 1 : -1;
rectYellow.transform = CGAffineTransformMakeRotation(angle*sign);
But my function return "nan" :/
Thx :)
I found the problem !
It just a probleme of parenthesis :
double cosa = ((u.x * v.x) + (u.y * v.y)) / (sqrt((u.x * u.x) + (u.y * u.y)) * sqrt((v.x * v.x) + (v.y * v.y)));
I don't understand why ?
Because the parentheses aren't necessary for multiplication ...
Consider points P1 (60°N, 20°E, 0) and P2 (60°N, 22°E, 0) on the
surface of the Earth
What is the shortest distance between the points P1 and P2, when the shape of the
Earth is modeled using WGS-84 ellipsoid?
Unfortunately, Vincenty's algorithm fails to converge for some inputs.
GeographicLib provides an alternative which always converges (and
is also more accurate). Implementations in C++, C, Fortran, Javascript, Python, Java, and Matlab are provided. E.g., using the
Matlab package:
format long;
geoddistance(60,20,60,22)
->
111595.753650629
As pointed out in a comment to your question, you should use Vincenty's formula for inverse problem.
Answer to your question is: 111595.75 metres (or 60.257 nautical miles).
Javascript implementation of Vincenty's inverse formula, as copied from http://jsperf.com/vincenty-vs-haversine-distance-calculations:
/**
* Calculates geodetic distance between two points specified by latitude/longitude using
* Vincenty inverse formula for ellipsoids
*
* #param {Number} lat1, lon1: first point in decimal degrees
* #param {Number} lat2, lon2: second point in decimal degrees
* #returns (Number} distance in metres between points
*/
function distVincenty(lat1, lon1, lat2, lon2) {
var a = 6378137,
b = 6356752.314245,
f = 1 / 298.257223563; // WGS-84 ellipsoid params
var L = (lon2 - lon1).toRad();
var U1 = Math.atan((1 - f) * Math.tan(lat1.toRad()));
var U2 = Math.atan((1 - f) * Math.tan(lat2.toRad()));
var sinU1 = Math.sin(U1),
cosU1 = Math.cos(U1);
var sinU2 = Math.sin(U2),
cosU2 = Math.cos(U2);
var lambda = L,
lambdaP, iterLimit = 100;
do {
var sinLambda = Math.sin(lambda),
cosLambda = Math.cos(lambda);
var sinSigma = Math.sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
if (sinSigma == 0) return 0; // co-incident points
var cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
var sigma = Math.atan2(sinSigma, cosSigma);
var sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
var cosSqAlpha = 1 - sinAlpha * sinAlpha;
var cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
if (isNaN(cos2SigmaM)) cos2SigmaM = 0; // equatorial line: cosSqAlpha=0 (§6)
var C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
lambdaP = lambda;
lambda = L + (1 - C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
} while (Math.abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);
if (iterLimit == 0) return NaN // formula failed to converge
var uSq = cosSqAlpha * (a * a - b * b) / (b * b);
var A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
var B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
var deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
var s = b * A * (sigma - deltaSigma);
s = s.toFixed(3); // round to 1mm precision
return s;
}
The Haversine Formula is commonly used (error < 0,5%)
I am trying to determine the midpoint between two locations in an MKMapView. I am following the method outlined here (and here) and rewrote it in Objective-C, but the map is being centered somewhere northeast of Baffin Island, which is no where near the two points.
My method based on the java method linked above:
+(CLLocationCoordinate2D)findCenterPoint:(CLLocationCoordinate2D)_lo1 :(CLLocationCoordinate2D)_loc2 {
CLLocationCoordinate2D center;
double lon1 = _lo1.longitude * M_PI / 180;
double lon2 = _loc2.longitude * M_PI / 100;
double lat1 = _lo1.latitude * M_PI / 180;
double lat2 = _loc2.latitude * M_PI / 100;
double dLon = lon2 - lon1;
double x = cos(lat2) * cos(dLon);
double y = cos(lat2) * sin(dLon);
double lat3 = atan2( sin(lat1) + sin(lat2), sqrt((cos(lat1) + x) * (cos(lat1) + x) + y * y) );
double lon3 = lon1 + atan2(y, cos(lat1) + x);
center.latitude = lat3 * 180 / M_PI;
center.longitude = lon3 * 180 / M_PI;
return center;
}
The 2 parameters have the following data:
_loc1:
latitude = 45.4959839
longitude = -73.67826455
_loc2:
latitude = 45.482889
longitude = -73.57522299
The above are correctly place on the map (in and around Montreal). I am trying to center the map in the midpoint between the 2, yet my method return the following:
latitude = 65.29055
longitude = -82.55425
which somewhere in the arctic, when it should be around 500 miles south.
In case someone need code in Swift, I have written library function in Swift to calculate the midpoint between MULTIPLE coordinates:
// /** Degrees to Radian **/
class func degreeToRadian(angle:CLLocationDegrees) -> CGFloat {
return ( (CGFloat(angle)) / 180.0 * CGFloat(M_PI) )
}
// /** Radians to Degrees **/
class func radianToDegree(radian:CGFloat) -> CLLocationDegrees {
return CLLocationDegrees( radian * CGFloat(180.0 / M_PI) )
}
class func middlePointOfListMarkers(listCoords: [CLLocationCoordinate2D]) -> CLLocationCoordinate2D {
var x = 0.0 as CGFloat
var y = 0.0 as CGFloat
var z = 0.0 as CGFloat
for coordinate in listCoords{
var lat:CGFloat = degreeToRadian(coordinate.latitude)
var lon:CGFloat = degreeToRadian(coordinate.longitude)
x = x + cos(lat) * cos(lon)
y = y + cos(lat) * sin(lon)
z = z + sin(lat)
}
x = x/CGFloat(listCoords.count)
y = y/CGFloat(listCoords.count)
z = z/CGFloat(listCoords.count)
var resultLon: CGFloat = atan2(y, x)
var resultHyp: CGFloat = sqrt(x*x+y*y)
var resultLat:CGFloat = atan2(z, resultHyp)
var newLat = radianToDegree(resultLat)
var newLon = radianToDegree(resultLon)
var result:CLLocationCoordinate2D = CLLocationCoordinate2D(latitude: newLat, longitude: newLon)
return result
}
Detailed answer can be found here
Updated For Swift 5
func geographicMidpoint(betweenCoordinates coordinates: [CLLocationCoordinate2D]) -> CLLocationCoordinate2D {
guard coordinates.count > 1 else {
return coordinates.first ?? // return the only coordinate
CLLocationCoordinate2D(latitude: 0, longitude: 0) // return null island if no coordinates were given
}
var x = Double(0)
var y = Double(0)
var z = Double(0)
for coordinate in coordinates {
let lat = coordinate.latitude.toRadians()
let lon = coordinate.longitude.toRadians()
x += cos(lat) * cos(lon)
y += cos(lat) * sin(lon)
z += sin(lat)
}
x /= Double(coordinates.count)
y /= Double(coordinates.count)
z /= Double(coordinates.count)
let lon = atan2(y, x)
let hyp = sqrt(x * x + y * y)
let lat = atan2(z, hyp)
return CLLocationCoordinate2D(latitude: lat.toDegrees(), longitude: lon.toDegrees())
}
}
Just a hunch, but I noticed your lon2 and lat2 variables are being computed with M_PI/100 and not M_PI/180.
double lon1 = _lo1.longitude * M_PI / 180;
double lon2 = _loc2.longitude * M_PI / 100;
double lat1 = _lo1.latitude * M_PI / 180;
double lat2 = _loc2.latitude * M_PI / 100;
Changing those to 180 might help you out a bit.
For swift users, corrected variant as #dinjas suggest
import Foundation
import MapKit
extension CLLocationCoordinate2D {
// MARK: CLLocationCoordinate2D+MidPoint
func middleLocationWith(location:CLLocationCoordinate2D) -> CLLocationCoordinate2D {
let lon1 = longitude * M_PI / 180
let lon2 = location.longitude * M_PI / 180
let lat1 = latitude * M_PI / 180
let lat2 = location.latitude * M_PI / 180
let dLon = lon2 - lon1
let x = cos(lat2) * cos(dLon)
let y = cos(lat2) * sin(dLon)
let lat3 = atan2( sin(lat1) + sin(lat2), sqrt((cos(lat1) + x) * (cos(lat1) + x) + y * y) )
let lon3 = lon1 + atan2(y, cos(lat1) + x)
let center:CLLocationCoordinate2D = CLLocationCoordinate2DMake(lat3 * 180 / M_PI, lon3 * 180 / M_PI)
return center
}
}
It's important to say that the formula the OP used to calculate geographic midpoint is based on this formula which explains the cos/sin/sqrt calculation.
This formula will give you the geographic midpoint for any long distance including the four quarters and the prime meridian.
But, if your calculation is for short-range around 1 Kilometer, using a simple average will produce the same midpoint results.
i.e:
let firstPoint = CLLocation(....)
let secondPoint = CLLocation(....)
let midPointLat = (firstPoint.coordinate.latitude + secondPoint.coordinate.latitude) / 2
let midPointLong = (firstPoint.coordinate.longitude + secondPoint.coordinate.longitude) / 2
You can actually use it for 10km but expect a deviation - if you only need an estimation for a short range midpoint with a fast solution it will be sufficient.
I think you are over thinking it a bit. Just do:
float lon3 = ((lon1 + lon2) / 2)
float lat3 = ((lat1 + lat2) / 2)
lat3 and lon3 will be the center point.
I have this query which does work fine in MySQL
SELECT ((ACOS(SIN(12.345 * PI() / 180) * SIN(lat * PI() / 180) +
COS(12.345 * PI() / 180) * COS(lat * PI() / 180) * COS((67.89 - lon) *
PI() / 180)) * 180 / PI()) * 60 * 1.1515 * 1.609344) AS distance, poi.*
FROM poi
WHERE lang='eng'
HAVING distance<='30'
distance is in Kilometers, the input is lat=12.345 and lon=67.89
The SQLite is 3, and I can't run custom functions with it as it's on Android. I also don't have acos() etc... as that is not part of the standard SQLite.
How would be the above query in SQLite?
Here is an implementation in Java for building a location based query on an Android device.
The idea comes from KennyTM (see accepted response) and implies the addition of 4 columns in your table to store values of sinus and cosinus of latitude and longitudes.
Here is the code preparing the data for a "Shop" table at insert time:
public static void injectLocationValues(ContentValues values, double latitude, double longitude) {
values.put(LocationColumns.LATITUDE, latitude);
values.put(LocationColumns.LONGITUDE, longitude);
values.put(LocationColumns.COSLAT, Math.cos(MathUtil.deg2rad(latitude)));
values.put(LocationColumns.SINLAT, Math.sin(MathUtil.deg2rad(latitude)));
values.put(LocationColumns.COSLNG, Math.cos(MathUtil.deg2rad(longitude)));
values.put(LocationColumns.SINLNG, Math.sin(MathUtil.deg2rad(longitude)));
}
public static double deg2rad(double deg) {
return (deg * Math.PI / 180.0);
}
You can then build your projection using the following function:
/**
* Build query based on distance using spherical law of cosinus
*
* d = acos(sin(lat1).sin(lat2)+cos(lat1).cos(lat2).cos(long2−long1)).R
* where R=6371 and latitudes and longitudes expressed in radians
*
* In Sqlite we do not have access to acos() sin() and lat() functions.
* Knowing that cos(A-B) = cos(A).cos(B) + sin(A).sin(B)
* We can determine a distance stub as:
* d = sin(lat1).sin(lat2)+cos(lat1).cos(lat2).(cos(long2).cos(long1)+sin(long2).sin(long1))
*
* First comparison point being fixed, sin(lat1) cos(lat1) sin(long1) and cos(long1)
* can be replaced by constants.
*
* Location aware table must therefore have the following columns to build the equation:
* sinlat => sin(radians(lat))
* coslat => cos(radians(lat))
* coslng => cos(radians(lng))
* sinlng => sin(radians(lng))
*
* Function will return a real between -1 and 1 which can be used to order the query.
* Distance in km is after expressed from R.acos(result)
*
* #param latitude, latitude of search
* #param longitude, longitude of search
* #return selection query to compute the distance
*/
public static String buildDistanceQuery(double latitude, double longitude) {
final double coslat = Math.cos(MathUtil.deg2rad(latitude));
final double sinlat = Math.sin(MathUtil.deg2rad(latitude));
final double coslng = Math.cos(MathUtil.deg2rad(longitude));
final double sinlng = Math.sin(MathUtil.deg2rad(longitude));
//#formatter:off
return "(" + coslat + "*" + LocationColumns.COSLAT
+ "*(" + LocationColumns.COSLNG + "*" + coslng
+ "+" + LocationColumns.SINLNG + "*" + sinlng
+ ")+" + sinlat + "*" + LocationColumns.SINLAT
+ ")";
//#formatter:on
}
It will inject a response column with the distance on which you need to apply the following formula to convert in kilometers:
public static double convertPartialDistanceToKm(double result) {
return Math.acos(result) * 6371;
}
If you want to order your query using the partial distance, you need to order DESC and not ASC.
Had the same issue while working on sqlite3 for ios, after playing a little with the formula here is a way to do it without using function from the sql side (pseudo-code):
Pre-calculate these value for each item you store in the database (and store them):
cos_lat = cos(lat * PI / 180)
sin_lat = sin(lat * PI / 180)
cos_lng = cos(lng * PI / 180)
sin_lng = sin(lng * PI / 180)
Pre-calculate these value at the search time (for a given position cur_lat,cur_lng)
CUR_cos_lat = cos(cur_lat * PI / 180)
CUR_sin_lat = sin(cur_lat * PI / 180)
CUR_cos_lng = cos(cur_lng * PI / 180)
CUR_sin_lng = sin(cur_lng * PI / 180)
cos_allowed_distance = cos(2.0 / 6371) # This is 2km
Your SQL query will look like this (replace CUR_* by the values you just calculated)
SELECT * FROM position WHERE CUR_sin_lat * sin_lat + CUR_cos_lat * cos_lat * (cos_lng* CUR_cos_lng + sin_lng * CUR_sin_lng) > cos_allowed_distance;
You can create 4 new columns, being sin and cos of lat and lon. Since cos(a+b) = cos a cos b - sin a sin b, and other appearances of sin and cos like SIN(12.345 * PI() / 180) can be calculated in the program before running the query, the big "distance" expression reduces to something of the form P * SIN_LAT + Q * COS_LAT + ... that can be handled by SQLite3.
BTW, see also Sqlite on Android: How to create a sqlite dist db function - to be used in the app for distance calculation using lat, long.