I am at the moment trying to form transformation matrices which translational part should form a sphere around an object, given its the center.
Problem is though, that's not happening, as I'm only getting a circle out of it, which I don't understand.
The red dots are those points I've computed and the blue dot should be the center of the sphere..
std::vector<Transform3D<>> sphere(double dx, double dy, double dz, Transform3D<> T_toolFrame_TCP)
{
double r = 0.01; // Radius of the sphere - set to 0.50 cm (TODO: has to be checked if that also is accurate)
cout << endl;
cout << "Create a sphere" << endl;
double current_x = T_toolFrame_TCP.P()[0];
double current_y = T_toolFrame_TCP.P()[1];
double current_z = T_toolFrame_TCP.P()[2];
// Formula for sphere (x-x0)²+(y-y0)²+(z-z0)²=r²
// x: x = x_0 + rcos(theta)sin(phi)
// y: y = y_0 + rsin(theta)sin(phi)
// z: z = z_0 + rcos(phi)
// Angle range: 0 <= theta <= 2M_PI ; 0 <= phi <= M_PI
double obj_x = current_x + dx;
double obj_y = current_y + dy;
double obj_z = current_z + dz;
cout << obj_x <<" , "<< obj_y <<" , "<< obj_z << endl;
std::vector<Transform3D<>> out;
int i = 0;
for(double azimuthal = 0; azimuthal <= 2*M_PI ; azimuthal+=0.1 ) // In a circle
{
for(double polar = 0.34; polar <= M_PI-0.35 ; polar+=0.1 ) // Top to bottom
{
double sphere_x = obj_x + r*cos(azimuthal)*sin(polar);
double sphere_y = obj_y + r*sin(azimuthal)*sin(polar);
double sphere_z = obj_z + + r*cos(polar);
Transform3D<> transformation_matrix = transform(obj_x,obj_y,obj_z,sphere_x,sphere_y,sphere_z);
out.push_back(transformation_matrix);
}
}
cout << i << " " << out.size() << endl;
return out;
}
Transform3D<> pathPlanning::transform(double obj_x, double obj_y, double obj_z, double sphere_x, double sphere_y ,double sphere_z)
{
// Z-axis should be oriented towards the object.
// Rot consist of 3 direction vector [x,y,z] which describes how the axis should be oriented in the world space.
// Looking at the simulation the z-axis is the camera out. X, and Y describes the orientation of the camera.
// The vector are only for direction purposes, so they have to be normalized....
//cout << endl;
//cout << "inside Transform" << endl;
//cout << obj_x <<" "<< sphere_x << " ; " << obj_y <<" "<< sphere_y <<" ; "<< obj_z <<" "<< sphere_z << endl;
Vector3D<> dir_z(double(obj_x - sphere_x), double(obj_y - sphere_y), double(obj_z - sphere_z));
//Vector3D<> dir_z((sphere_x-obj_x), (sphere_y - obj_y), (sphere_z-obj_z));
dir_z = normalize(dir_z);
Vector3D<> downPlane(0.0,0.0,-1.0);
Vector3D<> dir_x = cross(downPlane,dir_z);
dir_x = normalize(dir_x);
Vector3D<> dir_y = cross(dir_z,dir_x);
dir_y = normalize(dir_y);
Rotation3D<> rot_out (dir_x,dir_y,dir_z); // [x y z]
Vector3D<> pos_out(sphere_x,sphere_y,sphere_z);
Transform3D<> out(pos_out,rot_out);
//cout << endl;
//cout << "desired: " << endl << out.R().e() << endl << endl << out.P().e() << endl;
return out;
}
Each Transform3D<> is just a 4x4 matrix which contains [x_vec y_vec z_vec position]
Vector3D<> is just a vector which contains 3 elements.
Each Rotation3D<> is just a 3x3 matrix which contains [x_vec,y_vec,z_vec].
Each Transform3D<> can be constructed from a Rotation3D<> and Vector3D<>
The objective of the sphere function is to find all the positions on the sphere created around the object. The function is given a dx,dy,dz, t_toolFrame_TCP which tells the function how far the center of the sphere should be according to its current position which is given by t_toolFrame_TCP.P().
The t_toolFrame_TCP I use is
6.12323e-17 6.12323e-17 -1 -3.55148e-18
-1 3.7494e-33 -6.12323e-17 0
0 1 6.12323e-17 0.091
0 0 0 1
For some reason are the points not forming a sphere which I have no clue why it is happening? could someone elaborate me on my problem?
