I have two cartesian coordinate systems with known unit vectors:
System A(x_A,y_A,z_A)
and
System B(x_B,y_B,z_B)
Both systems share the same origin (0,0,0). I'm trying to calculate a quaternion, so that vectors in system B can be expressed in system A.
I am familiar with the mathematical concept of quaternions. I have already implemented the required math from here: http://content.gpwiki.org/index.php/OpenGL%3aTutorials%3aUsing_Quaternions_to_represent_rotation
One possible solution could be to calculate Euler angles and use them for 3 quaternions. Multiplying them would lead to a final one, so that I could transform my vectors:
v(A) = q*v(B)*q_conj
But this would incorporate Gimbal Lock again, which was the reason NOT to use Euler angles in the beginning.
Any idead how to solve this?
You can calculate the quaternion representing the best possible transformation from one coordinate system to another by the method described in this paper:
Paul J. Besl and Neil D. McKay
"Method for registration of 3-D shapes", Sensor Fusion IV: Control Paradigms and Data Structures, 586 (April 30, 1992); http://dx.doi.org/10.1117/12.57955
The paper is not open access but I can show you the Python implementation:
def get_quaternion(lst1,lst2,matchlist=None):
if not matchlist:
matchlist=range(len(lst1))
M=np.matrix([[0,0,0],[0,0,0],[0,0,0]])
for i,coord1 in enumerate(lst1):
x=np.matrix(np.outer(coord1,lst2[matchlist[i]]))
M=M+x
N11=float(M[0][:,0]+M[1][:,1]+M[2][:,2])
N22=float(M[0][:,0]-M[1][:,1]-M[2][:,2])
N33=float(-M[0][:,0]+M[1][:,1]-M[2][:,2])
N44=float(-M[0][:,0]-M[1][:,1]+M[2][:,2])
N12=float(M[1][:,2]-M[2][:,1])
N13=float(M[2][:,0]-M[0][:,2])
N14=float(M[0][:,1]-M[1][:,0])
N21=float(N12)
N23=float(M[0][:,1]+M[1][:,0])
N24=float(M[2][:,0]+M[0][:,2])
N31=float(N13)
N32=float(N23)
N34=float(M[1][:,2]+M[2][:,1])
N41=float(N14)
N42=float(N24)
N43=float(N34)
N=np.matrix([[N11,N12,N13,N14],\
[N21,N22,N23,N24],\
[N31,N32,N33,N34],\
[N41,N42,N43,N44]])
values,vectors=np.linalg.eig(N)
w=list(values)
mw=max(w)
quat= vectors[:,w.index(mw)]
quat=np.array(quat).reshape(-1,).tolist()
return quat
This function returns the quaternion that you were looking for. The arguments lst1 and lst2 are lists of numpy.arrays where every array represents a 3D vector. If both lists are of length 3 (and contain orthogonal unit vectors), the quaternion should be the exact transformation. If you provide longer lists, you get the quaternion that is minimizing the difference between both point sets.
The optional matchlist argument is used to tell the function which point of lst2 should be transformed to which point in lst1. If no matchlist is provided, the function assumes that the first point in lst1 should match the first point in lst2 and so forth...
A similar function for sets of 3 Points in C++ is the following:
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace Eigen;
/// Determine rotation quaternion from coordinate system 1 (vectors
/// x1, y1, z1) to coordinate system 2 (vectors x2, y2, z2)
Quaterniond QuaternionRot(Vector3d x1, Vector3d y1, Vector3d z1,
Vector3d x2, Vector3d y2, Vector3d z2) {
Matrix3d M = x1*x2.transpose() + y1*y2.transpose() + z1*z2.transpose();
Matrix4d N;
N << M(0,0)+M(1,1)+M(2,2) ,M(1,2)-M(2,1) , M(2,0)-M(0,2) , M(0,1)-M(1,0),
M(1,2)-M(2,1) ,M(0,0)-M(1,1)-M(2,2) , M(0,1)+M(1,0) , M(2,0)+M(0,2),
M(2,0)-M(0,2) ,M(0,1)+M(1,0) ,-M(0,0)+M(1,1)-M(2,2) , M(1,2)+M(2,1),
M(0,1)-M(1,0) ,M(2,0)+M(0,2) , M(1,2)+M(2,1) ,-M(0,0)-M(1,1)+M(2,2);
EigenSolver<Matrix4d> N_es(N);
Vector4d::Index maxIndex;
N_es.eigenvalues().real().maxCoeff(&maxIndex);
Vector4d ev_max = N_es.eigenvectors().col(maxIndex).real();
Quaterniond quat(ev_max(0), ev_max(1), ev_max(2), ev_max(3));
quat.normalize();
return quat;
}
What language are you using? If c++, feel free to use my open source library:
http://sourceforge.net/p/transengine/code/HEAD/tree/transQuaternion/
The short of it is, you'll need to convert your vectors to quaternions, do your calculations, and then convert your quaternion to a transformation matrix.
Here's a code snippet:
Quaternion from vector:
cQuat nTrans::quatFromVec( Vec vec ) {
float angle = vec.v[3];
float s_angle = sin( angle / 2);
float c_angle = cos( angle / 2);
return (cQuat( c_angle, vec.v[0]*s_angle, vec.v[1]*s_angle,
vec.v[2]*s_angle )).normalized();
}
And for the matrix from quaternion:
Matrix nTrans::matFromQuat( cQuat q ) {
Matrix t;
q = q.normalized();
t.M[0][0] = ( 1 - (2*q.y*q.y + 2*q.z*q.z) );
t.M[0][1] = ( 2*q.x*q.y + 2*q.w*q.z);
t.M[0][2] = ( 2*q.x*q.z - 2*q.w*q.y);
t.M[0][3] = 0;
t.M[1][0] = ( 2*q.x*q.y - 2*q.w*q.z);
t.M[1][1] = ( 1 - (2*q.x*q.x + 2*q.z*q.z) );
t.M[1][2] = ( 2*q.y*q.z + 2*q.w*q.x);
t.M[1][3] = 0;
t.M[2][0] = ( 2*q.x*q.z + 2*q.w*q.y);
t.M[2][1] = ( 2*q.y*q.z - 2*q.w*q.x);
t.M[2][2] = ( 1 - (2*q.x*q.x + 2*q.y*q.y) );
t.M[2][3] = 0;
t.M[3][0] = 0;
t.M[3][1] = 0;
t.M[3][2] = 0;
t.M[3][3] = 1;
return t;
}
I just ran into this same problem. I was on the track to a solution, but I got stuck.
So, you'll need TWO vectors which are known in both coordinate systems. In my case, I have 2 orthonormal vectors in the coordinate system of a device (gravity and magnetic field), and I want to find the quaternion to rotate from device coordinates to global orientation (where North is positive Y, and "up" is positive Z). So, in my case, I've measured the vectors in the device coordinate space, and I'm defining the vectors themselves to form the orthonormal basis for the global system.
With that said, consider the axis-angle interpretation of quaternions, there is some vector V about which the device's coordinates can be rotated by some angle to match the global coordinates. I'll call my (negative) gravity vector G, and magnetic field M (both are normalized).
V, G and M all describe points on the unit sphere.
So do Z_dev and Y_dev (the Z and Y bases for my device's coordinate system).
The goal is to find a rotation which maps G onto Z_dev and M onto Y_dev.
For V to rotate G onto Z_dev the distance between the points defined by G and V must be the same as the distance between the points defined by V and Z_dev. In equations:
|V - G| = |V - Z_dev|
The solution to this equation forms a plane (all points equidistant to G and Z_dev). But, V is constrained to be unit-length, which means the solution is a ring centered on the origin -- still an infinite number of points.
But, the same situation is true of Y_dev, M and V:
|V - M| = |V - Y_dev|
The solution to this is also a ring centered on the origin. These rings have two intersection points, where one is the negative of the other. Either is a valid axis of rotation (the angle of rotation will just be negative in one case).
Using the two equations above, and the fact that each of these vectors is unit length you should be able to solve for V.
Then you just have to find the angle to rotate by, which you should be able to do using the vectors going from V to your corresponding bases (G and Z_dev for me).
Ultimately, I got gummed up towards the end of the algebra in solving for V.. but either way, I think everything you need is here -- maybe you'll have better luck than I did.
Define 3x3 matrices A and B as you gave them, so the columns of A are x_A,x_B, and x_C and the columns of B are similarly defined. Then the transformation T taking coordinate system A to B is the solution TA = B, so T = BA^{-1}. From the rotation matrix T of the transformation you can calculate the quaternion using standard methods.
You need to express the orientation of B, with respect to A as a quaternion Q. Then any vector in B can be transformed to a vector in A e.g. by using a rotation matrix R derived from Q. vectorInA = R*vectorInB.
There is a demo script for doing this (including a nice visualization) in the Matlab/Octave library available on this site: http://simonbox.info/index.php/blog/86-rocket-news/92-quaternions-to-model-rotations
You can compute what you want using only quaternion algebra.
Given two unit vectors v1 and v2 you can directly embed them into quaternion algebra and get the corresponding pure quaternions q1 and q2. The rotation quaternion Q that align the two vectors such that:
Q q1 Q* = q2
is given by:
Q = q1 (q1 + q2)/(||q1 + q2||)
The above product is the quaternion product.
Firstly, this may sound very trivial, but currently I am creating a function getQuadrant(degree) for returning a quadrant from a given angle.
For instance, if degree is >= 0 and < 90, it will return 1. If degree is >= 90 and < 180, it will return 2. And so forth. This is very trivial. However, to be able to deal with degrees other than 0-360, I simply normalized those numbers to be in 0-360 degree range first, like this:
while (angle > 360)
angle = angle - 360;
end
while (angle < 0)
angle = angle + 360;
end
After that, I calculate. But to be frank, I hate using while statements like this. Are there other mathematical ways that can point out the quadrant of the angle in one go?
EDIT: I see that there are lots of good answers. Allow me to add "which algorithm will be the fastest?"
You can use the modulo operation:
angle %= 360.0; // [0..360) if angle is positive, (-360..0] if negative
if (angle < 0) angle += 360.0; // Back to [0..360)
quadrant = (angle/90) % 4 + 1; // Quadrant
(angle/90)%4+1
Assumptions:
angle is an integer
angle is positive
/ is integer division
For negative angles you'll need some additional handling.
Take advantage of integer arithmetics:
angle = angle - (angle/360)*360;
if (angle < 0) angle = angle + 360;
The idea is, since angle/360 is rounded down (floor()), (angle/360) gives you the k you need to do alpha = beta + 360k.
The second line is normalizing from [-359,-1] back to [1,359] if needed.
You've tagged your question trigonometry so here's some trigonometry:
a) take sin(theta) and cos(theta) -- it doesn't matter how many (positive or negative) multiples of 360° are included; sin(400°)==sin(40°)==sin(-320°) etc
b) if sin(theta)>0 and cos(theta)>0 theta is in quadrant 1
if sin(theta)>0 and cos(theta)<0 theta is in quadrant 2
and so on round the clock. Oh, and decide what to do at the 4 corners where sin and cos return 0.
Say I have point A (20,20) and point B (60,60).
The resulting vector would be 40, 40. How could I get the angle of this vector?
By this I mean, imagine there is an imaginary circle around the origin.
I guess sort of what atan2 does but without atan2.
Thank
Presuming that you want to find the angle of the vector against the X axis (in JavaScript):
var vector = {x: 40, y: 40};
var rad = Math.atan(vector.y/vector.x)
var deg = rad * 180/Math.PI;
alert(deg); // 45 deg
I'm not sure what you mean by angle, since you only give one vector in your example. But, given two vectors, you can find the angle between them like so:
Given vectors a and b, normalize both of them. Then, dot(a, b) = cos(θ), where θ is the angle between the two vectors. Use arccos to find θ.
Below is the link that will show you how to find the angle bewteen two vectors:
http://www.wikihow.com/Find-the-Angle-Between-Two-Vectors
I'm using Electro in Lua for some 3D simulations, and I'm running in to something of a mathematical/algorithmic/physics snag.
I'm trying to figure out how I would find the "spin" of a sphere of a sphere that is spinning on some axis. By "spin" I mean a vector along the axis that the sphere is spinning on with a magnitude relative to the speed at which it is spinning. The reason I need this information is to be able to slow down the spin of the sphere by applying reverse torque to the sphere until it stops spinning.
The only information I have access to is the X, Y, and Z unit vectors relative to the sphere. That is, each frame, I can call three different functions, each of which returns a unit vector pointing in the direction of the sphere model's local X, Y and Z axes, respectively. I can keep track of how each of these change by essentially keeping the "previous" value of each vector and comparing it to the "new" value each frame. The question, then, is how would I use this information to determine the sphere's spin? I'm stumped.
Any help would be great. Thanks!
My first answer was wrong. This is my edited answer.
Your unit vectors X,Y,Z can be put together to form a 3x3 matrix:
A = [[x1 y1 z1],
[x2 y2 z2],
[x3 y3 z3]]
Since X,Y,Z change with time, A also changes with time.
A is a rotation matrix!
After all, if you let i=(1,0,0) be the unit vector along the x-axis, then
A i = X so A rotates i into X. Similarly, it rotates the y-axis into Y and the
z-axis into Z.
A is called the direction cosine matrix (DCM).
So using the DCM to Euler axis formula
Compute
theta = arccos((A_11 + A_22 + A_33 - 1)/2)
theta is the Euler angle of rotation.
The magnitude of the angular velocity, |w|, equals
w = d(theta)/dt ~= (theta(t+dt)-theta(t)) / dt
The axis of rotation is given by e = (e1,e2,e3) where
e1 = (A_32 - A_23)/(2 sin(theta))
e2 = (A_13 - A_31)/(2 sin(theta))
e3 = (A_21 - A_12)/(2 sin(theta))
I applaud ~unutbu's, answer, but I think there's a simpler approach that will suffice for this problem.
Take the X unit vector at three successive frames, and compare them to get two deltas:
deltaX1 = X2 - X1
deltaX2 = X3 - X2
(These are vector equations. X1 is a vector, the X vector at time 1, not a number.)
Now take the cross-product of the deltas and you'll get a vector in the direction of the rotation vector.
Now for the magnitude. The angle between the two deltas is the angle swept out in one time interval, so use the dot product:
dx1 = deltaX1/|deltaX1|
dx2 = deltax2/|deltaX2|
costheta = dx1.dx2
theta = acos(costheta)
w = theta/dt
For the sake of precision you should choose the unit vector (X, Y or Z) that changes the most.
Given a point (pX, pY) and a circle with a known center (cX,cY) and radius (r), what is the shortest amount of code you can come up with to find the point on the circle closest to (pX, pY) ?
I've got some code kind of working but it involves converting the circle to an equation of the form (x - cX)^2 + (y - cY)^2 = r^2 (where r is radius) and using the equation of the line from point (pX, pY) to (cX, cY) to create a quadratic equation to be solved.
Once I iron out the bugs it'll do, but it seems such an inelegant solution.
where P is the point, C is the center, and R is the radius, in a suitable "mathy" language:
V = (P - C); Answer = C + V / |V| * R;
where |V| is length of V.
OK, OK
double vX = pX - cX;
double vY = pY - cY;
double magV = sqrt(vX*vX + vY*vY);
double aX = cX + vX / magV * R;
double aY = cY + vY / magV * R;
easy to extend to >2 dimensions.
i would make a line from the center to the point, and calc where that graph crosses the circle oO i think not so difficult
Solve it mathematically first, then translate into code. Remember that the shortest line between a point and the edge of a circle will also pass through its center (as stated by #litb).
The shortest distance point lies at the intersection of circumference and line passing through the center and the input point. Also center, input and output points lie on a straight line
let the center be (xc, yc) and shortest point from input (xi, yi) be (x,y) then
sqrt((xc-x)^2 + (yc-y)^2) = r
since center, input and output points lie on a straight line, slope calculated between
any of two of these points should be same.
(yc-yi)/(xc-xi) = (y-yc)/(x-xc)
4.solving equations 2&3 should give us the shortest point.
Trig functions, multiply by r, and add pX or pY as appropriate.
Treat the centre of the circular as your origin, convert the co-ordinates of (pX, pY) to polar co-ordinates, (theta, r') replace r' with the original circle's r and convert back to cartesian co-ordinates (and adjust for the origin).
You asked for the shortest code, so here it is. In four lines it can be done, although there is still a quadratic.
I've considered the point to be outside the circle.
I've not considered what happens if the point is directly above or below the circle center, that is cX=pX.
m=(cY-pY)/(cX-pX); //slope
b=cY-m*cX; //or Py-m*Px. Now you have a line in the form y=m*x+b
X=( (2mcY)*((-2*m*cY)^2-4*(cY^2+cX^2-b^2-2*b*cY-r^2)*(-1-m^2))^(1/2) )/(2*(cY^2+cX^2-b^2-2*bc*Y-r^2));
Y=mX+b;
1] Get an equation for a line connecting the point and the circle center.
2] Move along the line a distance of one radius from the center to find the point on the circle. That is: radius=a^2+b^2 which is: r=((cY-Y)+(cX-X))^(1/2)
3] Solve quadratically. X=quadratic_solver(r=((cY-Y)+(cX-X))^(1/2),X) which if you substitute in Y=m*X+b you get that hell above.
4] X and Y are your results on the circle.
I am rather certain I have made an error somewhere, please leave a comment if anyone finds something. Of course it is degenerate, one answer is furthest from your point and the other is closest.
Easy way to think about it in terms of a picture, and easy to turn into code: Take the vector (pX - cX, pY - cY) from the center to the point. Divide by its length sqrt(blah blah blah), multiply by radius. Add this to (cX, cY).
Here is a simple method I use in unity... for the math kn00bs amongst us.
Its dependent on the transform orientation but it works nicely. I am doing a postion.z = 0 but just fatten the axis of the 2d circle you are not using.
//Find closest point on circle
Vector3 closestPoint = transform.InverseTransformPoint(m_testPosition.position);
closestPoint.z = 0;
closestPoint = closestPoint.normalized * m_radius;
Gizmos.color = Color.yellow;
Gizmos.DrawWireSphere(transform.TransformPoint(closestPoint), 0.01f);