Projecting an arc through two points about a sphere - c++

I'm trying to solve a problem where I need to essentially project an arc from P0 some distance through P1 about a sphere of a known radius, however I have a fairly limited knowledge solving this sort of problem outside of the standard Cartesian context.
To simplify, if I were doing this on a plane I would simply do the following
float distance = 30;
Vector3 direction = (P1 - P0).Normalize();
Vector3 endPoint = P0 + (direction * distance);
I'm essentially trying to see if there is a way I can solve that same sort of problem, only on a sphere instead of on a plane. This get's conceptually difficult as I need to account for the fact that P0 and P1 can (and will) be at arbitrarily different altitudes on the sphere.
I have a feeling that I will potentially need to convert from Cartesian to spherical space, but finding a jump off point here has proven very difficult.
Thoughts?

Let P0 and P1 be points, and consider a sphere of radius r.
First, say that P0 and P1 lie on the sphere, and that we want to travel a distance d. First note that we need P0 ≠ P1, just like in the linear algebra case. (otherwise, P0 = P1 and so there is no unique arc through P0 and P1.) Similarly, we need P0 ≠ -P1, as otherwise there is no unique arc through P0 and P1.
Now the arc from P0 to P1 lies on what is known as a great circle of the sphere. Basically, this is an intersection of the sphere with some plane through O = (0,0,0). More precisely, this is the unique plane through the points O, P1 and P2.
Some basic linear algebra will show you that this is the plane perpendicular to the vector P0 × P1, the cross product of P0 and P1. Note that the order of P0 and P1 is important in this product: in general, P0 × P1 ≠ P1 × P0, and if we took P1 × P0 instead then the direction of motion would be reversed. (i.e. we would be going from P1 to P0 instead.) Therefore the "direction" of P0 to P1 is given by a rotation about P0 × P1.
Now consider general points P0, P1. As melak47 points out, we can first project P0 and P1 onto the sphere, so we can get the rotation above in the "direction" P0 to P1. To project these onto the sphere, we further need that both P0 and P1 are non-zero, i.e. are not the origin. We also run into a problem when P0 and P1 lie on the same line through the origin, because then they project to the same point (or antipodal points) on the sphere and so don't define a unique arc: so in general, we need to check that P0 and P1 are linearly independent. Now note that P0 lies on the sphere of radius ||P0||, where ||P0|| is the norm of the vector P0.
Note that the radius r introduced above hasn't actually been used anywhere yet, and so we can set r = ||P0||. To travel a total distance of d on the arc P0 to P1', where P1' is P1 projected onto the sphere of radius ||P0||, the angle becomes d/||P0||, as above. The good news is that P0 × P1 is still a valid axis of rotation.
So in summary, you want to rotate P0 about P0 × P1 by d/||P0|| radians. In pseudo-code,
float d;
Vector3 P0, P1;
float r = norm(P0);
float angle = d / r;
Vector3 axis = crossProduct(P0, P1);
Matrix3 rotationMatrix = RotationMatrix(angle, axis);
Vector3 endPoint = rotationMatrix * P0;
Checking that P0 and P1 are linearly independent is the same as checking that P0 × P1 is non-zero, so in general you may want to check that axis != Vector3(0,0,0) before constructing the rotation matrix.
If you instead want to travel in the direction P0 to P1 on the arc P0' to P1, where P0' is P0 projected onto the sphere of radius ||P1||, we first need to replace r = norm(P0) by r = norm(P1) (obviously). Further, P0' = ||P1||/||P0|| * P0, and so we also need to replace rotationMatrix * P0 by rotationMatrix * (r / norm(P0) * P0), or rotationMatrix * r * normalize(P0) if you prefer.

IMO, your question doesn't have a single answer. It seems to be a 2D problem that occurs in the plane formed by the two given points and the center of the sphere.
But you need to give more constraints on the trajectory. It could be a straight line, an Archimedean spiral, a logarithmic spiral, a spline... or whatever you like.

Related

How to compute quaternion from max and min coordinate values of the Cube?

I am facing some problem in creating the orientation of a cube (Bounding Box of the detected objects in ROS). I know the max and min coordinate values (xmin, xmax, ymin, ymax, zmin, zmax). So, I can easily find the vertex of the objects Bounding BOX (Cube) which are
[xmin ymin zmin;
xmax ymin zmin;
xmax ymax zmin;
xmin ymax zmin;
xmin ymin zmax;
xmax ymin zmax;
xmax ymax zmax;
xmin ymax zmax]
Now how can I create the quaternion from these vertex in order to get the orientation of the bounding box? I know that a quaternion is a set of 4 numbers, [x y z w], which represents rotations the following way:
// RotationAngle is in radians
x = RotationAxis.x * sin(RotationAngle / 2)
y = RotationAxis.y * sin(RotationAngle / 2)
z = RotationAxis.z * sin(RotationAngle / 2)
w = cos(RotationAngle / 2)
How to get the RotationAxis and RotationAngle when knowing the vertex of the object (in my case is a cube or 3D rectangle)?
Thanks
(I don't know what ROS is, but here's an abstract discussion of your question.)
First of all, a "rotation" starts from a "from" state and ends at a "to" state. You know your "to" state, but you'll have to specify the "from" state. Let's assume your box starts at a default state, with one corner at (0, 0, 0) and another at (1, 1, 1).
This default box state, like your final one is an "axis-aligned" box. Now, the box might have rotated 90, 180, or 270 degrees, but if different faces of the box are not distinct from each other, this might not matter. This scenario will have a 0-degree rotation, and the quaternion representing this rotation is trivially calculated to be (0, 0, 0, 1). You'll still need scale and translation (which are also trivial to compute) to get from that default state to your BB, but no rotation.
Now, if the faces are actually different, then we'll indeed have a rotation. Let's call it an "axis-aligned rotation" or AAR, which can take a default box to any one of 24 different states. Think of it like this: the axis of rotation can be any of the 6 basis vectors in either direction (+x, -x, +y, -y, +z, -z), and the angle can be 0, 90, 180, or 270 degrees (6 * 4 is 24!)
Each case, when you think about it like that, completely defines a rotation quaternion which is trivial to construct. The problem then, becomes finding with of the 24 rotations we have.
I can think of two mental models for this: either pick one "front" face and one "top" face for your box and find out where they ended up in the "to" state (6 places for "front" and 4 places for "top".) Or you can pick an "origin" vertex and a "neighbor" vertex and find where they ended up (8 places for "origin" and 3 places for "neighbor".) In either case, you probably would have a 24-entry table to pre-calculated quaternions, which you will choose from based on your "rotation".
The way you created your bounding box by using the minimum and maximum values leads to a bounding box aligned to the x, y, and z axes. Thus, it doesn't have any rotation and its quaternion would be [0, 0, 0, 1].
Getting the rotation matrix of randomly oriented cubes or rectangles is not complicated if you know which vertex relates to which corner of the cube/ rectangle. Then you can just use two perpendicular edges as the x and y axes in the objects coordinate frame. After normalizing these vectors you can create the z axis in the object frame by building the cross product of the x and y vectors. The columns of the 3x3 rotation matrix are these three vectors:
| x_0 y_0 z_0 |
R = | x_1 y_1 z_1 |
| x_2 y_2 z_2 |
You can search the internet for resources to get the rotation axis and quaternion from the rotation matrix.
For general objects, you can use the same method if you have the vertices of points on two perpendicular axes in the object coordinate frame. If you don't know the corespondig point in the object frame for your vertices, it gets more complicated. If your vertices are uniformly covering your object (e.g., vertices from 3D cameras or laser scans), you can use Principal Component Analysis to find the principal axes of the vertices. These axes form the columns of the rotation matrix (make sure the principal axes form a right-handed fram, i.e., the determinat of the rotation matrix is +1).
If these methods also don't apply to you, you could search for pose estimation methods in the context of point clouds (e.g.: Point Cloud Library PCL, Open3D).
Code (from the tutorial) for the extraction of the OBB:
pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
feature_extractor.setInputCloud (cloud);
feature_extractor.compute ();
pcl::PointXYZ min_point_OBB;
pcl::PointXYZ max_point_OBB;
pcl::PointXYZ position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
Eigen::Vector3f major_vector, middle_vector, minor_vector;
Eigen::Vector3f mass_center;
feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
feature_extractor.getMassCenter (mass_center);
So to get the final OBB - The coordinates:
Eigen::Vector3f p1 (min_point_OBB.x, min_point_OBB.y, min_point_OBB.z);
Eigen::Vector3f p2 (min_point_OBB.x, min_point_OBB.y, max_point_OBB.z);
Eigen::Vector3f p3 (max_point_OBB.x, min_point_OBB.y, max_point_OBB.z);
Eigen::Vector3f p4 (max_point_OBB.x, min_point_OBB.y, min_point_OBB.z);
Eigen::Vector3f p5 (min_point_OBB.x, max_point_OBB.y, min_point_OBB.z);
Eigen::Vector3f p6 (min_point_OBB.x, max_point_OBB.y, max_point_OBB.z);
Eigen::Vector3f p7 (max_point_OBB.x, max_point_OBB.y, max_point_OBB.z);
Eigen::Vector3f p8 (max_point_OBB.x, max_point_OBB.y, min_point_OBB.z);
p1 = rotational_matrix_OBB * p1 + position;
p2 = rotational_matrix_OBB * p2 + position;
p3 = rotational_matrix_OBB * p3 + position;
p4 = rotational_matrix_OBB * p4 + position;
p5 = rotational_matrix_OBB * p5 + position;
p6 = rotational_matrix_OBB * p6 + position;
p7 = rotational_matrix_OBB * p7 + position;
p8 = rotational_matrix_OBB * p8 + position
Bu rotational_matrix_OBB how to get?

OpenGl Rotate object on Y axis to look at another object

So like in a topic I got 2 objects one i moving around (on z and x axis) the other one is static but should rotate around y axis to always like a look at the other... and i am fighting with this already a week
what i got now is
vector from 1object to 2object and actual look at(also vector) of the 2object
i'am calculating angel betwean this two vectors and adding this to rotattion.y of the 2 object but its not working properly
any idea how to make it work? btw i'am using eular angel transforms
pseudCode:
vectorFrom1to2 = vector1 - vector2;
lookatVectorof2ndObject;
i normalize both of them and then
float angle = acos(dot(vectorFrom1to2, lookatVectorof2ndObject));
object2.rotateY = angle;
i dont know where i do mistake
As a general rule of thumb, which proved itself true in many situations I observed is: As soon as you find yourself calculating angles from vectors, you are most likely doing something in a more unnecessarily complicated way than necessary.
All you need is a basis transformation which transforms the first object's local coordinate system to make its local Z axis point towards the second object. You can do this with a simple rotation matrix (provided you have a matrix/vector library ready to facilitate this more easily).
So, provided you have object 1 with position p1 and object 2 with position p2 and you want p1 to rotate towards p2, then the rotation matrix can be obtained as follows:
(I am just using GLSL pseudo syntax here)
vec3 p1 = ... // <- position of first object
vec3 p2 = ... // <- position of second object
vec3 d = normalize(p2 - p1)
vec3 r = cross(vec3(0.0, 1.0, 0.0), d)
= vec3(d.z, 0, -d.x)
mat3 m = mat3(d.z, 0, -d.x, // <- first column ('right' vector)
0, 1, 0, // <- second column (keep Y)
d.x, 0, d.z) // <- third column (map Z to point towards p2)
When transforming the vertices v of the first object with m by: v' = m * v you get the Z axis of object p1 to point towards the position of p2, all formulated in the same "world" coordinate system.

Draw arbitrary plane from plane equation, OpenGL

I have a plane defined by the standard plane equation a*x + b*y + c*z + d = 0, which I would like to be able to draw using OpenGL. How can I derive the four points needed to draw it as a quadrilateral in 3D space?
My plane type is defined as:
struct Plane {
float x,y,z; // plane normal
float d;
};
void DrawPlane(const Plane & p)
{
???
}
EDIT:
So, rethinking the question, what I actually wanted was to draw a discreet representation of a plane in 3D space, not an infinite plane.
Base on the answer provided by #a.lasram, I have produced this implementation, which doest just that:
void DrawPlane(const Vector3 & center, const Vector3 & planeNormal, float planeScale, float normalVecScale, const fColorRGBA & planeColor, const fColorRGBA & normalVecColor)
{
Vector3 tangent, bitangent;
OrthogonalBasis(planeNormal, tangent, bitangent);
const Vector3 v1(center - (tangent * planeScale) - (bitangent * planeScale));
const Vector3 v2(center + (tangent * planeScale) - (bitangent * planeScale));
const Vector3 v3(center + (tangent * planeScale) + (bitangent * planeScale));
const Vector3 v4(center - (tangent * planeScale) + (bitangent * planeScale));
// Draw wireframe plane quadrilateral:
DrawLine(v1, v2, planeColor);
DrawLine(v2, v3, planeColor);
DrawLine(v3, v4, planeColor);
DrawLine(v4, v1, planeColor);
// And a line depicting the plane normal:
const Vector3 pvn(
(center[0] + planeNormal[0] * normalVecScale),
(center[1] + planeNormal[1] * normalVecScale),
(center[2] + planeNormal[2] * normalVecScale)
);
DrawLine(center, pvn, normalVecColor);
}
Where OrthogonalBasis() computes the tangent and bi-tangent from the plane normal.
To see the plane as if it's infinite you can find 4 quad vertices so that the clipped quad and the clipped infinite plane form the same polygon. Example:
Sample 2 random points P1 and P2 on the plane such as P1 != P2.
Deduce a tangent t and bi-tangent b as
t = normalize(P2-P1); // get a normalized tangent
b = cross(t, n); // the bi-tangent is the cross product of the tangent and the normal
Compute the bounding sphere of the view frustum. The sphere would have a diameter D (if this step seems difficult, just set D to a large enough value such as the corresponding sphere encompasses the frustum).
Get the 4 quad vertices v1 , v2 , v3 and v4 (CCW or CW depending on the choice of P1 and P2):
v1 = P1 - t*D - b*D;
v2 = P1 + t*D - b*D;
v3 = P1 + t*D + b*D;
v4 = P1 - t*D + b*D;
One possibility (possibly not the cleanest) is to get the orthogonal vectors aligned to the plane and then choose points from there.
P1 = < x, y, z >
t1 = random non-zero, non-co-linear vector with P1.
P2 = norm(P1 cross t1)
P3 = norm(P1 cross P2)
Now all points in the desired plane are defined as a starting point plus a linear combination of P2 and P3. This way you can get as many points as desired for your geometry.
Note: the starting point is just your plane normal < x, y, z > multiplied by the distance from the origin: abs(d).
Also of interest, with clever selection of t1, you can also get P2 aligned to some view. Say you are looking at the x, y plane from some z point. You might want to choose t1 = < 0, 1, 0 > (as long as it isn't co-linear to P1). This yields P2 with 0 for the y component, and P3 with 0 for the x component.

Need rotation matrix for opengl 3D transformation

The problem is I have two points in 3D space where y+ is up, x+ is to the right, and z+ is towards you. I want to orientate a cylinder between them that is the length of of the distance between both points, so that both its center ends touch the two points. I got the cylinder to translate to the location at the center of the two points, and I need help coming up with a rotation matrix to apply to the cylinder, so that it is orientated the correct way. My transformation matrix for the entire thing looks like this:
translate(center point) * rotateX(some X degrees) * rotateZ(some Z degrees)
The translation is applied last, that way I can get it to the correct orientation before I translate it.
Here is what I have so far for this:
mat4 getTransformation(vec3 point, vec3 parent)
{
float deltaX = point.x - parent.x;
float deltaY = point.y - parent.y;
float deltaZ = point.z - parent.z;
float yRotation = atan2f(deltaZ, deltaX) * (180.0 / M_PI);
float xRotation = atan2f(deltaZ, deltaY) * (180.0 / M_PI);
float zRotation = atan2f(deltaX, deltaY) * (-180.0 / M_PI);
if(point.y < parent.y)
{
zRotation = atan2f(deltaX, deltaY) * (180.0 / M_PI);
}
vec3 center = vec3((point.x + parent.x)/2.0, (point.y + parent.y)/2.0, (point.z + parent.z)/2.0);
mat4 translation = Translate(center);
return translation * RotateX(xRotation) * RotateZ(zRotation) * Scale(radius, 1, radius) * Scale(0.1, 0.1, 0.1);
}
I tried a solution given down below, but it did not seem to work at all
mat4 getTransformation(vec3 parent, vec3 point)
{
// moves base of cylinder to origin and gives it unit scaling
mat4 scaleFactor = Translate(0, 0.5, 0) * Scale(radius/2.0, 1/2.0, radius/2.0) * cylinderModel;
float length = sqrtf(pow((point.x - parent.x), 2) + pow((point.y - parent.y), 2) + pow((point.z - parent.z), 2));
vec3 direction = normalize(point - parent);
float pitch = acos(direction.y);
float yaw = atan2(direction.z, direction.x);
return Translate(parent) * Scale(length, length, length) * RotateX(pitch) * RotateY(yaw) * scaleFactor;
}
After running the above code I get this:
Every black point is a point with its parent being the point that spawned it (the one before it) I want the branches to fit into the points. Basically I am trying to implement the space colonization algorithm for random tree generation. I got most of it, but I want to map the branches to it so it looks good. I can use GL_LINES just to make a generic connection, but if I get this working it will look so much prettier. The algorithm is explained here.
Here is an image of what I am trying to do (pardon my paint skills)
Well, there's an arbitrary number of rotation matrices satisfying your constraints. But any will do. Instead of trying to figure out a specific rotation, we're just going to write down the matrix directly. Say your cylinder, when no transformation is applied, has its axis along the Z axis. So you have to transform the local space Z axis toward the direction between those two points. I.e. z_t = normalize(p_1 - p_2), where normalize(a) = a / length(a).
Now we just need to make this a full 3 dimensional coordinate base. We start with an arbitrary vector that's not parallel to z_t. Say, one of (1,0,0) or (0,1,0) or (0,0,1); use the scalar product ·(also called inner, or dot product) with z_t and use the vector for which the absolute value is the smallest, let's call this vector u.
In pseudocode:
# Start with (1,0,0)
mindotabs = abs( z_t · (1,0,0) )
minvec = (1,0,0)
for u_ in (0,1,0), (0,0,1):
dotabs = z_t · u_
if dotabs < mindotabs:
mindotabs = dotabs
minvec = u_
u = minvec_
Then you orthogonalize that vector yielding a local y transformation y_t = normalize(u - z_t · u).
Finally create the x transformation by taking the cross product x_t = z_t × y_t
To move the cylinder into place you combine that with a matching translation matrix.
Transformation matrices are effectively just the axes of the space you're "coming from" written down as if seen from the other space. So the resulting matrix, which is the rotation matrix you're looking for is simply the vectors x_t, y_t and z_t side by side as a matrix. OpenGL uses so called homogenuous matrices, so you have to pad it to a 4×4 form using a 0,0,0,1 bottommost row and rightmost column.
That you can load then into OpenGL; if using fixed functio using glMultMatrix to apply the rotation, or if using shader to multiply onto the matrix you're eventually pass to glUniform.
Begin with a unit length cylinder which has one of its ends, which I call C1, at the origin (note that your image indicates that your cylinder has its center at the origin, but you can easily transform that to what I begin with). The other end, which I call C2, is then at (0,1,0).
I'd like to call your two points in world coordinates P1 and P2 and we want to locate C1 on P1 and C2 to P2.
Start with translating the cylinder by P1, which successfully locates C1 to P1.
Then scale the cylinder by distance(P1, P2), since it originally had length 1.
The remaining rotation can be computed using spherical coordinates. If you're not familiar with this type of coordinate system: it's like GPS coordinates: two angles; one around the pole axis (in your case the world's Y-axis) which we typically call yaw, the other one is a pitch angle (in your case the X axis in model space). These two angles can be computed by converting P2-P1 (i.e. the local offset of P2 with respect to P1) into spherical coordinates. First rotate the object with the pitch angle around X, then with yaw around Y.
Something like this will do it (pseudo-code):
Matrix getTransformation(Point P1, Point P2) {
float length = distance(P1, P2);
Point direction = normalize(P2 - P1);
float pitch = acos(direction.y);
float yaw = atan2(direction.z, direction.x);
return translate(P1) * scaleY(length) * rotateX(pitch) * rotateY(yaw);
}
Call the axis of the cylinder A. The second rotation (about X) can't change the angle between A and X, so we have to get that angle right with the first rotation (about Z).
Call the destination vector (the one between the two points) B. Take -acos(BX/BY), and that's the angle of the first rotation.
Take B again, ignore the X component, and look at its projection in the (Y, Z) plane. Take acos(BZ/BY), and that's the angle of the second rotation.

How to plot two equidistant moving points?

Imagine a surface which has got 2 points which are moving over the surface (resulting into various lines and curves) while adhering to the following conditions.
These two points are always and always at a constant C distance from each other.
Each point has it's own velocity and direction vector. Of course both these parameters will be restricted based on condition 1.
Ex. if point 1 has 0 velocity and point 2 is moving it will result into a circle like shape with point one being it's center.
I am unable to express these conditions in programming constructs. Can someone help me with that?
I am using OpenGL but even a pseudo code will do for me.
vec3 center;
while(1):
vec3 centerDirection = ...
center += centerDirection
float angle = ...
vec3 dir(cos(angle), 0, sin(angle))
vec3 p1 = center + (C/2) * dir
vec3 p2 = center - (C/2) * dir
draw p1
draw p2
This is a solution where you move the center of a circle and make sure p1 and p2 are two diametrically opposed points
Another solution :
while 1:
p1 += random
p2 += random
delta = p1-p2
delta.normalize()
p2 = p1 + C * delta
i.e you restrain p2, not both points.
In openGL I achieved is as below
PlotTwoPoints(-SPAN,0,+SPAN,0); //two points on x axis
Then I rotated the co-ordinate system and kept on drawing two points at above two locations.
Simple!