Numerically stable Angle bisector algorithm - c++

Is there any numerically stable angle bisector algorithm?
The problem is the following:
Given three vectors (2 dimensional) A,B,C
Find the bisector of angle B (angle between AB and BC)
Actually I'm computing it in the following way:
Normalize AB
Normalize BC
Find (AB+CD)/2f (Mid Point)
The bisector is line passing between B and the Mid Point.
The problem with my approach is that when the angle is almost 180° (AB almost parallel to BC) the bisector is very inaccurate (of course because mid point is almost coincident with B). The current algorithm is so inaccurate that sometimes the resulting bisector is almost parallel to one of the other 2 segments.
And yes there are no "cast" problems, all computations are done in single precision floating point.

You could use that the angle bisector remains the same if you rotate BA by +90° and BC by -90°.
So use the original formula if the situation is stable, that is, if the dot product of BA and BC is positive.
If it is negative, apply the rotations, for BA (x,y) -> (-y,x) and for BC (x,y) -> (y,-x), which also renders the dot product positive. Proceed as before with the new vectors.
If you try this out you will note that the jump in direction of the bisector now occurs for the angle -90° between the vectors. It is not possible to avoid this jump, as a continuous bisector will only be the same after two turns (fixing BA and moving C).

It’s not trivial. Let’s say the two edge vectors are a and b:
float2 a = A - B;
float2 b = C - B;
Compute the dot product float dp = dot( a, b )
Normalize both vectors:
float2 a_norm = normalize( a );
float2 b_norm = normalize( b );
Check the sign bit of the dot product. When the dp is non-negative,
return normalize( a_norm + b_norm ); and you’re done.
When the dot product is negative, you have obtuse angle between input vectors.
Applying a naïve formula in this case would screw up the numerical precision.
Need another way.
float2 c = normalize( a_norm - b_norm );
float dir = dot( a, rotate90( b ) );
return ( dir < 0 ) ? rotate90( c ) : rotate270( c );
Note - instead of the +, this is what gives the precision win. When the angle between a and b is greater than 90°, the angle between a and -b is less than 90°, and the length of a_norm - b_norm is large enough to give accurate direction. We just need to rotate it by 90° afterwards, in the correct direction.
P.S. Rotating 2D vectors by multiples of 90° is lossless operation.
Here’s pseudocode for rotate90 and rotate270 functions:
float2 rotate90( float2 vec )
{
return float2( vec.y, -vec.x );
}
float2 rotate270( float2 vec )
{
return float2( -vec.y, vec.x );
}

A simple enough way to do this follows in two formats (but the content is otherwise identical):
Pseudocode
// Move A and C to the origin for easier rotation calculations
Aprime=A-B;
Cprime=C-B;
// The counter-clockwise angle between the positive X axis to A'
angle_a = arctan(Aprime.y, Aprimet.x);
// ditto for C'
angle_c = arctan(Cprime.y, Cprime.x);
// The counter-clockwise angle from A' to C'
angle_ac = angle_c - angle_a;
// The counter-clockwise angle from the positive X axis to M'
angle_m = angle_ac/2 + angle_a;
// Construct M' which, like A' and C', is relative to the origin.
Mprime=(cos(angle_m), sin(angle_m));
// Construct M which is relative to B rather than relative to the origin.
M=Mprime+B
In English
Move the vectors to the origin by
A'=A-B
B'=B
C'=C-B
Get the angle from the positive X axis to A' as angle_a = arctan(A_y, A_x).
Get the angle from the positive X axis to C' as angle_c = arctan(C_y, C_x).
Get the counter-clockwise angle from A' to C' as angle_ac = angle_c - angle_a.
Get the angle from the positive X axis to M' as angle_m = angle_ac/2 + angle_a.
Construct M' from this angle as M' = (cos(angle_m), sin(angle_m)).
Construct M as M = M' + B.
The vector BM bisects the angle ABC.
Since there is arbitrary division, there are no difficulties with this method. Here's a graphing calculator to encourage intuition with the solution: https://www.desmos.com/calculator/xwbno717da

You can find the bisecting vector quite simply with:
∥BC∥ * BA + ∥BA∥ * BC
But that also won't be numerically stable with ABC collinear or nearly so. What might work better would be to find the angle between AB and BC, via the dot product.
cos θ = (BA · BC) / (∥BC∥ * ∥BA∥)
That will produce the correct angle even in the collinear case.

Definition: If A and B are points, vector(A,B) is the vector from point A to B.
Lets say that point O is the point of origin for our coordinate system.
The coordinates of point A are the same as of radius-vector(O,A).
Let point M be the middle point for the bisector,so you need to:
-normalize vector(B,A)
-normalize vector(B,C)
-vector(B,M) = vector(B,A)+vector(B,C) //vector from B to middle point
-(optionally) You can multiply vector(B,M) with a scalar to get a longer vector / increase distance between B and M
-vector(O,M) = vector(O,B) + vector(B,M)//radius-vector from O to M
Now middle point M has the same coordinates as radius-vector(O,M).

Related

Algorithm for determine the Arc Mid Point

I am currently looking to implement an algorithm that will be able to compute the arc midpoint. From here on out, I will be referring to the diagram below. What is known are the start and end nodes (A and B respectively), the center (point C) and point P which is the intersection point of the line AB and CM (I am able to find this point without knowing point M because line AB is perpendicular to line CM and thus, the slope is -1/m). I also know the arc angle and the radius of the arc. I am looking to find point M.
I have been looking at different sources. Some suggest converting coordinates to polar, computing the mid point from the polar coordinates then reverting back to Cartesian. This involves sin and cos (and arctan) which I am a little reluctant to do since trig functions take computing time.
I have been looking to directly computing point M by treating the arc as a circle and having Line CP as a line that intersects the circle at Point M. I would then get two values and the value closest to point P would be the correct intersection point. However, this method, the algebra becomes long and complex. Then I would need to create special cases for when P = C and for when the line AB is horizontal and vertical. This method is ok but I am wondering if there are any better methods out there that can compute this point that are simpler?
Also, as a side note, I will be creating this algorithm in C++.
A circumference in polar form is expressed by
x = Cx + R cos(alpha)
y = Cy + R sin(alpha)
Where alpha is the angle from center C to point x,y. The goal now is how to get alpha without trigonometry.
The arc-midpoint M, the point S in the middle of the segment AB, and your already-calculated point P, all of them have the same alpha, they are on the same line from C.
Let's get vector vx,vy as C to S. Also calculate its length:
vx = Sx - Cx = (Ax + Bx)/2 - Cx
vy = Sy - Cy = (Ay + By)/2 - Cy
leV = sqrt(vx * vx + vy * vy)
I prefer S to P because we can avoid some issues like infinite CP slope or sign to apply to slope (towards M or its inverse).
By defintions of sin and cos we know that:
sin(alpha) = vy / leV
cos(alpha) = vx / leV
and finally we get
Mx = Cx + R * vx / leV
My = Cy + R * vy / leV
Note: To calculate Ryou need another sqrt function, which is not quick, but it's faster than sin or cos.
For better accuracy use the average of Ra= dist(AC) and Rb= dist(BC)
I would then get two values
This is algebraically unavoidable.
and the value closest to point P would be the correct intersection point.
Only if the arc covers less than 180°.
Then I would need to create special cases for when P = C
This is indeed the most tricky case. If A, B, C lie on a line, you don't know which arc is the arc, and won't be able to answer the question. Unless you have some additional information to start with, e.g. know that the arc goes from A to B in a counter-clockwise direction. In this case, you know the orientation of the triangle ABM and can use that to decide which solition to pick, instead of using the distance.
and for when the line AB is horizontal and vertical
Express a line as ax + by + c = 0 and you can treat all slopes the same. THese are homogeneous coordinates of the line, you can compute them e.g. using the cross product (a, b, c) = (Ax, Ay, 1) × (Bx, By, 1). But more detailed questions on how best to compute these lines or intersect it with the circle should probably go to the Math Stack Exchange.
if there are any better methods out there that can compute this point that are simpler?
Projective geometry and homogeneous coordinates can avoid a lot of nasty corner cases, like circles of infinite radius (also known as lines) or the intersection of parallel lines. But the problem of deciding between two solutions remains, so it probably doesn't make things as simple as you'd like them to be.

How to fit a plane to a 3D point cloud?

I want to fit a plane to a 3D point cloud. I use a RANSAC approach, where I sample several points from the point cloud, calculate the plane, and store the plane with the smallest error. The error is the distance between the points and the plane. I want to do this in C++, using Eigen.
So far, I sample points from the point cloud and center the data. Now, I need to fit the plane to the samples points. I know I need to solve Mx = 0, but how do I do this? So far I have M (my samples), I want to know x (the plane) and this fit needs to be as close to 0 as possible.
I have no idea where to continue from here. All I have are my sampled points and I need more data.
From you question I assume that you are familiar with the Ransac algorithm, so I will spare you of lengthy talks.
In a first step, you sample three random points. You can use the Random class for that but picking them not truly random usually gives better results. To those points, you can simply fit a plane using Hyperplane::Through.
In the second step, you repetitively cross out some points with large Hyperplane::absDistance and perform a least-squares fit on the remaining ones. It may look like this:
Vector3f mu = mean(points);
Matrix3f covar = covariance(points, mu);
Vector3 normal = smallest_eigenvector(covar);
JacobiSVD<Matrix3f> svd(covariance, ComputeFullU);
Vector3f normal = svd.matrixU().col(2);
Hyperplane<float, 3> result(normal, mu);
Unfortunately, the functions mean and covariance are not built-in, but they are rather straightforward to code.
Recall that the equation for a plane passing through origin is Ax + By + Cz = 0, where (x, y, z) can be any point on the plane and (A, B, C) is the normal vector perpendicular to this plane.
The equation for a general plane (that may or may not pass through origin) is Ax + By + Cz + D = 0, where the additional coefficient D represents how far the plane is away from the origin, along the direction of the normal vector of the plane. [Note that in this equation (A, B, C) forms a unit normal vector.]
Now, we can apply a trick here and fit the plane using only provided point coordinates. Divide both sides by D and rearrange this term to the right-hand side. This leads to A/D x + B/D y + C/D z = -1. [Note that in this equation (A/D, B/D, C/D) forms a normal vector with length 1/D.]
We can set up a system of linear equations accordingly, and then solve it by an Eigen solver as follows.
// Example for 5 points
Eigen::Matrix<double, 5, 3> matA; // row: 5 points; column: xyz coordinates
Eigen::Matrix<double, 5, 1> matB = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// Find the plane normal
Eigen::Vector3d normal = matA.colPivHouseholderQr().solve(matB);
// Check if the fitting is healthy
double D = 1 / normal.norm();
normal.normalize(); // normal is a unit vector from now on
bool planeValid = true;
for (int i = 0; i < 5; ++i) { // compare Ax + By + Cz + D with 0.2 (ideally Ax + By + Cz + D = 0)
if ( fabs( normal(0)*matA(i, 0) + normal(1)*matA(i, 1) + normal(2)*matA(i, 2) + D) > 0.2) {
planeValid = false; // 0.2 is an experimental threshold; can be tuned
break;
}
}
This method is equivalent to the typical SVD-based method, but much faster. It is suitable for use when points are known to be roughly in a plane shape. However, the SVD-based method is more numerically stable (when the plane is far far away from origin) and robust to outliers.

How to find correct rotation from one vector to another?

I have two objects, and each object has two vectors:
normal vector
up vector
Like on this image:
Up vector is perpendicular to normal vector. Now I want to find unique rotation from one object to another, how to do that?
I have one method to find rotation between one vector to another, and it works. The problem is that I need to take care the two vectors: normal vector and up vector. If I use this method to rotate normal vector from object one to normal from object two, the up vector could be pointing wrong way, and they needs to be parallel.
Here is the code for finding the shortest rotation:
GE::Quat GE::Quat::fromTo(const Vector3 &v1, const Vector3 &v2)
{
Vector3 a = Vector3::cross(v1, v2);
Quat q;
float dot = Vector3::dot(v1, v2);
if ( dot >= 1 )
{
q = Quat(0,0,0,1);
}
else if ( dot < -0.999999 )
{
Vector3 axis = Vector3::cross(Vector3(1,0,0),v2);
if (axis.length() == 0) // pick another if colinear
axis = Vector3::cross(Vector3(0,1,0),v2);
axis.normalize();
q = Quat::axisToQuat(axis,180);
}
else
{
float s = sqrt( (1+dot)*2 );
float invs = 1 / s;
Vector3 c = Vector3::cross(v1, v2);
q.x = c.x * invs;
q.y = c.y * invs;
q.z = c.z * invs;
q.w = s * 0.5f;
}
q.normalize();
return q;
}
What should I change/add to this code, to find the correct rotation?
Before we begin, I will assume that both UP vector and normal vector are normalized and orthogonal (dot product is zero) between them.
Let's say that you want to rotate your yellow plate to be aligned with the rose (red?) plate. So, our reference will be the vectors from yellow plate and we will call our coordinate system as XYZ, where Z -> normal yellow vector, Y -> Up yellow vector and X -> YxZ (cross product).
In the same way, for rose plate, the rotated coordinate system will be called X'Y'Z' where Z' -> normal rose vector, Y' -> up rose vector and X' -> Y'xZ' (cross product).
Ok to find the rotation matrix, we only need to make sure that our normal yellow vector will become normal rose vector; that our up yellow vector will be transfomed in the up rose vector, and so on, i.e.:
RyellowTOrose = |X'x Y'x Z'x|
|X'y Y'y Z'y|
|X'z Y'z Z'z|
in other words, after you have any primitives transformed to be in coordinates of yellow system, applying this transformation, will rotate it to be aligned with rose coordinates system
If your up and normal vector aren't orthogonal, you can correct one of them easily. Just make the cross product between normal and up (results in a vector called C, for convenience) and do again the cross product between with C and normal, to correct the up vector.
First of all, I make the claim that there is only one such transformation that will align the orientation of the two objects. So we needn't worry about finding the shortest one.
Let the object that will be rotated be called a, and call the object that stay stationary b. Let x and y be the normal and up vectors respectively for a, and similarly let u and v be these vectors for b. I will assume x, y, u, and v are unit length, and that is x is orthogonal to y, and u is orthogonal to v. If any of this is not the case code can be written to correct this (via planar projection and normalization).
Now let’s construct matrices defining the “world space” the orientation of a and b. (let ^ denote the cross product) construct z as x ^ y, and construct c as a ^ b. Writing x, y, z and a, b, c to columns of each matrix gives us the two matrices, call them A and B respectively. (the cross product here gives us a unit length and mutually orthogonal vector since the same is true of the operands)
The change of coordinate system transformation to obtain B in terms of A is A^-1 (the inverse of matrix A, where ^ denotes a generalization of an exponent), in this case A^-1 can be computed as A^T, the transpose, since A is an orthogonal matrix by construction. Then the physical transformation to B is just matrix B itself. So, transforming an object by A^-1, and then by B will give the desired result. However these transformations can be concatenated into one transformation by multiplying B on the right into A^-1 on the left.
You end up with this matrix (assuming no arithmetic errors):
_ _
| x0*u0+x1*u1+x2*u2 x0*v0+x1*v1+x2*v2 x0*(u1*v2-u2*v1)+x1*(u2*v0-u0*v2)+x2*(u0*v1-u1*v0) |
| |
| y0*u0+y1*u1+y2*u2 y0*v0+y1*v1+y2*v2 y0*(u1*v2-u2*v1)+y1*(u2*v0-u0*v2)+y2*(u0*v1-u1*v0) |
| |
| (x0*y2-x2*y1)*u0+(x2*y0-x0*y2)*u1+(x0*y1-x1*y0)*u2 (x0*y2-x2*y1)*v0+(x2*y0-x0*y2)*v1+(x0*y1-x1*y0)*v2 (x0*y2-x2*y1)*(u1*v2-u2*v1)+(x2*y0-x0*y2)*(u2*v0-u0*v2)+(x0*y1-x1*y0)*(u0*v1-u1*v0) |
|_ _|
The quaternion code rotates just one vector to another without "Up" vector.
In your case simply build rotation matrix from 3 orthogonal vectors
normalized (unit) direction vector
normalized (unit) up vector
cross product of direction and up vectors.
Than you will have R1 and R2 matrix (3x3) representing rotation of object in two cases.
To find rotation from R1 to R2 just do
R1_to_R2 = R2 * R1.inversed()
And matrix R1_to_R2 is the transformation matrix from one orientation to other. NOTE: R1.inversed() here can be replaced with R1.transposed()

How to project a point onto a plane in 3D?

I have a 3D point (point_x,point_y,point_z) and I want to project it onto a 2D plane in 3D space which (the plane) is defined by a point coordinates (orig_x,orig_y,orig_z) and a unary perpendicular vector (normal_dx,normal_dy,normal_dz).
How should I handle this?
Make a vector from your orig point to the point of interest:
v = point-orig (in each dimension);
Take the dot product of that vector with the unit normal vector n:
dist = vx*nx + vy*ny + vz*nz; dist = scalar distance from point to plane along the normal
Multiply the unit normal vector by the distance, and subtract that vector from your point.
projected_point = point - dist*normal;
Edit with picture:
I've modified your picture a bit. Red is v. dist is the length of blue and green, equal to v dot normal. Blue is normal*dist. Green is the same vector as blue, they're just plotted in different places. To find planar_xyz, start from point and subtract the green vector.
This is really easy, all you have to do is find the perpendicular (abbr here |_) distance from the point P to the plane, then translate P back by the perpendicular distance in the direction of the plane normal. The result is the translated P sits in the plane.
Taking an easy example (that we can verify by inspection) :
Set n=(0,1,0), and P=(10,20,-5).
The projected point should be (10,10,-5). You can see by inspection that Pproj is 10 units perpendicular from the plane, and if it were in the plane, it would have y=10.
So how do we find this analytically?
The plane equation is Ax+By+Cz+d=0. What this equation means is "in order for a point (x,y,z) to be in the plane, it must satisfy Ax+By+Cz+d=0".
What is the Ax+By+Cz+d=0 equation for the plane drawn above?
The plane has normal n=(0,1,0). The d is found simply by using a test point already in the plane:
(0)x + (1)y + (0)z + d = 0
The point (0,10,0) is in the plane. Plugging in above, we find, d=-10. The plane equation is then 0x + 1y + 0z - 10 = 0 (if you simplify, you get y=10).
A nice interpretation of d is it speaks of the perpendicular distance you would need to translate the plane along its normal to have the plane pass through the origin.
Anyway, once we have d, we can find the |_ distance of any point to the plane by the following equation:
There are 3 possible classes of results for |_ distance to plane:
0: ON PLANE EXACTLY (almost never happens with floating point inaccuracy issues)
+1: >0: IN FRONT of plane (on normal side)
-1: <0: BEHIND plane (ON OPPOSITE SIDE OF NORMAL)
Anyway,
Which you can verify as correct by inspection in the diagram above
This answer is an addition to two existing answers.
I aim to show how the explanations by #tmpearce and #bobobobo boil down to the same thing, while at the same time providing quick answers to those who are merely interested in copying the equation best suited for their situation.
Method for planes defined by normal n and point o
This method was explained in the answer by #tmpearce.
Given a point-normal definition of a plane with normal n and point o on the plane, a point p', being the point on the plane closest to the given point p, can be found by:
p' = p - (n ⋅ (p - o)) × n
Method for planes defined by normal n and scalar d
This method was explained in the answer by #bobobobo.
Given a plane defined by normal n and scalar d, a point p', being the point on the plane closest to the given point p, can be found by:
p' = p - (n ⋅ p + d) × n
If instead you've got a point-normal definition of a plane (the plane is defined by normal n and point o on the plane) #bobobobo suggests to find d:
d = -n ⋅ o
and insert this into equation 2. This yields:
p' = p - (n ⋅ p - n ⋅ o) × n
A note about the difference
Take a closer look at equations 1 and 4. By comparing them you'll see that equation 1 uses n ⋅ (p - o) where equation 2 uses n ⋅ p - n ⋅ o. That's actually two ways of writing down the same thing:
n ⋅ (p - o) = n ⋅ p - n ⋅ o = n ⋅ p + d
One may thus choose to interpret the scalar d as if it were a 'pre-calculation'. I'll explain: if a plane's n and o are known, but o is only used to calculate n ⋅ (p - o),
we may as well define the plane by n and d and calculate n ⋅ p + d instead, because we've just seen that that's the same thing.
Additionally for programming using d has two advantages:
Finding p' now is a simpler calculation, especially for computers. Compare:
using n and o: 3 subtractions + 3 multiplications + 2 additions
using n and d: 0 subtractions + 3 multiplications + 3 additions.
Using d limits the definition of a plane to only 4 real numbers (3 for n + 1 for d), instead of 6 (3 for n + 3 for o). This saves ⅓ memory.
It's not sufficient to provide only the plane origin and the normal vector. This does define the 3d plane, however this does not define the coordinate system on the plane.
Think that you may rotate your plane around the normal vector with regard to its origin (i.e. put the normal vector at the origin and "rotate").
You may however find the distance of the projected point to the origin (which is obviously invariant to rotation).
Subtract the origin from the 3d point. Then do a cross product with the normal direction. If your normal vector is normalized - the resulting vector's length equals to the needed value.
EDIT
A complete answer would need an extra parameter. Say, you supply also the vector that denotes the x-axis on your plane.
So we have vectors n and x. Assume they're normalized.
The origin is denoted by O, your 3D point is p.
Then your point is projected by the following:
x = (p - O) dot x
y = (p - O) dot (n cross x)
Let V = (orig_x,orig_y,orig_z) - (point_x,point_y,point_z)
N = (normal_dx,normal_dy,normal_dz)
Let d = V.dotproduct(N);
Projected point P = V + d.N
I think you should slightly change the way you describe the plane. Indeed, the best way to describe the plane is via a vector n and a scalar c
(x, n) = c
The (absolute value of the) constant c is the distance of the plane from the origin, and is equal to (P, n), where P is any point on the plane.
So, let P be your orig point and A' be the projection of a new point A onto the plane. What you need to do is find a such that A' = A - a*n satisfies the equation of the plane, that is
(A - a*n, n) = (P, n)
Solving for a, you find that
a = (A, n) - (P, n) = (A, n) - c
which gives
A' = A - [(A, n) - c]n
Using your names, this reads
c = orig_x*normal_dx + orig_y*normal_dy+orig_z*normal_dz;
a = point_x*normal_dx + point_y*normal_dy + point_z*normal_dz - c;
planar_x = point_x - a*normal_dx;
planar_y = point_y - a*normal_dy;
planar_z = point_z - a*normal_dz;
Note: your code would save one scalar product if instead of the orig point P you store c=(P, n), which means basically 25% less flops for each projection (in case this routine is used many times in your code).
Let r be the point to project and p be the result of the projection. Let c be any point on the plane and let n be a normal to the plane (not necessarily normalised). Write p = r + m d for some scalar m which will be seen to be indeterminate if their is no solution.
Since (p - c).n = 0 because all points on the plane satisfy this restriction one has (r - c).n + m(d . n) = 0 and so m = [(c - r).n]/[d.n] where the dot product (.) is used. But if d.n = 0 there is no solution. For example if d and n are perpendicular to one another no solution is available.

Angle between 3 vertices

For example, GetAngle((0,0),(100,0),(100,100)) = 90. How could I find the angle between 3 2D Points.
Given points A, B, and C, you want the angle between AB and AC? First compute the vectors AB and AC -- it's just the coordinates of B minus coordinates of A and likewise for AC. Take the dot product of the two vectors. This is just the product of the x coordinates plus the product of the y coordinates of the vectors. Divide this number by the length of AB, and again by the length of AC. This result is the cosine of the angle between the two vectors, so take the arccos() and you have it.
The problem with using just the dot product here is that it is unstable near 0 or 180 degrees -- the slope of acos() approaches infinity near +/- 1.0 which will cause you to lose precision.
To fix this, you can compute a pseudo-cross product, and use atan2(), as follows:
// given A, B, C are 2D points:
BA= B - A; CA= C - A // vector subtraction, to get vector between points
dot= BA.x * CA.x + BA.y * CA.y
pcross= BA.x * CA.y - BA.y * CA.x
angle= atan2(pcross, dot) // this should be the angle BAC, in radians
This should be numerically robust unless one of the legs of the angle has zero length.
Note that this will also give you a signed angle, depending on whether BAC goes clockwise or counterclockwise; the acos() method will always give you a positive value. Of course, if you want only a positive angle, you can take abs(angle); the atan2() method will still be more robust, and probably faster.
Use the dot product:
(a,b,c) dot (d,e,f) = ad + be + bf.
A dot B = length(A)*length(B)* cos(theta)
theta = arccos((A dot B)/(length(A)*length(B)) is the angle between vectors A and B.
This is easy if you have some basic knowledge of linear algebra.
A vector v (in a linear algebra sense, not std::vector ;) ) is a tuple v = (x,y,z).
The norm is the length of the vector |v| = sqrt(xx + yy + z*z)
The inner product of two vectors v1 = (x1, y1, z1) and v2 = (x2, y2, z2) is v1·v2 = x1*x2 + y1*y2 + z1*z2
The angle of vectors v1 and v2 is a = acos(v1·v2/(|v1|*|v2|))