I'm using a Reach RS+ device to capture GPS position data as well as IMU data (roll, pitch and yaw); see the "Point Collection" image on the manufacturer's website.
I'm trying to determine the GPS coordinates of the bottom point (the empty end of the rod that the receiver is fixed on).
To be able to make calculations in meters I'm converting longitude (X) and latitude (Y) to UTM while keeping altitude (Z) unaltered.
When the rod is upright, X and Y stay the same while
Z1 = Z - ROD_LENGTH
However when the rod is tilted all coordinates are affected and I need a way to calculate the rod's end position.
I have looked at rotation matrices, triangle equations, my own sin and cos formulas based on experimental observations but I don't have a background in 3D geometry and I'm not sure which route to take (for example I don't know how to use the rod length with a rotation matrix).
Basically I need the following formulas:
X1 = X + ROD_LENGTH * func_X(roll, pitch, yaw)
Y1 = Y + ROD_LENGTH * func_Y(roll, pitch, yaw)
Z1 = Z + ROD_LENGTH * func_Z(roll, pitch, yaw)
Roll, pitch and yaw have values between -180° and 180°.
I must say, this turned out to be a lot more complex than I expected. I think I have this all straight, but let me know in comments of any corrections and I will try to fix.
Before you look at the code, below, PLEASE NOTE! The assumptions are important and you need to verify they are true in your case. There are dozens (at least!) of ways to define orientation and locations in space. The main assumpions you need to make sure are aligned with your device are the spatial frame in which we are operating. This article will give you some appreciation for why this is so important! The most obvious being how are we labelling our axes, which way is up (positive Z, as I have chosen below, but if we were talking about submarines, for instance, we might choose negative Z).
Framework assumptions: Picture an airplane (I know its not an airplane, but its easier to explain this way) with a long rod hanging straight down. We will define the Z axis as up (positive) and down (negative). The X axis points forwards (positive) and backwards (negative). The Y axis is the axis of rotation about the wings, with positive off the left wing, and negative off the right wing - this is a "right handed coordinate system". So the axes intersect is in the middle of the airplane roughly where the wings are attached. Rotation is defined as counter-clockwise around the axis for positive angles and clockwise being negative. So...
It's important to get all this right, particularly the sign (+/-) associated with your angles - try to pitch it and roll it about 30 degrees and make sure the results agree with the output - otherwise change the sign of the angle. For yaw, you will need to change both the heading and either the pitch and roll, as the heading itself will not affect the location of the end of the rod, if its straight up and down. The data you have describing the "airplane" is location (three numbers), in the same XYZ framework as described above, and the three angles (in degrees, -180 to 180), as described above.
The code:
I left in some unnecessary things (which you might need in case you need to restructure it at all) and also didn't try to make it more efficient (for example constant recalculation of the same sins and cosines) to make it a little clearer. I left in the closure compiler typing, both for a little documentation and in case you want to minify it later. rodloc is the function you want...
function presentresult(location, length, yaw, pitch, roll) {
console.log("Starting point");
console.log(location);
console.log("Rod length = " + length);
console.log("Yaw = " + yaw + ", Pitch = " + pitch + ", Roll = " + roll);
console.log("Result:");
console.log(rodloc(location, length, yaw, pitch, roll));
}
presentresult([100, 100, 100], 2, 0, 0, 0); // Result: [100, 100, 98] (3)
presentresult([100, 100, 100], 2, 30, 0, 0); // Result: [100, 100, 98] (3)
presentresult([100, 100, 100], 2, -30, 0, 0); // Result: [100, 100, 98] (3)
presentresult([100, 100, 100], 2, 0, 30, 0); // Result: [99, 100, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 0, -30, 0); // Result: [101, 100, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 0, 0, 30); // Result: [100, 101, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 0, 0, -30); // Result: [100, 99, 98.26794919243112] (3)
presentresult([100, 100, 100], 2, 30, 30, 30); // Result: [98.75, 100.43301270189222, 98.5] (3)
presentresult([100, 100, 100], 2, -30, -30, -30); // Result: [100.25, 98.70096189432334, 98.5] (3)
presentresult([100, 100, 100], 2, -30, 30, -30); // Result: [98.75, 99.56698729810778, 98.5] (3)
/** @typedef {Array<number,number,number>} */ var Vector3D;
/** @typedef {Array<Vector3D,vector3D,Vector3D>} */ var Matrix3D;
/**
* @param {Vector3D} location - The location (3 coordinates) of the "plane"
* @param {number} length - The length of the rod
* @param {number} yaw - the yaw (heading) in degrees
* @param {number} pitch - the pitch in degrees
* @param {number} roll - the roll in degrees
* @returns {Vector3D} - the location of the end of the rod
*/
function rodloc(location, length, yaw, pitch, roll) {
let ryaw = yaw * Math.PI / 180.0; // Convert to radians
let rpitch = pitch * Math.PI / 180.0;
let rroll = roll * Math.PI / 180.0;
// This is where our axes start
let x = [1, 0, 0];
let y = [0, 1, 0];
let z = [0, 0, 1];
// NOTE: ORDER MATTERS - your data may mean different things (see
// assumptions in answer!
// Rotate axes around z by yaw
let yprime = rotatearound([0, 1, 0], [0, 0, 1], ryaw);
let xprime = rotatearound([1, 0, 0], [0, 0, 1], ryaw);
let zprime = z; // rotating around itself
// Next we need to rotate for pitch (around the Y axis...)
let x2prime = rotatearound(xprime, yprime, rpitch);
let y2prime = yprime; // dont need this
let z2prime = rotatearound(zprime, yprime, rpitch);
// Now we need to roll around the new x axis...
let x3prime = x2prime // dont need this
let y3prime = rotatearound(y2prime, x2prime, rroll); // dont need this
let z3prime = rotatearound(z2prime, x2prime, rroll);
// now take what started out as [0, 0, 1] and place the end of the rod
// (at what started out as [0, 0, -length])
let rotend = [0,1,2].map(n=>-length*z3prime[n]);
// now take that and add it to the original location of the plane
// and return it as the result
return [0,1,2].map(n=>location[n]+rotend[n]);
}
/** Multiply a vector times a matrix
* @param {Vector3D} offset - The vector of the offset
* @param {Matrix3D} rotate - The rotation vector
* @returns {Vector3D} - The new offset vector
*/
function vmmult(offset, rotate) {
return [0,1,2].map(x=>xmult(offset,rotate[x]));
}
/** dot product of two vectors
* @param {Vector3D} col
* @param {Vector3D} row
* @returns {number}
*/
function xmult(col, row) {
return [0,1,2].reduce((a,c)=>a+col[c]*row[c],0);
}
/** Rotate a point around a vector projecting from the origin
* @param {Vector3D} point - the we want to rotate
* @param {Vector3D} vec - the vector (from origin to here) to rotate around
* @param {number} angle - the angle (in radians) to rotate
* @returns {Vector3D} - the new point location
*/
function rotatearound(point, vec, angle) {
let rotmat = setuprotationmatrix(angle, vec);
return vmmult(point, rotmat);
}
/**
* Adapted from C courtesy of Bibek Subedi
* https://www.programming-techniques.com/2012/03/3d-rotation-algorithm-about-arbitrary.html
* @param {number} angle - the angle to rotate around the vector
* @param {Vector3D} vec - the vector around which to rotate
* @returns {Matrix3D} - the rotation matrix
*/
function setuprotationmatrix(angle, vec) {
// Leaving L in for reusability, but it should always be 1 in our case
let u = vec[0], v = vec[1], w = vec[2];
let L = (u*u + v * v + w * w);
let u2 = u * u;
let v2 = v * v;
let w2 = w * w;
let rotmat = [[],[],[]];
rotmat[0][0] = (u2 + (v2 + w2) * Math.cos(angle)) / L;
rotmat[0][1] = (u * v * (1 - Math.cos(angle)) - w * Math.sqrt(L) * Math.sin(angle)) / L;
rotmat[0][2] = (u * w * (1 - Math.cos(angle)) + v * Math.sqrt(L) * Math.sin(angle)) / L;
rotmat[1][0] = (u * v * (1 - Math.cos(angle)) + w * Math.sqrt(L) * Math.sin(angle)) / L;
rotmat[1][1] = (v2 + (u2 + w2) * Math.cos(angle)) / L;
rotmat[1][2] = (v * w * (1 - Math.cos(angle)) - u * Math.sqrt(L) * Math.sin(angle)) / L;
rotmat[2][0] = (u * w * (1 - Math.cos(angle)) - v * Math.sqrt(L) * Math.sin(angle)) / L;
rotmat[2][1] = (v * w * (1 - Math.cos(angle)) + u * Math.sqrt(L) * Math.sin(angle)) / L;
rotmat[2][2] = (w2 + (u2 + v2) * Math.cos(angle)) / L;
return rotmat;
}