Map angles to a 0-1 range - math

Let's say you have two angles, and you label them 0 and 1. Then you have another angle x. You also know if you'll be going Clockwise or Counter Clockwise to get from angle 0 to angle 1. How do you calculate a number that can describe that third angle?
Examples:
Angle at 0
Angle at 1
Rotation Direction
Target Angle
Mapped number (x)
0°
90°
CCW
60°
2/3
90°
0°
CW
60°
1/3
0°
180°
CW
90°
1.5
0°
180°
CCW
90°
0.5
Problems I'm having:
When x can't be supported within 0 and 1 (I am fine with it just telling me it couldn't do it, but having the number would be cooler).
When switching from Counter-Clock-Wise (CCW) to CW.

Check the next approach:
def ratio(x, a, b, dircw = False):
if dircw:
if x > a:
x -= 360
if b > a:
b -= 360
else:
if b < a:
b += 360
if x < a:
x += 360
return (x-a)/(b-a)
print(ratio(60, 0, 90))
print(ratio(60, 0, 90, True))
print(ratio(60, 90, 0, True))
print(ratio(90, 0, 180, True))
print(ratio(90, 0, 180))
0.6666666666666666
1.1111111111111112
0.3333333333333333
1.5
0.5
We consider solution of linear equation (solve inverse of linear interpolation)
x = a*(1-t) + b*t
for unknown t.
We have to make normalization to provide b after a in cyclic manner in both directions - so b+- correction.
To get only positive results, we normalize also x.

Related

Understand Quaternions axis angle

I'am working with Madgwick algorithm who gives me a Quaternions for accelerometer and gyro.
So I can get the angle from q0 with this formula 2 * acors(q0) it's works I've tried and I got the good value. But now I don't understand how can I know for x or y has rotate to x° because I have only one angle with Quaternion.
For example imagine I have this Quaternion q0 to q3 {0,71, 0,18, -0,65, 0.30}, so for q0 equals to 0.71 I have an angle to 90°. but in my example x and y are different, so how can I know x is 90° and y is 20° for example, is it possible without using Euler angle?
I've tried this formula for x q1/sin(angle/2) but the result doesn't convince me....
If you have quaternion (u is unit vector, direction vector of rotation axis)
(cos(a/2), u * sin(a/2))
and want to know how vector V is transformed with this quaternion (in the end of Rotation Identity section)
V' = Vperp*cos(a) + (u x Vperp) * sin(a) + Vpara
where Vperp and Vpara are components of vector V perpendicular and parallel to vector u
Vpara = u * (u.dot.V)
Vperp = V - Vpara
Example:
let rotation axis (u) is (0.707, 0.707, 0), we want to know how
OX-aligned vector (1,0,0) will look after rotation by 180 degrees
Vpara = (0.707, 0.707, 0) * (0.707*1 + 0.707*0 + 0) = (0.5, 0.5, 0)
Vperp = (1, 0, 0) - (0.5, 0.5, 0) = (0.5, -0.5, 0)
V' = (0.5, -0.5, 0) * -1 + (u x Vperp) * 0 + (0.5, 0.5, 0) = (0, 1, 0)
(OX becomes OY)

Determining if a point lies between two bearings from a central point

I am trying to determine if a point lies between two bearings from a central point.
The diagram below attempts to explain things
I have a central point labelled A
I have two points (labelled B & C) which provide the boundaries of the search area (based on bearing only - there is no distance element required).
I'm trying to determine if point D is within the sector formed by A-B and A-C
I've calculated the bearings from A to each B & C
In my real scenario the angle created between the bearings can be anything from 0 to 360.
There are some similar questions & answers
however in my case I'm not interested in restricting my search to the radius of a circle. And there seems to be some implementation issues around angle size and the location of the points in terms of clockwise vs counter-clockwise
It seems so simple in theory but my maths is clearly not up to scratch :(
Any advice or pseudo-code would be greatly appreciated.
Here would be my approach:
calculate first bearing angle X
calculate second bearing angle Y
calculate angle Z towards point D
if X < Z < Y, return true; otherwise, return false
In your example it looks like you'd calculate Z ~ 90deg and find 45 < 90 < 135 (is your picture wrong? is says 315).
You can use something like the "atan2" function in whatever language you're using. This is an extension of the basic arctangent function which takes not just the slope but both the rise and run and instead of returning an angle from only a 180-degree range, it returns the true angle from a 360-degree range. So
Z = atan2(Dy, Dx)
Should give you the angle (possibly in radians; be careful) that you can compare to your bearings to tell whether you're inside the search. Note that the order of X and Y matter since the order is what defines which of the two sections is in the search area (X to Y gives ~90 deg in your picture, but Y to X gives ~270 deg).
You can calculate and compare the cross products of the vectors (AB X BD), and (AC X CD).
if (AB X BD) > 0, you have a counter clock wise turn
if (AC X CD) < 0, you have a clock wise turn
If both above tests are true, then the point D is in the sector BAC
This allows you to completely avoid using expensive trig functions.
class Point:
"""small class for point arithmetic convenience
"""
def __init__(self, x: float = 0, y: float = 0) -> None:
self.x = x
self.y = y
def __sub__(self, other: 'Point') -> 'Vector':
return Vector(self.x - other.x, self.y - other.y)
class Vector:
"""small class for vector arithmetic convenience
"""
def __init__(self, x: float = 0, y: float = 0) -> None:
self.x = x
self.y = y
def cross(self, other: 'Vector') -> float:
return (self.x * other.y) - (self.y * other.x)
def in_sector(A: Point, B: Point, C: Point, D: Point) -> bool:
# construct vectors:
ab = B - A
bd = D - B
ac = C - A
cd = D - C
print(f'ab x bc = {ab.cross(bd)}, ac x cd = {ac.cross(cd)}')
return ab.cross(bd) > 0 and ac.cross(cd) < 0
if __name__ == '__main__':
A = Point(0, 0)
B = Point(1, 1)
C = Point(-1, 1)
D = Point(0, 1)
print(f'D in sector ABC: {in_sector(A, B, C, D)}', end='\n\n')
print(f'D in sector ACB: {in_sector(A, C, B, D)}') # inverting the sector definition so D is now outside
Output:
ab x bc = 1, ac x cd = -1
D in sector ABC: True
ab x bc = -1, ac x cd = 1
D in sector ACB: False

Quaternion to matrix rotation only one axis

I have a quaternion that contains the rotation of the three axes (x, y, z) at the same time.
I want to convert this quaternion to a rotation matrix but only the rotation on the Y axis of the quaternion or of any of the other axes, without all three at the same time.
A possible route:
Transform unit vectors X=(1,0,0) and Z=(0,0,1) by the quaternion
Call these rotated vectors (x0,x1,x2) and (z0,z1,z2)
If the rotation would have been purely around Y, we would have:
(x0,x1,x2) = (cos(theta), 0, sin(theta))
(z0,z1,z2) = (-sin(theta), 0, cos(theta))
not used is (y0,y1,y2) = (0, 1, 0)
so, calculate
c = (x0+z2) / 2
and s = (x2-z0) / 2
then normalize to get c2 + s2 equal to 1
norm = sqrt(c * c + s * s)
if norm != 0:
c = c / norm
s = s / norm
(if the norm would be zero, there is not much we can do)
the angle would be atan2(c, s)
the rotation matrix would be [[c,0,-s],[0,1,0],[s,0,c]]

Getting direction vectors from 3d position and/or rotation

I have a camera, in which, I've obtained its 3d position and its 3d rotation (in degrees). The coordinate system is as follows:
The order is XYZ and is intrinsic.
pitch
roll
yaw
(aka right/front/top vector)
With this information, how can I calculate the Up and Front vectors?
Yaw rotation is about OY (vertical) axis and has matrix MY (for CCW positive)
Cos(Yaw) -Sin(Yaw) 0
Sin(Yaw) Cos(Yaw) 0
0 0 1
Pitch rotation is about OZ axis and has matrix MP
Cos(Pitch) 0 Sin(Pitch)
0 1 0
-Sin(Pitch) 0 Cos(Yaw)
Roll rotation is about OX axis and has matrix MR
1 0 0
0 Cos(Roll) -Sin(Roll)
0 Sin(Roll) Cos(Roll)
Now one has to multiply matrices MP, MR, MY in right order and get matrix M.
To find forward direction vector - multiply M and vector [1, 0, 0]. To get top direction vector - multiply M and vector [0, 1, 0]. To get side direction vector - multiply M and vector [1, 0, 0].
I cannot check right matrix multiplication order and the last result now. If you have any symbolic math software like Maple, Matlab - try it.
Result should look like this for forward
X = Cos(Pitch) * Cos(Yaw)
Y = Sin(Pitch)
Z = Cos(Pitch) * Sin(Yaw)
and up (perhaps wrong)
X = -Cos(Yaw) * Sin(Roll) - Sin(Yaw) * Sin(Pitch) * Cos(Roll)
Y = Sin(Yaw) * Sin(Roll) - Cos(Yaw) * Sin(Pitch) * Cos(Roll)
Z = Cos(Pitch) * Cos(Roll)

Determine which side of a line a point lies [duplicate]

I have a set of points. I want to separate them into 2 distinct sets. To do this, I choose two points (a and b) and draw an imaginary line between them. Now I want to have all points that are left from this line in one set and those that are right from this line in the other set.
How can I tell for any given point z whether it is in the left or in the right set? I tried to calculate the angle between a-z-b – angles smaller than 180 are on the right hand side, greater than 180 on the left hand side – but because of the definition of ArcCos, the calculated angles are always smaller than 180°. Is there a formula to calculate angles greater than 180° (or any other formula to chose right or left side)?
Try this code which makes use of a cross product:
public bool isLeft(Point a, Point b, Point c){
return ((b.X - a.X)*(c.Y - a.Y) - (b.Y - a.Y)*(c.X - a.X)) > 0;
}
Where a = line point 1; b = line point 2; c = point to check against.
If the formula is equal to 0, the points are colinear.
If the line is horizontal, then this returns true if the point is above the line.
Use the sign of the determinant of vectors (AB,AM), where M(X,Y) is the query point:
position = sign((Bx - Ax) * (Y - Ay) - (By - Ay) * (X - Ax))
It is 0 on the line, and +1 on one side, -1 on the other side.
You look at the sign of the determinant of
| x2-x1 x3-x1 |
| y2-y1 y3-y1 |
It will be positive for points on one side, and negative on the other (and zero for points on the line itself).
The vector (y1 - y2, x2 - x1) is perpendicular to the line, and always pointing right (or always pointing left, if you plane orientation is different from mine).
You can then compute the dot product of that vector and (x3 - x1, y3 - y1) to determine if the point lies on the same side of the line as the perpendicular vector (dot product > 0) or not.
Using the equation of the line ab, get the x-coordinate on the line at the same y-coordinate as the point to be sorted.
If point's x > line's x, the point is to the right of the line.
If point's
x < line's x, the point is to the left of the line.
If point's x == line's x, the point is on the line.
I implemented this in java and ran a unit test (source below). None of the above solutions work. This code passes the unit test. If anyone finds a unit test that does not pass, please let me know.
Code: NOTE: nearlyEqual(double,double) returns true if the two numbers are very close.
/*
* #return integer code for which side of the line ab c is on. 1 means
* left turn, -1 means right turn. Returns
* 0 if all three are on a line
*/
public static int findSide(
double ax, double ay,
double bx, double by,
double cx, double cy) {
if (nearlyEqual(bx-ax,0)) { // vertical line
if (cx < bx) {
return by > ay ? 1 : -1;
}
if (cx > bx) {
return by > ay ? -1 : 1;
}
return 0;
}
if (nearlyEqual(by-ay,0)) { // horizontal line
if (cy < by) {
return bx > ax ? -1 : 1;
}
if (cy > by) {
return bx > ax ? 1 : -1;
}
return 0;
}
double slope = (by - ay) / (bx - ax);
double yIntercept = ay - ax * slope;
double cSolution = (slope*cx) + yIntercept;
if (slope != 0) {
if (cy > cSolution) {
return bx > ax ? 1 : -1;
}
if (cy < cSolution) {
return bx > ax ? -1 : 1;
}
return 0;
}
return 0;
}
Here's the unit test:
#Test public void testFindSide() {
assertTrue("1", 1 == Utility.findSide(1, 0, 0, 0, -1, -1));
assertTrue("1.1", 1 == Utility.findSide(25, 0, 0, 0, -1, -14));
assertTrue("1.2", 1 == Utility.findSide(25, 20, 0, 20, -1, 6));
assertTrue("1.3", 1 == Utility.findSide(24, 20, -1, 20, -2, 6));
assertTrue("-1", -1 == Utility.findSide(1, 0, 0, 0, 1, 1));
assertTrue("-1.1", -1 == Utility.findSide(12, 0, 0, 0, 2, 1));
assertTrue("-1.2", -1 == Utility.findSide(-25, 0, 0, 0, -1, -14));
assertTrue("-1.3", -1 == Utility.findSide(1, 0.5, 0, 0, 1, 1));
assertTrue("2.1", -1 == Utility.findSide(0,5, 1,10, 10,20));
assertTrue("2.2", 1 == Utility.findSide(0,9.1, 1,10, 10,20));
assertTrue("2.3", -1 == Utility.findSide(0,5, 1,10, 20,10));
assertTrue("2.4", -1 == Utility.findSide(0,9.1, 1,10, 20,10));
assertTrue("vertical 1", 1 == Utility.findSide(1,1, 1,10, 0,0));
assertTrue("vertical 2", -1 == Utility.findSide(1,10, 1,1, 0,0));
assertTrue("vertical 3", -1 == Utility.findSide(1,1, 1,10, 5,0));
assertTrue("vertical 3", 1 == Utility.findSide(1,10, 1,1, 5,0));
assertTrue("horizontal 1", 1 == Utility.findSide(1,-1, 10,-1, 0,0));
assertTrue("horizontal 2", -1 == Utility.findSide(10,-1, 1,-1, 0,0));
assertTrue("horizontal 3", -1 == Utility.findSide(1,-1, 10,-1, 0,-9));
assertTrue("horizontal 4", 1 == Utility.findSide(10,-1, 1,-1, 0,-9));
assertTrue("positive slope 1", 1 == Utility.findSide(0,0, 10,10, 1,2));
assertTrue("positive slope 2", -1 == Utility.findSide(10,10, 0,0, 1,2));
assertTrue("positive slope 3", -1 == Utility.findSide(0,0, 10,10, 1,0));
assertTrue("positive slope 4", 1 == Utility.findSide(10,10, 0,0, 1,0));
assertTrue("negative slope 1", -1 == Utility.findSide(0,0, -10,10, 1,2));
assertTrue("negative slope 2", -1 == Utility.findSide(0,0, -10,10, 1,2));
assertTrue("negative slope 3", 1 == Utility.findSide(0,0, -10,10, -1,-2));
assertTrue("negative slope 4", -1 == Utility.findSide(-10,10, 0,0, -1,-2));
assertTrue("0", 0 == Utility.findSide(1, 0, 0, 0, -1, 0));
assertTrue("1", 0 == Utility.findSide(0,0, 0, 0, 0, 0));
assertTrue("2", 0 == Utility.findSide(0,0, 0,1, 0,2));
assertTrue("3", 0 == Utility.findSide(0,0, 2,0, 1,0));
assertTrue("4", 0 == Utility.findSide(1, -2, 0, 0, -1, 2));
}
First check if you have a vertical line:
if (x2-x1) == 0
if x3 < x2
it's on the left
if x3 > x2
it's on the right
else
it's on the line
Then, calculate the slope: m = (y2-y1)/(x2-x1)
Then, create an equation of the line using point slope form: y - y1 = m*(x-x1) + y1. For the sake of my explanation, simplify it to slope-intercept form (not necessary in your algorithm): y = mx+b.
Now plug in (x3, y3) for x and y. Here is some pseudocode detailing what should happen:
if m > 0
if y3 > m*x3 + b
it's on the left
else if y3 < m*x3 + b
it's on the right
else
it's on the line
else if m < 0
if y3 < m*x3 + b
it's on the left
if y3 > m*x3+b
it's on the right
else
it's on the line
else
horizontal line; up to you what you do
I wanted to provide with a solution inspired by physics.
Imagine a force applied along the line and you are measuring the torque of the force about the point. If the torque is positive (counterclockwise) then the point is to the "left" of the line, but if the torque is negative the point is the "right" of the line.
So if the force vector equals the span of the two points defining the line
fx = x_2 - x_1
fy = y_2 - y_1
you test for the side of a point (px,py) based on the sign of the following test
var torque = fx*(py-y_1)-fy*(px-x_1)
if torque>0 then
"point on left side"
else if torque <0 then
"point on right side"
else
"point on line"
end if
Assuming the points are (Ax,Ay) (Bx,By) and (Cx,Cy), you need to compute:
(Bx - Ax) * (Cy - Ay) - (By - Ay) * (Cx - Ax)
This will equal zero if the point C is on the line formed by points A and B, and will have a different sign depending on the side. Which side this is depends on the orientation of your (x,y) coordinates, but you can plug test values for A,B and C into this formula to determine whether negative values are to the left or to the right.
basically, I think that there is a solution which is much easier and straight forward, for any given polygon, lets say consist of four vertices(p1,p2,p3,p4), find the two extreme opposite vertices in the polygon, in another words, find the for example the most top left vertex (lets say p1) and the opposite vertex which is located at most bottom right (lets say ). Hence, given your testing point C(x,y), now you have to make double check between C and p1 and C and p4:
if cx > p1x AND cy > p1y ==> means that C is lower and to right of p1
next
if cx < p2x AND cy < p2y ==> means that C is upper and to left of p4
conclusion, C is inside the rectangle.
Thanks :)
#AVB's answer in ruby
det = Matrix[
[(x2 - x1), (x3 - x1)],
[(y2 - y1), (y3 - y1)]
].determinant
If det is positive its above, if negative its below. If 0, its on the line.
Here's a version, again using the cross product logic, written in Clojure.
(defn is-left? [line point]
(let [[[x1 y1] [x2 y2]] (sort line)
[x-pt y-pt] point]
(> (* (- x2 x1) (- y-pt y1)) (* (- y2 y1) (- x-pt x1)))))
Example usage:
(is-left? [[-3 -1] [3 1]] [0 10])
true
Which is to say that the point (0, 10) is to the left of the line determined by (-3, -1) and (3, 1).
NOTE: This implementation solves a problem that none of the others (so far) does! Order matters when giving the points that determine the line. I.e., it's a "directed line", in a certain sense. So with the above code, this invocation also produces the result of true:
(is-left? [[3 1] [-3 -1]] [0 10])
true
That's because of this snippet of code:
(sort line)
Finally, as with the other cross product based solutions, this solution returns a boolean, and does not give a third result for collinearity. But it will give a result that makes sense, e.g.:
(is-left? [[1 1] [3 1]] [10 1])
false
Issues with the existing solution:
While I found Eric Bainville's answer to be correct, I found it entirely inadequate to comprehend:
How can two vectors have a determinant? I thought that applied to matrices?
What is sign?
How do I convert two vectors into a matrix?
position = sign((Bx - Ax) * (Y - Ay) - (By - Ay) * (X - Ax))
What is Bx?
What is Y? Isn't Y meant to be a Vector, rather than a scalar?
Why is the solution correct - what is the reasoning behind it?
Moreover, my use case involved complex curves rather than a simple line, hence it requires a little re-jigging:
Reconstituted Answer
Point a = new Point3d(ax, ay, az); // point on line
Point b = new Point3d(bx, by, bz); // point on line
If you want to see whether your points are above/below a curve, then you would need to get the first derivative of the particular curve you are interested in - also known as the tangent to the point on the curve. If you can do so, then you can highlight your points of interest. Of course, if your curve is a line, then you just need the point of interest without the tangent. The tangent IS the line.
Vector3d lineVector = curve.GetFirstDerivative(a); // where "a" is a point on the curve. You may derive point b with a simple displacement calculation:
Point3d b = new Point3d(a.X, a.Y, a.Z).TransformBy(
Matrix3d.Displacement(curve.GetFirstDerivative(a))
);
Point m = new Point3d(mx, my, mz) // the point you are interested in.
The Solution:
return (b.X - a.X) * (m.Y - a.Y) - (b.Y - a.Y) * (m.X - a.X) < 0; // the answer
Works for me! See the proof in the photo above. Green bricks satisfy the condition, but the bricks outside were filtered out! In my use case - I only want the bricks that are touching the circle.
Theory behind the answer
I will return to explain this. Someday. Somehow...
An alternative way of getting a feel of solutions provided by netters is to understand a little geometry implications.
Let pqr=[P,Q,R] are points that forms a plane that is divided into 2 sides by line [P,R]. We are to find out if two points on pqr plane, A,B, are on the same side.
Any point T on pqr plane can be represented with 2 vectors: v = P-Q and u = R-Q, as:
T' = T-Q = i * v + j * u
Now the geometry implications:
i+j =1: T on pr line
i+j <1: T on Sq
i+j >1: T on Snq
i+j =0: T = Q
i+j <0: T on Sq and beyond Q.
i+j: <0 0 <1 =1 >1
---------Q------[PR]--------- <== this is PQR plane
^
pr line
In general,
i+j is a measure of how far T is away from Q or line [P,R], and
the sign of i+j-1 implicates T's sideness.
The other geometry significances of i and j (not related to this solution) are:
i,j are the scalars for T in a new coordinate system where v,u are the new axes and Q is the new origin;
i, j can be seen as pulling force for P,R, respectively. The larger i, the farther T is away from R (larger pull from P).
The value of i,j can be obtained by solving the equations:
i*vx + j*ux = T'x
i*vy + j*uy = T'y
i*vz + j*uz = T'z
So we are given 2 points, A,B on the plane:
A = a1 * v + a2 * u
B = b1 * v + b2 * u
If A,B are on the same side, this will be true:
sign(a1+a2-1) = sign(b1+b2-1)
Note that this applies also to the question: Are A,B in the same side of plane [P,Q,R], in which:
T = i * P + j * Q + k * R
and i+j+k=1 implies that T is on the plane [P,Q,R] and the sign of i+j+k-1 implies its sideness. From this we have:
A = a1 * P + a2 * Q + a3 * R
B = b1 * P + b2 * Q + b3 * R
and A,B are on the same side of plane [P,Q,R] if
sign(a1+a2+a3-1) = sign(b1+b2+b3-1)
equation of line is y-y1 = m(x-x1)
here m is y2-y1 / x2-x1
now put m in equation and put condition on y < m(x-x1) + y1 then it is left side point
eg.
for i in rows:
for j in cols:
if j>m(i-a)+b:
image[i][j]=0
A(x1,y1) B(x2,y2) a line segment with length L=sqrt( (y2-y1)^2 + (x2-x1)^2 )
and a point M(x,y)
making a transformation of coordinates in order to be the point A the new start and B a point of the new X axis
we have the new coordinates of the point M
which are
newX = ((x-x1)(x2-x1)+(y-y1)(y2-y1)) / L
from (x-x1)*cos(t)+(y-y1)*sin(t) where cos(t)=(x2-x1)/L, sin(t)=(y2-y1)/L
newY = ((y-y1)(x2-x1)-(x-x1)(y2-y1)) / L
from (y-y1)*cos(t)-(x-x1)*sin(t)
because "left" is the side of axis X where the Y is positive, if the newY (which is the distance of M from AB) is positive, then it is on the left side of AB (the new X axis)
You may omit the division by L (allways positive), if you only want the sign

Resources