static struct solid *transform_poly(const struct solid *solid, int flip,
int key0, int key1, float angle)
{
struct solid *ret = snew(struct solid);
float vx, vy, ax, ay;
float vmatrix[9], amatrix[9], vmatrix2[9];
int i;
*ret = *solid; /* structure copy */
flip_poly(ret, flip);
/*
* Now rotate the polyhedron through the given angle. We must
* rotate about the Z-axis to bring the two vertices key0 and
* key1 into horizontal alignment, then rotate about the
* X-axis, then rotate back again.
*/
vx = ret->vertices[key1*3+0] - ret->vertices[key0*3+0];
vy = ret->vertices[key1*3+1] - ret->vertices[key0*3+1];
assert(APPROXEQ(vx*vx + vy*vy, 1.0));
vmatrix[0] = vx; vmatrix[3] = vy; vmatrix[6] = 0;
vmatrix[1] = -vy; vmatrix[4] = vx; vmatrix[7] = 0;
vmatrix[2] = 0; vmatrix[5] = 0; vmatrix[8] = 1;
ax = (float)cos(angle);
ay = (float)sin(angle);
amatrix[0] = 1; amatrix[3] = 0; amatrix[6] = 0;
amatrix[1] = 0; amatrix[4] = ax; amatrix[7] = ay;
amatrix[2] = 0; amatrix[5] = -ay; amatrix[8] = ax;
memcpy(vmatrix2, vmatrix, sizeof(vmatrix));
vmatrix2[1] = vy;
vmatrix2[3] = -vy;
for (i = 0; i < ret->nvertices; i++) {
MATMUL(ret->vertices + 3*i, vmatrix, ret->vertices + 3*i);
MATMUL(ret->vertices + 3*i, amatrix, ret->vertices + 3*i);
MATMUL(ret->vertices + 3*i, vmatrix2, ret->vertices + 3*i);
}
for (i = 0; i < ret->nfaces; i++) {
MATMUL(ret->normals + 3*i, vmatrix, ret->normals + 3*i);
MATMUL(ret->normals + 3*i, amatrix, ret->normals + 3*i);
MATMUL(ret->normals + 3*i, vmatrix2, ret->normals + 3*i);
}
return ret;
}
void KatanaSimCam::update () throw ()
{
// data
///< \todo consider offsets, focal point not at gripper center and not straight
_pose = _robot->get_pose();
// generate axis rotation matrices
#define CELSET(mat,xxx,yyy,val) \
mat[(xxx)%3][(yyy)%3] = val;
#define ROTMAT(mat,ang,axs) \
for (_i=0; _i<9; _i++) CELSET(mat,_i,_i/3,0.); \
CELSET(mat,axs,axs,1.); \
CELSET(mat,axs+1,axs+1,cos(ang)); \
CELSET(mat,axs+2,axs+2,cos(ang)); \
CELSET(mat,axs+1,axs+2,sin(ang)); \
CELSET(mat,axs+2,axs+1,-sin(ang));
ROTMAT(_rot1, _pose[5], 2);
ROTMAT(_rot2, _pose[4], 0);
ROTMAT(_rot3, _pose[3], 2);
// compute ZYZ rotation matrix
#define MATMUL(xxx,yyy,zzz) \
for (_i=0; _i<3; _i++) \
for (_j=0; _j<3; _j++) { \
zzz[_i][_j] = 0.; \
for (_k=0; _k<3; _k++) \
zzz[_i][_j] += xxx[_i][_k] * yyy[_k][_j]; \
}
MATMUL(_rot1, _rot2,_rot12);
MATMUL(_rot12,_rot3,_roto);
// apply camera translation
for (_i=0; _i<3; _i++) {
_d[_i] = 0.;
for (_j=0; _j<3; _j++)
_d[_i] += _roto[_i][_j] * (_object_xyz[_j] - _pose[_j]);
}
// check if object is in front of camera
currentObject.known = _current[0].known = false;
if (_d[2] > 0.) {
// project object onto sensor
_d[0] *= 320. / _d[2];
_d[1] *= 320. / _d[2];
// check if projected object within camera resolution
if (fabs(_d[0]) <= 320. && fabs(_d[1]) <= 240) {
currentObject.pos.x = _current[0].x = _d[0];
currentObject.pos.y = _current[0].y = _d[1];
_current[0].x += 320.;
_current[0].y += 240.;
currentObject.known = _current[0].known = true;
}
}
if (_draw) {
// visualize projected object
int bx=4, by=10;
for (int a=0; a<641; a+=bx) std::cout << '-';
std::cout << "--" << std::endl;
for (int b=0; b<481; b+=by) {
std::cout << '|';
for (int a=0; a<641; a+=bx)
std::cout << (_current[0].known && fabs(_current[0].x-a)<bx && fabs(_current[0].y-b)<by ? 'X' : a==320&&b==240 ? '+' : ' ');
std::cout << '|' << std::endl;
}
for (int a=0; a<641; a+=bx) std::cout << '-';
std::cout << "--" << std::endl;
}
}
