I have this problem I can't figure out and need help.
The problem is about calculating how many balloons are hit by a pellet gun. Balloons positions are described by 3D coordinates (X,Y,Z) and radius R. The gunshot is defined by 3D location of the end of the barrel "p" (Px,Py,Pz) and vector "v" (Vx, Vy, Vz) describing the direction barrel is pointing to.
I've tried to implement the solution suggested here: https://math.stackexchange.com/questions/1939423/calculate-if-vector-intersects-sphere
// C = center of sphere
// r = radius of sphere
// P = point on line
// U = unit vector in direction of line
Q = P - C;
a = U*U; // should be = 1
b = 2*U*Q
c = Q*Q - r*r;
d = b*b - 4*a*c; // discriminant of quadratic
if d < 0 then solutions are complex, so no intersections
if d >= 0 then solutions are real, so there are intersections
But the problem with this is that I get intersection with balloons that are positioned behind the gun. How can I modify this algorithm in order to produce the correct result? Or is my approach maybe wrong?
You need to actually solve the quadratic equation defined by your variables a, b and c.
Often, there are math libraries to do this, something like:
(t1,t2) = QuadraticSolve(a, b, c);
You can also do it manually for each parameter:
t1 = (-b + sqrt(b*b - 4*a*c)) / (2*a)
t2 = (-b - sqrt(b*b - 4*a*c)) / (2*a)
If t1 or t2 is positive then that intersection is in front of your gun.
Circle center : Cx,Cy
Circle radius : a
Point from which we need to draw a tangent line : Px,Py
I need the formula to find the two tangents (t1x, t1y) and (t2x,t2y) given all the above.
Edit: Is there any simpler solution using vector algebra or something, rather than finding the equation of two lines and then solving equation of two straight lines to find the two tangents separately? Also this question is not off-topic because I need to write a code to find this optimally
Here is one way using trigonometry. If you understand trig, this method is easy to understand, though it may not give the exact correct answer when one is possible, due to the lack of exactness in trig functions.
The points C = (Cx, Cy) and P = (Px, Py) are given, as well as the radius a. The radius is shown twice in my diagram, as a1 and a2. You can easily calculate the distance b between points P and C, and you can see that segment b forms the hypotenuse of two right triangles with side a. The angle theta (also shown twice in my diagram) is between the hypotenuse and adjacent side a so it can be calculated with an arccosine. The direction angle of the vector from point C to point P is also easily found by an arctangent. The direction angles of the tangency points are the sum and difference of the original direction angle and the calculated triangle angle. Finally, we can use those direction angles and the distance a to find the coordinates of those tangency points.
Here is code in Python 3.
# Example values
(Px, Py) = (5, 2)
(Cx, Cy) = (1, 1)
a = 2
from math import sqrt, acos, atan2, sin, cos
b = sqrt((Px - Cx)**2 + (Py - Cy)**2) # hypot() also works here
th = acos(a / b) # angle theta
d = atan2(Py - Cy, Px - Cx) # direction angle of point P from C
d1 = d + th # direction angle of point T1 from C
d2 = d - th # direction angle of point T2 from C
T1x = Cx + a * cos(d1)
T1y = Cy + a * sin(d1)
T2x = Cx + a * cos(d2)
T2y = Cy + a * sin(d2)
There are obvious ways to combine those calculations and make them a little more optimized, but I'll leave that to you. It is also possible to use the angle addition and subtraction formulas of trigonometry with a few other identities to completely remove the trig functions from the calculations. However, the result is more complicated and difficult to understand. Without testing I do not know which approach is more "optimized" but that depends on your purposes anyway. Let me know if you need this other approach, but the other answers here give you other approaches anyway.
Note that if a > b then acos(a / b) will throw an exception, but this means that point P is inside the circle and there is no tangency point. If a == b then point P is on the circle and there is only one tangency point, namely point P itself. My code is for the case a < b. I'll leave it to you to code the other cases and to decide the needed precision to decide if a and b are equal.
Here's another way using complex numbers.
If a is the direction (a complex number of length 1) of the tangent point on the circle from the centre c, and d is the (real) length along the tangent to get to p, then (because the direction of the tangent is I*a)
p = c + r*a + d*I*a
rearranging
(r+I*d)*a = p-c
But a has length 1 so taking the length we get
|r+I*d| = |p-c|
We know everything but d, so we can solve for d:
d = +- sqrt( |p-c|*|p-c| - r*r)
and then find the a's and the points on the circle, one of each for each value of d above:
a = (p-c)/(r+I*d)
q = c + r*a
Hmm not really an algorithm question (people tend to mistake algorithm and equation) If you want to write a code then do (you did not specify language nor what prevents you from doing this which is the reason of close votes)... Without this info your OP is just asking for math equation which is indeed off-topic here and by answering this I risk (right-full) down-votes too (but this is/was asked a lot here with much less info and 4 reopen votes against 1 close put my decision weight on reopen and answering this anyway).
You can exploit the fact that you are in 2D as in 2D perpendicular vectors to vector a(x,y) are computed like this:
c = (-y, x)
d = ( y,-x)
c = -d
so you swap x,y and negate one (which one determines if the perpendicular vector is CW or CCW). It is really a rotation formula but as we rotate by 90deg the cos,sin are just +1 and -1.
Now normal n to any circumference point on circle lies in the line going through that point and circles center. So putting all this together your tangents are:
// normal
nx = Px-Cx
ny = Py-Cy
// tangent 1
tx = -ny
ty = +nx
// tangent 2
tx = +ny
ty = -nx
If you want unit vectors than just divide by radius a (not sure why you do not call it r like the rest of the math world) so:
// normal
nx = (Px-Cx)/a
ny = (Py-Cy)/a
// tangent 1
tx = -ny
ty = +nx
// tangent 2
tx = +ny
ty = -nx
Let's go through derivation process:
As you can see, if the interior of the square is < 0 it's because the point is interior to the circumferemce. When the point is outside of the circumference there are two solutions, depending on the sign of the square.
The rest is easy. Take atan(solution) and be carefull here with the signs, you may better do some checks.
Use (2) and then undo (1) transformations and that's all.
c# implementation of dmuir's answer:
static void FindTangents(Vector2 point, Vector2 circle, float r, out Line l1, out Line l2)
{
var p = new Complex(point.x, point.y);
var c = new Complex(circle.x, circle.y);
var cp = p - c;
var d = Math.Sqrt(cp.Real * cp.Real + cp.Imaginary * cp.Imaginary - r * r);
var q = GetQ(r, cp, d, c);
var q2 = GetQ(r, cp, -d, c);
l1 = new Line(point, new Vector2((float) q.Real, (float) q.Imaginary));
l2 = new Line(point, new Vector2((float) q2.Real, (float) q2.Imaginary));
}
static Complex GetQ(float r, Complex cp, double d, Complex c)
{
return c + r * (cp / (r + Complex.ImaginaryOne * d));
}
Move the circle to the origin, rotate to bring the point on X and downscale by R to obtain a unit circle.
Now tangency is achieved when the origin (0, 0), the (reduced) given point (d, 0) and an arbitrary point on the unit circle (cos t, sin t) form a right triangle.
cos t (cos t - d) + sin t sin t = 1 - d cos t = 0
From this, you draw
cos t = 1 / d
and
sin t = ±√(1-1/d²).
To get the tangency points in the initial geometry, upscale, unrotate and untranslate. (These are simple linear algebra operations.) Notice that there is no need to perform the direct transform explicitly. All you need is d, ratio of the distance center-point over the radius.
I'm trying to derive a formula to extract vector u.
I'm given some initial data:
Plane F with the method to extract its normal n = F->normal().
vector c that does not lie within the plane F and passes through some point E that also does not lie within the plane F.
And some constrains to use:
The desired vector u is perpendicular to the vector c.
Vector u is also perpendicular to some vector r which is not given. The vector r is parallel to the plane F and also perpendicular to the vector c. Therefore, we can say the vectors c, r and u are orthogonal.
Let's denote * as dot product, and ^ operator is cross product between two 3d vectors.
The calculation of the vector u is easy by using cross product: vec3 u = c^r. So, my whole task is narrowed down to how to find the vector r which is parallel to a given plane F and at the same time perpendicular to the given vector c.
Because we know that r is parallel to F, we can use plane's normal and dot product: n*r = 0. Since r is unknown, an infinite number of lines can satisfy the aforementioned equation. So, we can also use the condition that r is perpendicular to c: r*c = 0.
To summarize, there are two dot-product equations that should help us to find the vector r:
r*c = 0;
r*n = 0;
However, I am having hard time trying to figure out how to obtain the vector r coordinates provided the two equations, in algorithmic way. Assuming r = (x, y, z) and we want to find x, y and z; it does not seem possible from only two equations:
x*c.x + y*c.y + z*c.z = 0;
x*n.x + y*n.y + z*n.z = 0;
I feel like I'm missing something, e.g., I need a third constrain. Is there anything else needed to extract x, y and z? Or do I have a flaw in my logic?
You can find the vector r by computing the cross product of n and c.
This will be automatically satisfy r.c=r.n=0
You are right that your two equations will have multiple solutions. The other solutions are given by any scalar multiple of r.
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)