rw::math::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....
// TODO: case [0 0 -1]... Why is it happening at what can be done to undo it?
rw::math::Vector3D<> dir_z((obj_x - sphere_x), (obj_y - sphere_y), (obj_z - sphere_z));
dir_z = normalize(dir_z);
rw::math::Vector3D<> downPlane(0.0,0.0,-1.0);
rw::math::Vector3D<> dir_x = cross(downPlane,dir_z);
dir_x = normalize(dir_x);
rw::math::Vector3D<> dir_y = cross(dir_z,dir_x);
dir_y = normalize(dir_y);
rw::math::Rotation3D<> rot_out (dir_z,dir_y,dir_x); // [x y z]
rw::math::Vector3D<> pos_out(sphere_x,sphere_y,sphere_z);
rw::math::Transform3D<> out(pos_out,rot_out);
return out;
}
static void
setup(void)
{
if (in.name == NULL) {
in.name = "stdin";
in.fd = STDIN_FILENO;
} else {
in.fd = open(in.name, O_RDONLY, 0);
if (in.fd < 0)
err(EXIT_FAILURE, "%s", in.name);
/* NOTREACHED */
/* Ensure in.fd is outside the stdio descriptor range */
in.fd = redup_clean_fd(in.fd);
}
getfdtype(&in);
if (files_cnt > 1 && !(in.flags & ISTAPE)) {
errx(EXIT_FAILURE, "files is not supported for non-tape devices");
/* NOTREACHED */
}
if (out.name == NULL) {
/* No way to check for read access here. */
out.fd = STDOUT_FILENO;
out.name = "stdout";
} else {
#define OFLAGS \
(O_CREAT | (ddflags & (C_SEEK | C_NOTRUNC) ? 0 : O_TRUNC))
out.fd = open(out.name, O_RDWR | OFLAGS, DEFFILEMODE);
/*
* May not have read access, so try again with write only.
* Without read we may have a problem if output also does
* not support seeks.
*/
if (out.fd < 0) {
out.fd = open(out.name, O_WRONLY | OFLAGS, DEFFILEMODE);
out.flags |= NOREAD;
}
if (out.fd < 0) {
err(EXIT_FAILURE, "%s", out.name);
/* NOTREACHED */
}
/* Ensure out.fd is outside the stdio descriptor range */
out.fd = redup_clean_fd(out.fd);
}
getfdtype(&out);
/*
* Allocate space for the input and output buffers. If not doing
* record oriented I/O, only need a single buffer.
*/
if (!(ddflags & (C_BLOCK|C_UNBLOCK))) {
if ((in.db = malloc(out.dbsz + in.dbsz - 1)) == NULL) {
err(EXIT_FAILURE, NULL);
/* NOTREACHED */
}
out.db = in.db;
} else if ((in.db =
malloc((u_int)(MAX(in.dbsz, cbsz) + cbsz))) == NULL ||
(out.db = malloc((u_int)(out.dbsz + cbsz))) == NULL) {
err(EXIT_FAILURE, NULL);
/* NOTREACHED */
}
in.dbp = in.db;
out.dbp = out.db;
/* Position the input/output streams. */
if (in.offset)
pos_in();
if (out.offset)
pos_out();
/*
* Truncate the output file; ignore errors because it fails on some
* kinds of output files, tapes, for example.
*/
if ((ddflags & (C_OF | C_SEEK | C_NOTRUNC)) == (C_OF | C_SEEK))
(void)ftruncate(out.fd, (off_t)out.offset * out.dbsz);
/*
* If converting case at the same time as another conversion, build a
* table that does both at once. If just converting case, use the
* built-in tables.
*/
if (ddflags & (C_LCASE|C_UCASE)) {
#ifdef NO_CONV
/* Should not get here, but just in case... */
errx(EXIT_FAILURE, "case conv and -DNO_CONV");
/* NOTREACHED */
#else /* NO_CONV */
u_int cnt;
if (ddflags & C_ASCII || ddflags & C_EBCDIC) {
if (ddflags & C_LCASE) {
for (cnt = 0; cnt < 256; ++cnt)
casetab[cnt] = tolower(ctab[cnt]);
} else {
for (cnt = 0; cnt < 256; ++cnt)
casetab[cnt] = toupper(ctab[cnt]);
}
} else {
if (ddflags & C_LCASE) {
for (cnt = 0; cnt < 256; ++cnt)
casetab[cnt] = tolower(cnt);
} else {
for (cnt = 0; cnt < 256; ++cnt)
casetab[cnt] = toupper(cnt);
}
}
ctab = casetab;
#endif /* NO_CONV */
}
(void)gettimeofday(&st.start, NULL); /* Statistics timestamp. */
}
static void
setup(void)
{
u_int cnt;
struct timeval tv;
if (in.name == NULL) {
in.name = "stdin";
in.fd = STDIN_FILENO;
} else {
in.fd = open(in.name, O_RDONLY, 0);
if (in.fd == -1)
err(1, "%s", in.name);
}
getfdtype(&in);
if (files_cnt > 1 && !(in.flags & ISTAPE))
errx(1, "files is not supported for non-tape devices");
if (out.name == NULL) {
/* No way to check for read access here. */
out.fd = STDOUT_FILENO;
out.name = "stdout";
} else {
#define OFLAGS \
(O_CREAT | (ddflags & (C_SEEK | C_NOTRUNC) ? 0 : O_TRUNC))
out.fd = open(out.name, O_RDWR | OFLAGS, DEFFILEMODE);
/*
* May not have read access, so try again with write only.
* Without read we may have a problem if output also does
* not support seeks.
*/
if (out.fd == -1) {
out.fd = open(out.name, O_WRONLY | OFLAGS, DEFFILEMODE);
out.flags |= NOREAD;
}
if (out.fd == -1)
err(1, "%s", out.name);
}
getfdtype(&out);
/*
* Allocate space for the input and output buffers. If not doing
* record oriented I/O, only need a single buffer.
*/
if (!(ddflags & (C_BLOCK | C_UNBLOCK))) {
if ((in.db = malloc(out.dbsz + in.dbsz - 1)) == NULL)
err(1, "input buffer");
out.db = in.db;
} else if ((in.db = malloc(MAX(in.dbsz, cbsz) + cbsz)) == NULL ||
(out.db = malloc(out.dbsz + cbsz)) == NULL)
err(1, "output buffer");
in.dbp = in.db;
out.dbp = out.db;
/* Position the input/output streams. */
if (in.offset)
pos_in();
if (out.offset)
pos_out();
/*
* Truncate the output file. If it fails on a type of output file
* that it should _not_ fail on, error out.
*/
if ((ddflags & (C_OF | C_SEEK | C_NOTRUNC)) == (C_OF | C_SEEK) &&
out.flags & ISTRUNC)
if (ftruncate(out.fd, out.offset * out.dbsz) == -1)
err(1, "truncating %s", out.name);
/*
* If converting case at the same time as another conversion, build a
* table that does both at once. If just converting case, use the
* built-in tables.
*/
if (ddflags & (C_LCASE | C_UCASE)) {
if (ddflags & (C_ASCII | C_EBCDIC)) {
if (ddflags & C_LCASE) {
for (cnt = 0; cnt <= 0377; ++cnt)
casetab[cnt] = tolower(ctab[cnt]);
} else {
for (cnt = 0; cnt <= 0377; ++cnt)
casetab[cnt] = toupper(ctab[cnt]);
}
} else {
if (ddflags & C_LCASE) {
for (cnt = 0; cnt <= 0377; ++cnt)
casetab[cnt] = tolower((int)cnt);
} else {
for (cnt = 0; cnt <= 0377; ++cnt)
casetab[cnt] = toupper((int)cnt);
}
}
ctab = casetab;
}
(void)gettimeofday(&tv, (struct timezone *)NULL);
st.start = tv.tv_sec + tv.tv_usec * 1e-6;
}
static void
setup(void)
{
if (in.name == NULL) {
in.name = "stdin";
in.fd = STDIN_FILENO;
} else {
in.fd = open(in.name, O_RDONLY, 0);
if (in.fd < 0) {
fprintf(stderr, "%s: cannot open for read: %s\n",
in.name, strerror(errno));
exit(1);
/* NOTREACHED */
}
/* Ensure in.fd is outside the stdio descriptor range */
in.fd = redup_clean_fd(in.fd);
}
getfdtype(&in);
if (files_cnt > 1 && !(in.flags & ISTAPE)) {
fprintf(stderr,
"files is not supported for non-tape devices\n");
exit(1);
/* NOTREACHED */
}
if (out.name == NULL) {
/* No way to check for read access here. */
out.fd = STDOUT_FILENO;
out.name = "stdout";
} else {
#define OFLAGS \
(O_CREAT | (ddflags & (C_SEEK | C_NOTRUNC) ? 0 : O_TRUNC))
out.fd = open(out.name, O_RDWR | OFLAGS, DEFFILEMODE);
/*
* May not have read access, so try again with write only.
* Without read we may have a problem if output also does
* not support seeks.
*/
if (out.fd < 0) {
out.fd = open(out.name, O_WRONLY | OFLAGS, DEFFILEMODE);
out.flags |= NOREAD;
}
if (out.fd < 0) {
fprintf(stderr, "%s: cannot open for write: %s\n",
out.name, strerror(errno));
exit(1);
/* NOTREACHED */
}
/* Ensure out.fd is outside the stdio descriptor range */
out.fd = redup_clean_fd(out.fd);
}
getfdtype(&out);
/*
* Allocate space for the input and output buffers. If not doing
* record oriented I/O, only need a single buffer.
*/
if (!(ddflags & (C_BLOCK|C_UNBLOCK))) {
if ((in.db = malloc(out.dbsz + in.dbsz - 1)) == NULL) {
exit(1);
/* NOTREACHED */
}
out.db = in.db;
} else if ((in.db =
malloc((u_int)(MAX(in.dbsz, cbsz) + cbsz))) == NULL ||
(out.db = malloc((u_int)(out.dbsz + cbsz))) == NULL) {
exit(1);
/* NOTREACHED */
}
in.dbp = in.db;
out.dbp = out.db;
/* Position the input/output streams. */
if (in.offset)
pos_in();
if (out.offset)
pos_out();
/*
* Truncate the output file; ignore errors because it fails on some
* kinds of output files, tapes, for example.
*/
if ((ddflags & (C_OF | C_SEEK | C_NOTRUNC)) == (C_OF | C_SEEK))
(void)ftruncate(out.fd, (off_t)out.offset * out.dbsz);
(void)gettimeofday(&st.start, NULL); /* Statistics timestamp. */
}
