I was reading chapter 7 of "Competitive Programming 3" by Steven and Felix Halim. I then found this problem, given in picture:
Here two intersecting points of two circles and corresponding radius are also given. I have to find centres of two circles. They have given the solution code. But I didn't understand the technique behind it. The given code is:
I have searched many times, but I didn't find. Can anyone explain the technique?
Anyway, thanks in advance :)
Let's first visualize variables d2 and h defined in the code:
As we can see, d2 stands for the square of the distance between p1 and p2. Let's put d = sqrt(d2). Then d^2 = d2.
Using Pythagoras: r^2 = h^2 + d^2/4. Hence, h^2 = r^2 - d^2/4.
The unitary (norm = 1) vector in the direction of the line joining p1 and p2 is:
v := (p2 - p1)/d = (p2.x - p1.x, p2.y - p1.y)/d.
its perpendicular is
w := (p2.y - p1.y, p1.x - p2.x)/d
Now we can express c1 as a point in the perpendicular direction:
c1 = q + w*h = (p1 + p2)/2 + w*h,
because w has norm 1 and h is precisely the distance between c1 and q. Therefore,
c1 = (p1.x + p2.x, p1.y + p2.y)/2 + (p2.y - p1.y, p1.x - p2.x)*h/d
where
h/d = sqrt(r^2 - d^2/4)/d = sqrt(r^2/d2 - 1/4)
which explains the code.
Notes
From the picture that r is always ge than d/2. Thus, r^2 ≥ d^2/4 or (r/d)^2 ≥ 1/4 and there is no need to check whether det < 0, because it never is (provided the circles intersect).
The derivation above will actually produce two solutions for c1, one to the right of the blue line from p1 to p2, which is the one in the drawing, and another on the left of said line. In fact, these correspond to the equations
c1 = q ± w*h = q + w*(±h)
In order to decide whether we should use +h or -h, we can apply one of the well-known criteria for establishing whether a point lies on the left or the right of a directed segment. For example, we could compute the sign of the determinant
| 1 p1.x p1.y |
D = | 1 p2.x p2.y | = (p2.x-p1.x)(c1.y-p1.y) - (p2.y-p1.y)(c1.x-p1.x)
| 1 c1.x c1.y |
which will have a sign for +h and the opposite for -h. The value of h that makes D < 0 is the one that leaves c1 on the right of the segment from p1 to p2.
Apply Pythagoras in the right triangles. This gives you the distances between the midpoint of p1p2 and the centers.
From unit vectors parallel then orthogonal to p1p2, the offset vectors are easily obtained.
There is a triangle with points P1(x1,y1),P2(x2,y2),P3(x3,y3) on an XY plane.
The final position after transformation are known to us, P1'(x,y) and P2'(x,y)
How can I find the third point?
Using slope (or distance) formula gives two solutions (one is mirror image of another). Assuming the transformation is a combination of translation and rotation, how do I get the new coordinates of the final point P3' ?
If you already have got solution using distance formula, you need only to choose what mirror point is needed. To clarify, find sign of cross product of P1P2 vector and P1P3 vector. Then find sign of cross product of of P1'P2' vector and P1'Px vector. If signs differ, get another point.
CrossProduct = (P2.X - P1.X) * (P3.Y - P1.Y) - (P2.Y - P1.Y) * (P3.X - P1.X)
In general case you can find transformation matrix coefficients and apply this matrix to the third point
c -s 0
M = s c 0
dx dy 1
equation system
c * x1 + s * y1 + dx = x1'
-s * x1 + c * y1 + dy = y1'
c * x2 + s * y2 + dx = x2'
-s * x2 + c * y2 + dy = y2'
solve it for unknown c, s, dx, dy (really c and s are not independent)
I have triangle: a, b, c. Each vertex has a value: va, vb, vc. In my software the user drags point p around inside and outside of this triangle. I use barycentric coordinates to determine the value vp at p based on va, vb, and vc. So far, so good.
Now I want to limit p so that vp is within range min and max. If a user chooses p where vp is < min or > max, how can I find the point closest to p where vp is equal to min or max, respectively?
Edit: Here is an example where I test each point. Light gray is within min/max. How can I find the equations of the lines that make up the min/max boundary?
a = 200, 180
b = 300, 220
c = 300, 300
va = 1
vb = 1.4
vc = 3.2
min = 0.5
max = 3.5
Edit: FWIW, so far first I get the barycentric coordinates v,w for p using the triangle vertices a, b, c (standard stuff I think, but looks like this). Then to get vp:
u = 1 - w - v
vp = va * u + vb * w + vc * v
That is all fine. My trouble is that I need the line equations for min/max so I can choose a new position for p when vp is out of range. The new position for p is the point closest to p on the min or max line.
Note that p is an XY coordinate and vp is a value for that coordinate determined by the triangle and the values at each vertex. min and max are also values. The two line equations I need will give me XY coordinates for which the values determined by the triangle are min or max.
It doesn't matter if barycentric coordinates are used in the solution.
The trick is to use the ratio of value to cartesian distance to extend each triangle edge until it hits min or max. Easier to see with a pic:
The cyan lines show how the triangle edges are extended, the green Xs are points on the min or max lines. With just 2 of these points we know the slope if the line. The yellow lines show connecting the Xs aligns with the light gray.
The math works like this, first get the value distance between vb and vc:
valueDistBtoC = vc - vb
Then get the cartesian distance from b to c:
cartesianDistBtoC = b.distance(c)
Then get the value distance from b to max:
valueDistBtoMax = max - vb
Now we can cross multiply to get the cartesian distance from b to max:
cartesianDistBtoMax = (valueDistBtoMax * cartesianDistBtoC) / valueDistBtoC
Do the same for min and also for a,b and c,a. The 6 points are enough to restrict the position of p.
Consider your triangle to actually be a 3D triangle, with points (ax,ay,va), (bx,by,vb), and (cx,cy,vc). These three points define a plane, containing all the possible p,vp triplets obtainable through barycentric interpolation.
Now think of your constraints as two other planes, at z>=max and z<=min. Each of these planes intersects your triangle's plane along an infinite line; the infinite beam between them, projected back down onto the xy plane, represents the area of points which satisfy the constraints. Once you have the lines (projected down), you can just find which (if either) is violated by a particular point, and move it onto that constraint (along a vector which is perpendicular to the constraint).
Now I'm not sure about your hexagon, though. That's not the shape I would expect.
Mathematically speaking the problem is simply a change of coordinates. The more difficult part is finding a good notation for the quantities involved.
You have two systems of coordinates: (x,y) are the cartesian coordinates of your display and (v,w) are the baricentric coordinates with respect to the vectors (c-a),(b-a) which determine another (non orthogonal) system.
What you need is to find the equation of the two lines in the (x,y) system, then it will be easy to project the point p on these lines.
To achieve this you could explicitly find the matrix to pass from (x,y) coordinates to (v,w) coordinates and back. The function you are using toBaryCoords makes this computation to find the coordinates (v,w) from (x,y) and we can reuse that function.
We want to find the coefficients of the transformation from world coordinates (x,y) to barycentric coordinates (v,w). It must be in the form
v = O_v + x_v * x + y_v * y
w = O_w + x_w * x + y_w * y
i.e.
(v,w) = (O_v,O_w) + (x_v,y_y) * (x,y)
and you can determine (O_v,O_w) by computing toBaryCoord(0,0), then find (x_v,x_w) by computing the coordinates of (1,0) and find (y_v,y_w)=toBaryCoord(1,0) - (O_v,O_w) and then find (y_v,y_w) by computing (y_v,y_w) = toBaryCoord(0,1)-(O_v,O_w).
This computation requires calling toBaryCoord three times, but actually the coefficients are computed inside that routine every time, so you could modify it to compute at once all six values.
The value of your function vp can be computed as follows. I will use f instead of v because we are using v for a baricenter coordinate. Hence in the following I mean f(x,y) = vp, fa = va, fb = vb, fc = vc.
You have:
f(v,w) = fa + (fb-fa)*v + (fc-fa)*w
i.e.
f(x,y) = fa + (fb-fa) (O_v + x_v * x + y_v * y) + (fc-fa) (O_w + x_w * x + y_w * y)
where (x,y) are the coordinates of your point p. You can check the validity of this equation by inserting the coordinates of the three vertices a, b, c and verify that you obtain the three values fa, fb and fc. Remember that the barycenter coordinates of a are (0,0) hence O_v + x_v * a_x + y_v * a_y = 0 and so on... (a_x and a_y are the x,y coordinates of the point a).
If you let
q = fa + (fb_fa)*O_v + (fc-fa)*O_w
fx = (fb-fa)*x_v + (fc-fa) * x_w
fy = (fb-fa)*y_v + (fc-fa) * y_w
you get
f(x,y) = q + fx*x + fy * y
Notice that q, fx and fy can be computed once from a,b,c,fa,fb,fc and you can reuse them if you only change the coordinates (x,y) of the point p.
Now if f(x,y)>max, you can easily project (x,y) on the line where max is achieved. The coordinates of the projection are:
(x',y') = (x,y) - [(x,y) * (fx,fy) - max + q]/[(fx,fy) * (fx,fy)] (fx,fy)
Now. You would like to have the code. Well here is some pseudo-code:
toBarycoord(Vector2(0,0),a,b,c,O);
toBarycoord(Vector2(1,0),a,b,c,X);
toBarycoord(Vector2(0,1),a,b,c,Y);
X.sub(O); // X = X - O
Y.sub(O); // Y = Y - O
V = Vector2(fb-fa,fc-fa);
q = fa + V.dot(O); // q = fa + V*O
N = Vector2(V.dot(X),V.dot(Y)); // N = (V*X,V*Y)
// p is the point to be considered
f = q + N.dot(p); // f = q + N*p
if (f > max) {
Vector2 tmp;
tmp.set(N);
tmp.multiply((N.dot(p) - max + q)/(N.dot(N))); // scalar multiplication
p.sub(tmp);
}
if (f < min) {
Vector2 tmp;
tmp.set(N);
tmp.multiply((N.dot(p) - min + q)/(N.dot(N))); // scalar multiplication
p.sum(tmp);
}
We think of the problem as follows: The three points are interpreted as a triangle floating in 3D space with the value being the Z-axis and the cartesian coordinates mapped to the X- and Y- axes respectively.
Then the question is to find the gradient of the plane that is defined by the three points. The lines where the plane intersects with the z = min and z = max planes are the lines you want to restrict your points to.
If you have found a point p where v(p) > max or v(p) < min we need to go in the direction of the steepest slope (the gradient) until v(p + k * g) = max or min respectively. g is the direction of the gradient and k is the factor we need to find. The coordinates you are looking for (in the cartesian coordinates) are the corresponding components of p + k * g.
In order to determine g we calculate the orthonormal vector that is perpendicular to the plane that is determined by the three points using the cross product:
// input: px, py, pz,
// output: p2x, p2y
// local variables
var v1x, v1y, v1z, v2x, v2y, v2z, nx, ny, nz, tp, k,
// two vectors pointing from b to a and c respectively
v1x = ax - bx;
v1y = ay - by;
v1z = az - bz;
v2x = cx - bx;
v2y = cy - by;
v2z = cz - bz;
// the cross poduct
nx = v2y * v1z - v2z * v1y;
ny = v2z * v1x - v2x * v1z;
nz = v2x * v1y - v2y * v1x;
// using the right triangle altitude theorem
// we can calculate the vector that is perpendicular to n
// in our triangle we are looking for q where p is nz, and h is sqrt(nx*nx+ny*ny)
// the theorem says p*q = h^2 so p = h^2 / q - we use tp to disambiguate with the point p - we need to negate the value as it points into the opposite Z direction
tp = -(nx*nx + ny*ny) / nz;
// now our vector g = (nx, ny, tp) points into the direction of the steepest slope
// and thus is perpendicular to the bounding lines
// given a point p (px, py, pz) we can now calculate the nearest point p2 (p2x, p2y, p2z) where min <= v(p2z) <= max
if (pz > max){
// find k
k = (max - pz) / tp;
p2x = px + k * nx;
p2y = py + k * ny;
// proof: p2z = v = pz + k * tp = pz + ((max - pz) / tp) * tp = pz + max - pz = max
} else if (pz < min){
// find k
k = (min - pz) / tp;
p2x = px + k * nx;
p2y = py + k * ny;
} else {
// already fits
p2x = px;
p2y = py;
}
Note that obviously if the triangle is vertically oriented (in 2D it's not a triangle anymore actually), nz becomes zero and tp cannot be calculated. That's because there are no more two lines where the value is min or max respectively. For this case you will have to choose another value on the remaining line or point.
I have a single of x-y coordinate system
This diagram should represent what you've told me.
The key point, is to express [x2],[y2] in CS1. (I can't use latex here so let's assume that [A] means the vector A, |A| is the length of the vector A)
[v2] = v2x * [x2] + v2y * [y2]
Since we have well defined [v1] and [d2], we can calculate [x']
[x`] = [d2] - [v1]
From [x'] we can calculate x2
[x2] = (|x2|/|x'|)[x`] = (|x1|/|x'|)[x'] since |x1| = |x2|
From x2 we can calculate y2, although I don't remember how. It's a simple 90° rotation.
Should be this:
y2x = - x2y
y2y = x2x
Once we have expressed x2,y2 in CS1, we can compute v2
v2 = v2x * [x2] + v2y * [y2] = v2x * (x2x*[x1]+x2y*[y1]) + v2y * (y2x*[x1]+y2y*[y1])
= (v2xx2x + v2yy2x)[x1] + (v2xx2y + v2yy2y) [y1] // Hope I didn't make any mistake here :)
And finally
[X] = [v1] + [v2]
I think the best option is to create a vector class and do all the math using vector algebra. You just need to define 3 operation: Addition, ScalarMultiplication, 90Rotation.
I am trying to do inverse kinematics for a serial chain of arbitrarily many links.
In the following paper, I have found an example for how to calculate the Jacobian matrix.
Entry (i, j) = v[j] * (s[i] - p[j])
where:
v[j] is the unit vector of the axis of
rotation for joint j
s[i] is the position (int world
coords?) of joint i
p[j] is the position (in world
coords?) of joint j
The paper says that this works if j is a rotational joint with a single degree of freedom. But my rotational joints have no constraints on their rotation. What formula do I then want? (Or am I possibly misunderstanding the term "degree of freedom"?)
This question is old, but I'll answer anyway, as it is something I have thought about but never really gotten around to implement.
Rotational joints with no constraints are called ball joints or spherical joints; they have 3 degrees of freedom. You can use the formula in the tutorial for spherical joints also, if you parameterize each spherical joint in terms of 3 rotational (revolute) joints of one degree of freedom each.
For example: Let N be the number of spherical joints. Suppose each joint has a local transformation T_local[i] and a world transformation
T_world[i] = T_local[0] * ... * T_local[i]
Let R_world[i][k], k = 0, 1, 2, be the k-th column of the rotation matrix of T_world[i]. Define the 3 * N joint axes as
v[3 * j + 0] = R_world[i][0]
v[3 * j + 1] = R_world[i][1]
v[3 * j + 2] = R_world[i][2]
Compute the Jacobian J for some end-effector s[i], using the formula of the tutorial. All coordinates are in the world frame.
Using for example the pseudo-inverse method gives a displacement dq that moves the end-effector in a given direction dx.
The length of dq is 3 * N. Define
R_dq[j] =
R_x[dq[3 * j + 0]] *
R_y[dq[3 * j + 1]] *
R_z[dq[3 * j + 2]]
for j = 0, 1, ..., N-1, where R_x, R_y, R_z are the transformation matrices for rotation about the x-, y-, and z-axes.
Update the local transformations:
T_local[j] := T_local[j] * R_dq[j]
and repeat from the top to move the end-effector in other directions dx.
Let me suggest a simpler approach to Jacobians in the context of arbitrary many DOFs: Basically, the Jacobian tells you, how far each joint moves, if you move the end effector frame in some arbitrarily chosen direction. Let f(θ) be the forward kinematics, where θ=[θ1,...,θn] are the joints. Then you can obtain the Jacobian by differentiating the forward kinematics with respect to the joint variables:
Jij = dfi/dθj
is your manipulator's Jacobian. Inverting it would give you the inverse kinematics with respect to velocities. It can still be useful though, if you want to know how far each joint has to move if you want to move your end effector by some small amount Δx in any direction (because on position level, this would effectively be a linearization):
Δθ=J-1Δx
Hope that this helps.