Eigen::Affine3d rigid2d(
const Eigen::Vector2d & center,
const double angle)
{
using namespace Eigen;
Affine3d t = Affine3d::Identity();
const Vector3d c3(center(0),center(1),0);
t.translate(c3);
t.rotate(AngleAxisd(angle,Vector3d(0,0,1)));
t.translate(-c3);
return t;
}
/**
* \ingroup Geometry
* \brief Initialize this object with 3 screen coordinates and 3 mirror coordinates.
* Virtual screen coordinates are computed from the inputs given.
* \param _realScreen coordinates of 3 points on the screenCoord
* \param _realMirror coordinates of 3 points that define the mirror plane
**/
void ScreenCoordinatesExtractor::init( const vector<Vector3d> &_realScreen, const vector<Vector3d> &_realMirror )
{
if (_realScreen.size() != 3 || _realMirror.size() !=3 )
throw std::runtime_error("I need exactly 3 points to define a plane!");
virtualScreenCoordinates.resize(3);
screenCoordinates.resize(3);
for (int i=0; i<3; i++)
screenCoordinates[i]=_realScreen[i];
Hyperplane<double,3> screenPlane,mirrorPlane;
screenPlane = Hyperplane<double,3>::Through( _realScreen[0],_realScreen[1],_realScreen[2] );
mirrorPlane = Hyperplane<double,3>::Through( _realMirror[0],_realMirror[1],_realMirror[2] );
if ( screenPlane.isApprox(mirrorPlane) )
throw std::runtime_error("Planes are parallel!");
// Save the planes informations for debug purposes
mirrorOffset=mirrorPlane.offset();
screenOffset=screenPlane.offset();
mirrorNormal=mirrorPlane.normal();
screenNormal=screenPlane.normal();
// Correct the two planes for the marker offset (3.3 mm), translating them back along their normal of markerOffset
if (hasMarkerOffset)
{ Affine3d transScreen = Affine3d::Identity();
Affine3d transMirror = Affine3d::Identity();
transScreen.translate ( screenNormal*markerOffset );
transMirror.translate ( mirrorNormal*markerOffset );
screenPlane.transform(transScreen);
mirrorPlane.transform(transMirror);
//cerr << "SCREEN\n" << transScreen.matrix() << endl << "MIRROR\n" << transMirror.matrix() << endl;
}
for (int i=0; i<3; i++)
{ Vector3d tmp = mirrorPlane.projection(Vector3d(screenCoordinates[i]));
virtualScreenCoordinates[i] = Vector3d(tmp)*2 -screenCoordinates[i];
}
}
bool pre_draw(igl::opengl::glfw::Viewer & viewer)
{
using namespace Eigen;
using namespace std;
if(viewer.core.is_animating)
{
// Interpolate pose and identity
RotationList anim_pose(pose.size());
for(int e = 0;e<pose.size();e++)
{
anim_pose[e] = pose[e].slerp(anim_t,Quaterniond::Identity());
}
// Propagate relative rotations via FK to retrieve absolute transformations
RotationList vQ;
vector<Vector3d> vT;
igl::forward_kinematics(C,BE,P,anim_pose,vQ,vT);
const int dim = C.cols();
MatrixXd T(BE.rows()*(dim+1),dim);
for(int e = 0;e<BE.rows();e++)
{
Affine3d a = Affine3d::Identity();
a.translate(vT[e]);
a.rotate(vQ[e]);
T.block(e*(dim+1),0,dim+1,dim) =
a.matrix().transpose().block(0,0,dim+1,dim);
}
// Compute deformation via LBS as matrix multiplication
U = M*T;
// Also deform skeleton edges
MatrixXd CT;
MatrixXi BET;
igl::deform_skeleton(C,BE,T,CT,BET);
viewer.data().set_vertices(U);
viewer.data().set_edges(CT,BET,sea_green);
viewer.data().compute_normals();
anim_t += anim_t_dir;
anim_t_dir *= (anim_t>=1.0 || anim_t<=0.0?-1.0:1.0);
}
return false;
}
void display()
{
using namespace igl;
using namespace std;
using namespace Eigen;
const float back[4] = {30.0/255.0,30.0/255.0,50.0/255.0,0};
glClearColor(back[0],back[1],back[2],0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
if(is_animating)
{
double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION;
if(t > 1)
{
t = 1;
is_animating = false;
}
Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized();
auto & camera = s.camera;
camera.orbit(q.conjugate());
}
glEnable(GL_DEPTH_TEST);
glEnable(GL_NORMALIZE);
lights();
push_scene();
// Draw a nice floor
glEnable(GL_DEPTH_TEST);
glPushMatrix();
const double floor_offset =
-2./bbd*(V.col(1).maxCoeff()-Vmid(1));
glTranslated(0,floor_offset,0);
const float GREY[4] = {0.5,0.5,0.6,1.0};
const float DARK_GREY[4] = {0.2,0.2,0.3,1.0};
glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
draw_floor(GREY,DARK_GREY);
glPopMatrix();
push_object();
// Set material properties
glDisable(GL_COLOR_MATERIAL);
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, SILVER_AMBIENT);
glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, SILVER_DIFFUSE );
glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, SILVER_SPECULAR);
glMaterialf (GL_FRONT_AND_BACK, GL_SHININESS, 128);
typedef std::vector<
Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> >
RotationList;
RotationList dQ(BE.rows(),Quaterniond::Identity()),vQ;
vector<Vector3d> vT;
Matrix3d A = Matrix3d::Identity();
for(int e = 0;e<BE.rows();e++)
{
dQ[e] = AngleAxisd((sin(get_seconds()+e))*0.06*PI,A.col(e%3));
}
forward_kinematics(C,BE,P,dQ,vQ,vT);
const int dim = C.cols();
MatrixXd T(BE.rows()*(dim+1),dim);
for(int e = 0;e<BE.rows();e++)
{
Affine3d a = Affine3d::Identity();
a.translate(vT[e]);
a.rotate(vQ[e]);
T.block(e*(dim+1),0,dim+1,dim) =
a.matrix().transpose().block(0,0,dim+1,dim);
}
if(wireframe)
{
glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
}
glLineWidth(1.0);
MatrixXd U = M*T;
per_face_normals(U,F,N);
draw_mesh(U,F,N);
glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
if(skeleton_on_top)
{
glDisable(GL_DEPTH_TEST);
}
switch(skel_style)
{
default:
case SKEL_STYLE_TYPE_3D:
draw_skeleton_3d(C,BE,T,MAYA_VIOLET,bbd*0.5);
break;
case SKEL_STYLE_TYPE_VECTOR_GRAPHICS:
draw_skeleton_vector_graphics(C,BE,T);
break;
}
pop_object();
pop_scene();
report_gl_error();
TwDraw();
glutSwapBuffers();
glutPostRedisplay();
}