There are 2 Point A(a,b,c), B(d,e,f)
There is the line AB, connecting A and B
Line CC' is perpendicular to line AB
I want to Point C 's position on line CC'
If length of line CC' is 1, what is C 's position?
How do I calculate C's position with unity?
A : Vector3(a,b,c)
B : Vector3(d,e,f)
AC' length = BC' length
CC' length = 1
AB is perpendicular to CC'
-> C Vector3(?,?,?)
As you describe it, the problem is undetermined. There are infinitely many solutions for C because you are working in 3D space. To visualize, imagine the point C orbiting around the axis AB. Regardless of where C is in its orbit, it will always be orthogonal to AB and length(CC') = 1. You need to further constrain the problem.
The Mathematics
First, we calculate the point C'. To do this, we take the vector from A to B, which is AB = B - A. Then, to get to C' we just travel half the distance of AB from point A:
C' = A + AB/2
Now we need to find a vector orthogonal to AB. Here we encounter the problem that I described initially. There are infinitely many such vectors so we need to further constrain the problem. Suppose that we can choose a vector v that is not colinear to the lines AB or C'C. Now we can find C'C by finding a vector orthogonal to both AB and v.
We know that the cross-product of two linearly independent vectors produces a vector that is orthogonal to both vectors. So all that is left to do is normalize the result so that the length is 1:
C'C = normalize(AB x v)
Finally, we can find point C by traveling from C' along the vector C'C:
C = C' + C'C
Unity Code
Here, I provide some untested code that simply implements the mathematics described above. I'm not awfully familiar with Unity so it is quite possible that there exists some built-in functions that would relieve some of the work:
Vector3 v = new Vector3(0, 0, 1); // Choose this as you wish
Vector3 AB = B - A;
Vector3 C_prime = A + AB / 2;
Vector3 C = C_prime + Vector3.Normalize(Vector3.cross(AB, v));
Related
I am trying to find a solution to the following problem but it is not clear how to solve it. Imagine that I have the following points in the space as in the image below:
If I consider that my only known information is the point positions and their normals I would like to determine whether two points (considering as a reference the position of the first point) are facing each other or not. For example from the above image for points a, b, c, d and e I have:
Point a faces points c and e but not points b and d.
Point b faces points d and e but not points a and c.
Point c faces point a but not points b, d and e.
Point d faces points b and e but not points a and c.
and finaly
Point e faces points a, b and d but not point c.
My first though was to play with the signed angles between the two normal vectors of each pair by using the solutions proposed here but this works for some pairs while not for others. The idea regarding what two point are facing each other is that if we consider a point as the origin then it faces another point if the other point is within the origin point's 180 degrees field of view and its normal vector is going inwards (kind of "towards") the origin point.
Any ideas what could help.
Thanks.
Update:
to try to be a bit more clear and answer some of the comments below. In principle its point in the space corresponds to the centroid of a face. However, I do not have this information beforehand (i.e. that each point corresponds to the center of a face, or the list of faces and their vertices). So in a higher level, if we were dealing with faces the problem would be how to determine if two faces are visible to each other or not but as I said the only information that I have now are the actual points in the space and their normals.
Sample points:
a = [26415.3720833199 11986.0504166605 739];
na = [0 0 1];
b = [27263.8100000023 11103.1983333336 1512.50000000021];
nb = [0.102791963903622 -0.994702876318771 0];
c = [28059.5700000001 11185.4316666667 962.499999999998];
nc = [-0.102791963903623 0.994702876318771 -9.06557542353252e-16];
d = [26606.7112499615 10390.7487916521 739];
nd = [0 0 1];
e = [27792.4499999996 9225.36499999984 2782];
ne = [0 0 -1];
You can solve your problem with a few simple dot products...
Based on your description, a point b is within the field of view (FOV) of another point a if the angle between the normal of a (i.e. na) and a vector going from a to b is less than or equal to 90 degrees. As described here, the angle can be found by taking the dot product of b-a and na, dividing by the length of b-a (and assuming the length of na is already 1), and taking the inverse cosine of the result. Putting it into an anonymous function, you have:
isInFOV = #(b, a, na) (acosd(dot(b-a, na)./norm(b-a)) <= 90);
You can then define a point b as "pointing toward" another point a if the component of nb (the normal of b) running along the vector going from b to a is positive. As described here, the component can be found by taking the dot product of a-b and nb and dividing by the length of a-b (and assuming the length of nb is already 1). Putting it into an anonymous function, you have:
isPointingToward = #(b, nb, a) (dot(a-b, nb)./norm(a-b) > 0);
We can then define whether a point a is "facing" another point b as:
isFacing = #(a, na, b, nb) (isInFOV(b, a, na) && isPointingToward(b, nb, a));
Note that I used the logical short circuit AND operator && since isPointingToward doesn't need to be evaluated if isInFOV already evaluates to false.
Vectorizing
You can reformulate the above equations to vectorize the operations, using functions like bsxfun or replacing a call to dot with standard matrix operations. This will allow you to check which points in a set a given point is facing. A vectorized version of the function isFacing is given below:
function index = isFacing(a, na, b, nb)
V = bsxfun(#minus, b, a); % Compute b-a for all b
V = bsxfun(#rdivide, V, sqrt(sum(V.^2, 2))); % Normalize each row
index = (acosd(V*na.') <= 90); % Find points in FOV of a
index(index) = (sum(V(index, :).*nb(index, :), 2) < 0); % Of those points in FOV,
% find those pointing
% towards a
end
Example
Using the sample data in the question:
pointMat = [26415.3720833199 11986.0504166605 739; ... % Point a
27263.8100000023 11103.1983333336 1512.50000000021; ... % Point b
28059.5700000001 11185.4316666667 962.499999999998; ... % Point c
26606.7112499615 10390.7487916521 739]; % Point d
normalMat = [0 0 1; ...
0.102791963903622 -0.994702876318771 0; ...
-0.102791963903623 0.994702876318771 -9.06557542353252e-16; ...
0 0 1];
p = [27792.4499999996 9225.36499999984 2782]; % Point e
np = [0 0 -1];
>> isFacing(p, np, pointMat, normalMat)
ans =
4×1 logical array
1 % Facing a
1 % Facing b
0 % Not facing c
1 % Facing d
I have a plain with known four co-ordinates and a line with two known co-ordinates as shown in figure.
The four co-ordinates of plane are
A = (-5 -5 -8)
B = ( 15 15 -8)
C = ( 15 15 12)
D = ( -5 -5 12)
The co-ordinates of line are
M = (1.3978,40,6.1149)
N = 4.3943, 4.8078,0.3551)
In this case line and plain intersects,then how can I find point of intersection of line and plane in 3D space by using MATLAB?
or How can I check both are intersecting or not?
I have tried to find solution by following video tutorial to find equation of plane from three points and tutorial for finding point where line intersects a plain
But in my case, equation of plane is zero. So I am confused. Can anyone help me?
Thanks in advance,
Manu
I would use simple linear algebra to find the intersection point.
Let n be normal to the plain (you can calculate it as a vector product of say N = cross(AB, AD), then unit n = N / |N| where |N| = sqrt(dot(N, N)) is length of vector N.
You can use the following function from matlabcentral which covers all the corner cases as well (such as when the line is parallel to the plane) and describes them in the comments.
Example from comment:
A =[ -6.8756 39.9090 10.0000],B =[ -6.0096 40.4090 10.0000],C =[ -6.0096 40.4090 11.0000],D=[ -6.8756 39.9090 11.0000];
P0 =[ 1.3978 40.0000 6.1149],P1 =[ 4.3943 -4.8078 0.3551];
I don't know where you made a mistake, but I am pretty sure there is an intersection point which is outside your segment. So you should have got check=3. Here is the output of step by step operation:
>> AB = B-A
AB = 0.8660 0.5000 0
>> AD = D-A
AD = 0 0 1
>> n = cross(AB,AD)/sqrt(dot(cross(AB,AD),cross(AB,AD)))
n = 0.5000 -0.8660 0
>> [I,check]=plane_line_intersect(n,A,P0,P1)
I = 1.0961 44.5116 6.6948
check = 3
It produces the same results with any other point (B, C or D) passed in. check=3 means there is an intersection point I, which is outside of the P01 segment.
As a verification step, notice that normal n has Nz = 0 which means that it's perpendicular to the Z axis. The only way a line wouldn't intersect with it is if it would be parallel to Z axis (and therefore vector P01 would be parallel to Z and have zero Z component).
Your P01 is not aligned with Z:
>> P01 = P1 - P0
P01 = 2.9965 -44.8078 -5.7598
Decomposing rotation matrix (x,y',z'') - Cartesian angles
Im currently working with rotation matrices and I have the following problem:
Given three coordinate systems (O0,x0,y0,z0; O1,x1,y1,z1; O2,x2,y2,z2) which coincide. We rotate first the frame #1 with the respect to frame #0, then the frame #2 with respect to frame #1.
The order of the rotations: R = Rx_alpha * Ry_beta * Rz_gamma, so first about x, then y', then z'', which are also known as the Cartesian angles.
If R1 stands for the 1st and R2 for the 2nd rotation, we are looking for the angles of the 2nd frame with respect to initial frame (#0) after both of the rotations. This can be done by decomposing the rotation matrix R (where:R = R1*R2 ). There are many literature available, how it can be done by Euler- and RPY-angles, but I don't find any, how to solve this problem in case of Cartesian angles.
I have a matlab function which works only by simple rotations. If all the angles have values different than 0 (example below), then the result becomes really unstable.
Orientation of the 1st frame with respect to the frame #0:
alpha1 = 30*pi/180;
beta1 = 10*pi/180;
gamma1 = 0*pi/180;
Orientation of the 2nd frame with respect to the frame #1
alpha2 = 10*pi/180;
beta2 = 10*pi/180;
gamma2 = 0*pi/180;
The matlab function I was using for solving the problem:
function [q] = cartesian_angles(R)
beta = asin(R(1,3));
*% Catching the numerical singularty*
if abs(abs(beta)-pi/2) > eps;
*% singulartiy of acos*
gamma1 = acos(R(1,1) / cos(beta));
gamma2 = asin(-R(1,2) / cos(beta));
if gamma2<0
gamma=2*pi-gamma1;
else
gamma=gamma1;
end
alpha1 = acos(R(3,3) / cos(beta));
alpha2 = asin(-R(2,3) / cos(beta));
if alpha2<0
alpha = 2*pi-alpha1;
else
alpha = alpha1;
end
else
fprintf('beta=pi/2 \n')
gamma = 0;
alpha = 0;
beta = 0;
end;
alpha = alpha*180/pi;
beta = beta*180/pi;
gamma = gamma*180/pi;
q = [alpha; beta; gamma];
Thank you for any help! If you have some questions don't hesitate to ask!
Marci
First, I'm going to assume you are passing into your function a well conditioned, right-handed rotation matrix. I'm going to use the same rotation sequence as you listed above, X Y' Z''
If you know the symbolic construction of the rotation matrix you are trying to extract angles from, the math is pretty straight forward. Below is an example of matlab code to determine the construction of the rotation matrix of order X-Y'-Z''
a = sym('a');%x
b = sym('b');%y
g = sym('g');%z
Rx = [1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
Ry = [cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
Rz = [cos(g) -sin(g) 0;sin(g) cos(g) 0;0 0 1];
R = Rz*Ry*Rx
The output looks like this:
R =
[ cos(b)*cos(g), cos(g)*sin(a)*sin(b) - cos(a)*sin(g), sin(a)*sin(g) + cos(a)*cos(g)*sin(b)]
[ cos(b)*sin(g), cos(a)*cos(g) + sin(a)*sin(b)*sin(g), cos(a)*sin(b)*sin(g) - cos(g)*sin(a)]
[ -sin(b), cos(b)*sin(a), cos(a)*cos(b)]
Here's the same result in a nicer looking format:
Now let's go over the math to extract the angles from this matrix. Now would be a good time to become comfortable with the atan2() function.
First solve for the beta angle (by the way, alpha is the rotation about the X axis, beta is the rotation about Y' axis, and gamma is the angle about the Z'' axis):
beta = atan2(-1*R(3,1),sqrt(R(1,1)^2+R(2,1)^2))
Written more formally,
Now that we have solved for the beta angle we can solve more simply for the other two angles:
alpha = atan2(R(3,2)/cos(beta),R(3,3)/cos(beta))
gamma = atan2(R(2,1)/cos(beta),R(1,1)/cos(beta))
Simplified and in a nicer format,
The above method is a pretty robust way of getting the Euler angles out of your rotation matrix. The atan2 function really makes it much simpler.
Finally I will answer how to solve for the rotation angles after a series of rotations. First consider the following notation. A vector or rotation matrix will be notated in the following way:
Here "U" represents the universal frame, or global coordinate system. "Fn" represents the nth local coordinate system that is different from U. R means rotation matrix (this notation could also be used for homogeneous transformations). The left side superscript will always represent the parent frame of reference of the rotation matrix or vector. The left side subscript indicates the child frame of reference. For example, if I have a vector in F1 and I want to know what it is equivalently in the universal frame of reference I would perform the following operation:
To get the vector resolved in the universal frame I simply multiplied it by the rotation matrix that transforms things from F1 to U. Notice how the subscripts are "cancelled" out by the superscript of the next item in the equation. This is a clever notation to help someone from getting things mixed up. If you recall, a special property of well conditioned rotation matrices is that the inverse matrix is the transpose of the matrix, which is will also be the inverse transformation like this:
Now that the notation details are out of the way, we can start to consider solving for complicated series of rotations. Lets say I have "n" number of coordinate frames (another way of saying "n" distinct rotations). To figure out a vector in the "nth" frame in the universal frame I would do the following:
To determine the Cardan/Euler angles that result from "n" rotations, you already know how to decompose the matrix to get the correct angles (also known as inverse kinematics in some fields), you simply need the correct matrix. In this example I am interested in the rotation matrix that takes things in the "nth" coordinate frame and resolves them into the Universal frame U:
There is it, I combined all the rotations into the one of interest simply by multiplying in the correct order. This example was easy. More complicated cases come when someone wants to find the reference frame of one rigid body resolved in the frame of another and the only thing the two rigid bodies have in common is their measurement in a universal frame.
I want to also note that this notation and method can also be used with homogeneous transformations but with some key differences. The inverse of a rotation matrix is its transpose, this is not true for homogeneous transformations.
Thank you for you answer willpower2727, your answer was really helpful!
But I would like to mention, that the code you have shown is useful to decompose rotational matrices, which are built in the following way:
R = Rz*Ry*Rx
What I'm looking for:
R = Rx*Ry*Rz
Which results into the following rotational matrix:
However, it's not a problem, as following the method how you calculate the angles alpha, beta and gamma, it was easy to modify the code so it decomposes the matrix shown above.
The angles:
beta = atan2( R(1,3), sqrt(R(1,1)^2+(-R(1,2))^2) )
alpha = atan2( -(R(2,3)/cos(beta)),R(3,3)/cos(beta) )
gamma = atan2( -(R(1,2)/cos(beta)),R(1,1)/cos(beta) )
One thing is still not clear though. The method is perfectly useful, bunt only if I calculate the angles after one rotation. As there are more rotations linked after each other, the results are false. However, it's still solvable, I guess, considering the following way: Let's say, we have two rotations linked after each other (R1 and R2). q1 shows the angles of R1, q2 of R2. after decomposing the single matrices. The total angle of rotation of the matrix R=R1*R2can be easily calculated through summing up the rangles before: q=q1+q2
Is there no way, how to calculate the angles of the total rotation, not by summing the partial angles, but decomposing the matrix R=R1*R2?
UPDATE:
Considering the following basic example. The are to rotations linked after each other:
a1 = 10*pi/180
b1 = 20*pi/180
g1 = 40*pi/180
R1 = Rx_a1*Ry_b1_Rz_g1
a2 = 20*pi/180
b2 = 30*pi/180
g2 = 30*pi/180
R2 = Rx_a2*Ry_b2*Rz_g2
Decomposing the individual matrices R1 and R2 results in the rights angles. The problem occures, when I link the rotations after each other and I try to determinate the angles of the last frame in the inertial frame. Theoretically this could be done by decomposing the product of all rotational matrices of the chain of transformations.
R = R1*R2
Decomposing this matrix gives the following false result shown in degrees:
a = 0.5645
b = 54.8024
g = 61.4240
Marci
Given a 3-dimensional polyline P = {(x1, y1, t1), ..., (xn, yn, tn)} and another polyline Q = {(x1, y1, t1), ..., (xm, ym, tm)} (m not necessarly equal to n, so polylines could have different length), coincidence in space and time occurs when the trajectories of moving objects P and Q, have some timing and positioning in common (Point A, as seen in example figure is a coincidence point cause (xa, ya, ta)==(xb, yb, tb) obviously coincidence point could be a point outside of initial sets of point)
The concept is quite simple and visual perspective easily identify where colocation happen. Hardest part is how to realize an algorithm that efficiently compute coincidence and return the calculated (remember: point could be outside given sets of points) x, y coords and time t of where colocation happens!! This algorithm will be developed in Matlab so I have all necessary to rapidly work.
Best regards
assuming that x, y, z are functions of t for all segments of each polyline, here's a brute-force start. in 4 dimensions: P has segments p1 from (x_start(t), y_start(t), z_start(t), t) to (x_end(t), y_end(t), z_end(t), t), and similarly Q
for each segment p of P
for each segment q of Q
if p intersects q (in 4 dimensions)
output intersection point
the intersection condition is:
there exists alpha and beta in [0,1] where alpha * px_start(t) + (1 - alpha) * (px_end(t) - px_start(t)) = beta * qx_start(t) + (1 - beta) * (qx_end(t) - qx_start(t)) and 2 more similar conditions for y and z
solvability of the intersection condition depends on what are the functions x(t), y(t), z(t) -- linear? polynomials? etc.
Given 2 points A and B belonging to a sphere with a given Radius R.
I want to find the sphere whose center has the minimum distance to a given point G.
Thanks
The centers of a sphere defined by two points and a radius is a circle. You can connect C (the center of the circle) and G and create a 90° projection on the circle plane. The minimum distance is where the projection intersects the circle tangent by 90°. There are two solutions. You have to take the smaller one.
The point C you want is in the plane that contains A, B, and G. You compute
AG = G - A;
BG = G - B;
N = cross(AG, BG);
N = N / norm( N ); % the normal to the plane
Now you solve for C in this plane. Three equations:
dot((C-G), N)=0;
sqrt(sum(A-C).^2) = R;
sqrt(sum(B-C).^2) = R;
Three unknowns are the three elements of C. You end up with two solutions, so compute the distance to G and pick the closer one.