int
make_file(int fd)
{
int ret, count, i, fd1, offset;
unsigned char b[256*4];
count = 0;
offset = part_offset("FILE");
if (offset <= 0)
return -1;
printf("making FILE...\n");
printf("offset %o\n", offset);
while (1) {
memset(b, 0, sizeof(b));
write_block(fd, offset+count, b);
count++;
if (ret < 256*4)
break;
}
printf("%d blocks\n", count);
return 0;
}
int
make_lod2(int fd)
{
int ret, count, i, fd1, offset;
unsigned char b[256*4];
printf("making LOD2...\n");
fd1 = open(lod2_filename, O_RDONLY);
if (fd1 < 0) {
perror(lod2_filename);
return -1;
}
count = 0;
offset = part_offset("LOD2");
printf("offset %o\n", offset);
while (1) {
ret = read(fd1, b, 256*4);
if (ret <= 0)
break;
/* LOD2 start XXX */
write_block(fd, offset/*041674*/+count, b);
count++;
if (ret < 256*4)
break;
}
printf("%d blocks\n", count);
return 0;
}
tf::Transform process_measured_points(tf::Vector3 point1, tf::Vector3 point2, tf::Vector3 point3)
{
tf::Transform pose;
//0.0546165, 0.030832, 0.0565557
double part_x_offset, part_y_offset, part_z_offset;
part_x_offset = 0.0546165;
part_y_offset = 0.030832;
part_z_offset = 0.0565557;
tf::Vector3 part_offset(part_x_offset, part_y_offset, part_z_offset);
/*part_x_offset = point1.x() - part_center.x();
part_y_offset = point1.y() - part_center.y();
part_z_offset = point1.z() - part_center.z();*/
ROS_INFO_STREAM("Part offsets x, y, z: "<<part_x_offset<<", "<<part_y_offset<<", "<<part_z_offset);
tf::Vector3 x_axis, y_axis, z_axis;
x_axis = point2 - point3;
//x_axis = point3-point2;
ROS_INFO_STREAM("X vector for part: "<< x_axis.x() <<", "<<x_axis.y()<<", "<< x_axis.z());
y_axis = point1 - point2;
//y_axis=point1-point2;
z_axis = tf::tfCross(x_axis, y_axis);
x_axis.normalize();
ROS_INFO_STREAM("Normalized X vector for part: "<< x_axis.x() <<", "<<x_axis.y()<<", "<< x_axis.z());
y_axis.normalize();
z_axis.normalize();
tf::Matrix3x3 part_o;
part_o.setValue(x_axis.getX(), y_axis.getX(), z_axis.getX(), x_axis.getY(), y_axis.getY(), z_axis.getY(),
x_axis.getZ(), y_axis.getZ(), z_axis.getZ());
double yaw, pitch, roll;
part_o.getEulerYPR(yaw, pitch, roll);
tf::Quaternion part_q;
part_q.setRPY(roll, pitch, yaw);
//tf::Transform part_pose;
pose.setRotation(part_q);
pose.setOrigin(tf::Vector3(0, 0, 0));
tf::Vector3 offset_inpart = pose * part_offset;
//double part_o_x, part_o_y, part_o_z;
tf::Vector3 part_or;
part_or = point1 - offset_inpart;
//part_o_x = point1.x() + part_x_offset;
//part_o_y = point1.y() + part_y_offset;
//part_o_z = point1.z() + part_z_offset;
tf::Vector3 origin(part_or.x(), part_or.y(), part_or.z());
ROS_INFO_STREAM("Part origin " << origin.x()<<", "<< origin.y()<<", "<< origin.z());
pose.setOrigin(origin);
return pose;
}
int
make_mcr1(int fd)
{
int ret, count, i, fd1, offset;
unsigned char b[256*4];
printf("making MCR1...\n");
fd1 = open(mcr_filename, O_RDONLY);
if (fd1 < 0) {
perror(mcr_filename);
return -1;
}
count = 0;
offset = part_offset("MCR1");
printf("offset %o\n", offset);
while (1) {
ret = read(fd1, b, 256*4);
if (ret <= 0)
break;
#ifndef NEED_SWAP
swapbytes((unsigned int *)b);
#endif
/* MCR1 start XXX */
write_block(fd, offset/*021*/+count, b);
count++;
if (ret < 256*4)
break;
}
printf("%d blocks\n", count);
return 0;
}
int
zero_part(int fd, char *name)
{
int ret, count, i, fd1, offset, size;
unsigned char b[256*4];
offset = part_offset(name);
size = part_size(name);
if (offset <= 0)
return -1;
printf("making %s...\n", name);
printf("offset %o\n", offset);
for (count = 0; count < size; count++) {
memset(b, 0, sizeof(b));
write_block(fd, offset+count, b);
}
printf("%d blocks\n", count);
return 0;
}
