Related
I am writing a graphics application that needs to calculate and display a list of points along a curve arc which is described by three points.
Lets say we have points (1,1), (2,4) and (5,2). I need an algorithm that can give me the values of y for each x from 1 to 5 that fall on the interpolated arc.
I'm sure this is a simple task for you math whizes out there, but for me it's a bit beyond my mathematical payscale.
Thanks in advance!
So the problem is how to compute the center C = (c1, c2) and radius r of a circumference given by three points P = (p1, p2), Q = (q1, q2) and S = (s1, s2).
The idea is very simple. It consists in realizing that, by definition, the center has the same distance to all three points P, Q and S.
Now, the set of all points that are equidistant from Pand Q is the perpendicular to the segment PQ incident at the mid point (P+Q)/2. Similarly, the set of all points equidistant from Q and S is the perpendicular to QS passing thru (Q+S)/2. So, the center C must be the intersection of these two lines.
Let's compute the parametric equations of these two straight lines.
For this we will need two additional functions that I will call dist(A,B) which computes the distance between points A and B and perp(A,B) that normalizes the vector B-A dividing it by its length (or norm) and answers the perpendicular vector to this normalized vector (keep in mind that a perpendicular to (a,b) is (-b,a) because their inner product is 0)
dist((a1,a2),(b1,b2))
Return sqrt(square(b1-a1) + square(b2-a2))
perp((a1,a2),(b1,b2))
dist := dist((a1,a2),(b1,b2)).
a := (b1-a1)/dist.
b := (b2-a2)/dist.
Return (-b,a).
We can now write the parametric expressions of our two lines
(P+Q)/2 + perp(P,Q)*t
(Q+S)/2 + perp(Q,S)*u
Note that both parameters are different, hence the introduction of two variables t and u.
Equating these parametric expressions:
(P+Q)/2 + perp(P,Q)*t = (Q+S)/2 + perp(Q,S)*u
which consists of two linear equations, one for each coordinate, and two unknowns t and u (see below). The solution of this 2x2 system gives the values of the parameters t and u that injected into the parametric expressions give the center C of the circumference.
Once C is known, the radius r can be calculated as r := dist(P,C).
Linear equations
(P+Q)/2 + perp(P,Q)*t = (Q+S)/2 + perp(Q,S)*u
First linear equation (coordinate x)
(p1+q1)/2 + (p2-q2)/dist(P,Q)*t = (q1+s1)/2 + (q2-s2)/dist(Q,S)*u
Second linear equation (coordinate y)
(p2+q2)/2 + (q1-p1)/dist(P,Q)*t = (q2+s2)/2 + (s1-q1)/dist(Q,S)*u
Linear System (2x2)
(p2-q2)/dist(P,Q)*t + (s2-q2)/dist(Q,S)*u = (s1-p1)/2
(q1-p1)/dist(P,Q)*t + (q1-s1)/dist(Q,S)*u = (s2-p2)/2
From a three dimensional cartesian coordinate, object A's coordinate can be expressed as xyzwpr (green arrow). And from object A's coordinate world, object B can be also expressed as xyzwpr (blue arrow).
Then can anyone write down the C# code for calculating xyzwpr of object B relative to the original coordinate system (red arrow)?
Say A's coordinate is (30,50,70, -15,44,-80) B (60,90,110, 33,150,-90).
And say the order of the rotation is yaw(z)-> pitch(x) -> roll(y)
--- EDIT ---
Can anyone validate below assumptions?
Assumption for xyz of point B.
xyz of point B, the smaller airplane, can be calculated by adding xyz of point A, the first airplane, and the xyz of B and then applying the 3d rotation of A's wpr on the A's xyz.
The order of doing this is;
1) translate the A point to the origin (subtract A which is translate by -Ax,-Ay,-Az)
2) rotate about the origin (can use 3×3 matrix R0 of A)
3) then translate back. (add A which is translate by +Ax,+Ay,+Az)
Assumption for wpr of point B
is simply succession of rotations of two points. AwApArBwBpBr.
--- SOLVED. A few references with detailed explanation and codes ---
Global frame-of-reference VS Local frame-of-reference
3D matrix rotation about an arbitrary point
Euler to matrix conversion
This question has some issues.
First, I think it's not good practice to request directly for code. Instead, show code you tried, ask for errors in your code, or a better approach, or libraries that may help you.
I would suggest rephrasing your question. Now it looks like "Can anyone do my homework, please?".
What problems are you facing? Maybe you don't want to implement matrix multiplication and you would like to know libraries that already do it, or you don't know how to make the call to atan2.
Once you get matrix multiplication, translation matrix build up, rotation matrix build up and atan2 (made by yourself or by a library), you just have to (pseudocode):
Matrix c = a;
Matrix yaw, pitch, roll;
Matrix pos;
buildTranslationMatrix(pos, x, y, z);
buildRotationZMatrix(yaw, w);
buildRotationXMatrix(pitch, p);
buildRotationYMatrix(roll, r);
mult (c, c, pos); //c = c*pos
mult (c, c, yaw); //c = c*yaw
mult (c, c, pitch);
mult (c, c, roll);
decomposePos(c, x, y, z); // obtain final xyz from c
decomposeAngles(c, w, p, r); // obtain final wpr from c
Note the post-multiplication.
Hope I made a constructive criticism. :)
EDIT
Second assumption is correct.
Maybe I misunderstood the first one, but I think it's wrong. As I am more used to transformation matrices than to euler angles (and you pointed that link), I understand it this way:
To obtain xyz (as well as wpr) I would compute the transformation matrix, which contains all the values. The final transformation matrix of the second plane, in the original coordinate system, is computed as:
M = TA * RA * TB * RB
(TA is A translation matrix of plane A and RA is its rotation matrix)
Transformation matrices can be understood this way:
r r r t
r r r t
M = r r r t
s s s w
We only care about rotation and translation. If you multiply TA*RA:
1 0 0 x r r r 0 r r r x
0 1 0 y r r r 0 r r r y
0 0 1 z * r r r 0 = r r r z
0 0 0 1 0 0 0 1 0 0 0 1
which is how we understand the coordinate system of A. Remember that this means first rotating, as if it were at the origin, and then translating to position x, y, z. Post-multiplicating means intern transformation, transformation in the mobile coordinate system. So, if we continue post-multiplicating, we will composite the final transformation matrix.
Also, matrices are associative, so
M = (TA * RA) * (TB * RB)
is the same as
M = ((TA * RA) * TB) * RB
Recapitulation
xyz will be in the last column of M and wpr will have to be decomposed from 3*3 submatrix of M.
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.
Suppose one of a tetrahedron's four vertices is at the origin and the other three are at the end of vectors u, v, and w. If vectors u and v are known, and the angles between u and v, v and w, and w and u are also known, it seems there is a closed form solution for w: the intersection of the two cones formed by rotating a vector at the u and w angle about the u axis, and by rotating a vector at the v and w angle about the v axis.
Although I haven't been able to come up with a closed form solution in a couple days, I'm hoping it is due to my lack of experience with 3d geometry and that someone with more experience might have a helpful suggestion.
I had the same problem, and found MBo's answer very useful. But I think we can say a bit more about the value of w, because we're free to pick the coordinate system to work in. In particular, if we choose the x-axis to be in the direction of u, and the xy-plane to contain the vector v, then MBo's system of equations becomes:
wx = cos(uw)
vx*wx + vy*wy = cos(vw)
||w|| = 1
and this coordinate system gives
vx = cos(uv), vy = sin(uv)
so immediately we get that
_____________________
( cos(vw) - cos(uv) * cos(uw) + / 2 )
w = ( cos(uw), ----------------------------- , - / 1 - cos (uw) - wy*wy )
( sin(uv) \/ )
The +- on the square root gives the two possible solutions, unless of course 1 - cos^2(uw) - wy^2 <= 0. The division by sin(uv) also highlights a degenerate case when u and v are linearly dependent (point in the same direction).
Another check we can make is that if the vectors u and v are orthogonal, it's known that wy = cos(vw) (see https://math.stackexchange.com/questions/726782/find-a-3d-vector-given-the-angles-of-the-axes-and-a-magnitude). This is what falls out of the expression above (because cos(uv) = 0 and sin(uv) = 1).
There are not enough data to calculate vertice w position. But it is possible to find unit vector w (if it exists). Just use scalar product properties and solve equation system
(I've used (vx,vy,vz) as components of unit (normalized) vector v)
vx*wx+vy*wy+vz*wz=Cos(v,w angle)
ux*wx+uy*wy+uz*wz=Cos(u,w angle)
wx^2+wy^2+wz^2=1 //unit vector
This system can give us: no solutions (cones don't overlap); one solution (cones touching); two solutions (two rays as cones' surfaces intersection)
A-B-C-D are 4 points. We define r = length(B-C), angle, ang1 = (A-B-C) and angle ang2 = (B-C-D) and the torsion angle tors1 = (A-B-C-D). What I really need to do is to find the coordinates of C and D provided that I have the new values of r, ang1, ang2 and tors1.
The thing is that the points A and B are rigidly connected to each other, and points C and D are also connected to each other by a rigid connector, so to speak. That is the distance (C-D) remains fixed and also distance A-B remains fixed. There is no such rigid connection between the points B and C.
We have the old coordinates of the 4 points for some other set of (r,ang1,ang2,tors1) and we need to find the new coordinates when this defining set of variables changes to some arbitrary value.
I would be grateful for any helpful comments.
Thanks a lot.
I'm not allowed to post a picture because I'm a new user :(
Additional Info: An iterative solution is not going to be useful because I need to do this in a simulation "plenty of times O(10^6)".
I think the best way to approach this problem would be to think in terms of analytic geometry.
Each point A,B,C,D has some 3D coordinates (x,y,z) and you have some relationships between
them (e.g. distance B-C is equal to r means that
r = sqrt[ (x_b - x_c)^2 + (y_b - y_c)^2 + (z_b - z_c)^2 ]
Once you define such relations it remains to solve the resulting system of equations for the unknown values of coordinates of the points you need to determine.
This is a general approach, if you describe the problem better (maybe a picture?) it might be easy to find some efficient ways of solving such systems because of some special properties your problem has.
You haven't mentioned the coordinate system. Even if (r, a1, a2, t) don't change, the "coordinates" will change if the whole structure can be sent whirling off into space. So I'll make some assumptions:
Put B at the origin, C on the positive X axis and A in the XY plane with y>0. If you don't know the distance AB, calculate it from the old coordinates. Likewise CD.
A: (-AB cos(a1), AB sin(a1), 0)
B: (0, 0, 0)
C: (r, 0, 0)
D: (r + CD cos(a2), CD sin(a2) cos(t), CD sin(a2) sin(t))
(Just watch out for sign conventions in the angles.)
you are describing a set of constraints.
what you need to do is for every constraint check if they are still satisfied, and if not calc the most efficient way to get it correct again.
for instance, in case of length b-c=r if b-c is not r anymore, make it r again by moving both b and c to or from eachother so that the constraint is met again.
for every constraint one by one do this.
Then repeat a few times until the system has stabilized again (e.g. all constraints are met).
that's it
You are asking for a solution to a nonlinear system of equations. For the mathematically inclined, I will write out the constraint equations:
Suppose you have positions of points A,B,C,D. We define vectors AB=A-B, etc., and furthermore, we use the notation nAB to denote the normalized vector AB/|AB|. With this notation, we have:
AB.AB = fixed
CD.CD = fixed
CB.CB = r*r
nAB.nCB = cos(ang1)
nDC.nBC = cos(ang2)
Let E = D - DC.(nCB x nAB) // projection of D onto plane defined by ABC
nEC.nDC = cos(tors1)
nEC x nDC = sin(tors1) // not sure if your torsion angle is signed (if not, delete this)
where the dot (.) denotes dot product, and cross (x) denotes cross product.
Each point is defined by 3 coordinates, so there are 12 unknowns, and 6 constraint equations, leaving 6 degrees of freedom that are unconstrained. These are the 6 gauge DOFs from the translational and rotational invariance of the space.
Assuming you have old point positions A', B', C', and D', and you want to find a new solution which is "closest" (in a sense I defined) to those old positions, then you are solving an optimization problem:
minimize: AA'.AA' + BB'.BB' + CC'.CC' + DD'.DD'
subject to the 4-5 constraints above.
This optimization problem has no nice properties so you will want to use something like Conjugate Gradient descent to find a locally optimal solution with the starting guess being the old point positions. That is an iterative solution, which you said is unacceptable, but there is no direct solution unless you clarify your problem.
If this sounds good to you, I can elaborate on the nitty gritty of performing the numerical optimization.
This is a different solution than the one I gave already. Here I assume that the positions of A and B are not allowed to change (i.e. positions of A and B are constants), similar to Beta's solution. Note that there are still an infinite number of solutions, since we can rotate the structure around the axis defined by A-B and all your constraints are still satisfied.
Let the coordinates of A be A[0], A[1] and A[2], and similarly for B. You want explicit equations for C and D, as you mentioned in the response to Beta's solution, so here they are:
First find the position of C. As mentioned before, there are an infinite number of possibilities, so I will pick a good one for you.
Vector AB = A-B
Normalize(AB)
int best_i = 0;
for i = 1 to 2
if AB[i] < AB[best_i]
best_i = i
// best_i contains dimension in which AB is smallest
Vector N = Cross(AB, unit_vec[best_i]) // A good normal vector to AB
Normalize(N)
Vector T = Cross(N, AB) // AB, N, and T form an orthonormal frame
Normalize(T) // redundant, but just in case
C = B + r*AB*cos(ang1) + r*N*sin(ang1)
// Assume s is the known, fixed distance between C and D
// Update the frame
Vector BC = B-C, Normalize(BC)
N = Cross(BC, T), Normalize(N)
D = C + s*cos(tors1)*BC*cos(ang2) + s*cos(tors1)*N*sin(ang1) +/- s*sin(tors1)*T
That last plus or minus depends on how you define the orthonormal frame. Try one and see if it's what you want, otherwise it's the other sign. The notation above is pretty informal, but it gives a definite recipe for how to generate C and D from A, B, and your parameters. It also chooses a good C (which depends on a good, nondegenerate N). unit_vec[i] refers to the vector of all zeros, except for a 1 at index i. As usual, I have not tested the pseudocode above :)