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; } }