void Canvas::wheelEvent(QWheelEvent *event)
{
// Find GL position before the zoom operation
// (to zoom about mouse cursor)
auto p = event->pos();
QVector3D v(1 - p.x() / (0.5*width()),
p.y() / (0.5*height()) - 1, 0);
QVector3D a = transform_matrix().inverted() *
view_matrix().inverted() * v;
if (event->delta() < 0)
{
for (int i=0; i > event->delta(); --i)
zoom *= 1.001;
}
else if (event->delta() > 0)
{
for (int i=0; i < event->delta(); ++i)
zoom /= 1.001;
}
// Then find the cursor's GL position post-zoom and adjust center.
QVector3D b = transform_matrix().inverted() *
view_matrix().inverted() * v;
center += b - a;
update();
}
void compute_matrix(t_mesh *mesh)
{
mesh->result = id_matrix();
transform_matrix(mesh->result, mesh->scale);
transform_matrix(mesh->result, mesh->rot_x);
transform_matrix(mesh->result, mesh->rot_y);
transform_matrix(mesh->result, mesh->rot_z);
transform_matrix(mesh->result, mesh->trans);
}
void Canvas::draw_mesh()
{
mesh_shader.bind();
// Load the transform and view matrices into the shader
glUniformMatrix4fv(
mesh_shader.uniformLocation("transform_matrix"),
1, GL_FALSE, transform_matrix().data());
glUniformMatrix4fv(
mesh_shader.uniformLocation("view_matrix"),
1, GL_FALSE, view_matrix().data());
// Compensate for z-flattening when zooming
glUniform1f(mesh_shader.uniformLocation("zoom"), 1/zoom);
// Find and enable the attribute location for vertex position
const GLuint vp = mesh_shader.attributeLocation("vertex_position");
glEnableVertexAttribArray(vp);
// Then draw the mesh with that vertex position
mesh->draw(vp);
// Clean up state machine
glDisableVertexAttribArray(vp);
mesh_shader.release();
}
void ParticleSystem2D::submit(Renderer2D *renderer) const {
renderer->push( transform_matrix() );
for (auto &&emitter : m_emitters) {
emitter->submit(renderer);
}
renderer->pop();
}
void ParticleEffect2D::submit(Renderer2D* renderer) const {
renderer->push( transform_matrix() );
for (auto &&system : m_systems) {
system->submit(renderer);
}
renderer->pop();
}
void LayoutShowUnit::CalcTransformToScalePosition()
{
b2Fixture *outsideFixture=GetOutsideFixture(this->body);
b2PolygonShape *polygonShape=(b2PolygonShape*)outsideFixture->GetShape();
b2Transform tx=outsideFixture->GetBody()->GetTransform();
bee::Point b1=GetWorldPoint(polygonShape->m_vertices[0],tx);
bee::Point b2=GetWorldPoint(polygonShape->m_vertices[1],tx);
bee::Point b3=GetWorldPoint(polygonShape->m_vertices[3],tx);
b2Mat33 transform_matrix(b2Vec3(b1.x,b2.x,b3.x),b2Vec3(b1.y,b2.y,b3.y),b2Vec3(1,1,1));
this->xDirectionVector=transform_matrix.Solve33(b2Vec3(0,1,0));
this->yDirectionVector=transform_matrix.Solve33(b2Vec3(0,0,1));
}
void ESKOgre::buildBone(unsigned short b, Ogre::Skeleton *ogre_skeleton, Ogre::Bone *parent_bone) {
ESKBone *bone = bones[b];
Ogre::Bone *mBone = ogre_skeleton->createBone(bone->name);
if (parent_bone)
{
parent_bone->addChild(mBone);
}
Ogre::Matrix4 parent_matrix = Ogre::Matrix4::IDENTITY;
if (bone->parent_index < bones.size()) {
ESKBone *pbone = bones[bone->parent_index];
parent_matrix = Ogre::Matrix4(pbone->transform_matrix[0], pbone->transform_matrix[4], pbone->transform_matrix[8], pbone->transform_matrix[12],
pbone->transform_matrix[1], pbone->transform_matrix[5], pbone->transform_matrix[9], pbone->transform_matrix[13],
pbone->transform_matrix[2], pbone->transform_matrix[6], pbone->transform_matrix[10], pbone->transform_matrix[14],
pbone->transform_matrix[3], pbone->transform_matrix[7], pbone->transform_matrix[11], pbone->transform_matrix[15]);
}
Ogre::Matrix4 transform_matrix(bone->transform_matrix[0], bone->transform_matrix[4], bone->transform_matrix[8], bone->transform_matrix[12],
bone->transform_matrix[1], bone->transform_matrix[5], bone->transform_matrix[9], bone->transform_matrix[13],
bone->transform_matrix[2], bone->transform_matrix[6], bone->transform_matrix[10], bone->transform_matrix[14],
bone->transform_matrix[3], bone->transform_matrix[7], bone->transform_matrix[11], bone->transform_matrix[15]);
transform_matrix = transform_matrix * parent_matrix.inverse();
Ogre::Vector3 mPos;
Ogre::Vector3 mScale;
Ogre::Quaternion mRot;
transform_matrix.decomposition(mPos, mScale, mRot);
LOG_DEBUG("Bone %d Setup for %s: %f %f %f %d\n", b, bone->name.c_str(), (float)mPos.x, (float)mPos.y, (float)mPos.z, bone->index_4);
mBone->setPosition(mPos * -1);
mBone->setScale(mScale);
mBone->setOrientation(mRot.Inverse());
mBone->setInitialState();
mBone->setManuallyControlled(false);
for (size_t i = 0; i < bones.size(); i++) {
if (bones[i]->parent_index == b) {
buildBone(i, ogre_skeleton, mBone);
}
}
}
void Canvas::mouseMoveEvent(QMouseEvent* event)
{
auto p = event->pos();
auto d = p - mouse_pos;
if (event->buttons() & Qt::LeftButton)
{
yaw = fmod(yaw - d.x(), 360);
tilt = fmax(0, fmin(180, tilt - d.y()));
update();
}
else if (event->buttons() & Qt::RightButton)
{
center = transform_matrix().inverted() *
view_matrix().inverted() *
QVector3D(-d.x() / (0.5*width()),
d.y() / (0.5*height()), 0);
update();
}
mouse_pos = p;
}
material::material(std::istream &stream_, uint32_t version = 68) {
uint32_t textures_count, transforms_count;
READ_STRING(name);
stream_.read((char *)&type, sizeof(uint32_t));
stream_.read((char *)&emissive, sizeof(float) * 4);
stream_.read((char *)&ambient, sizeof(float) * 4);
stream_.read((char *)&diffuse, sizeof(float) * 4);
stream_.read((char *)&forced_diffuse, sizeof(float) * 4);
stream_.read((char *)&specular, sizeof(float) * 4);
stream_.read((char *)&specular_2, sizeof(float) * 4);
stream_.read((char *)&specular_power, sizeof(float));
stream_.read((char *)&pixel_shader, sizeof(uint32_t));
stream_.read((char *)&vertex_shader, sizeof(uint32_t));
stream_.read((char *)&u_long_1, sizeof(uint32_t));
stream_.read((char *)&an_index, sizeof(uint32_t));
stream_.read((char *)&u_long_2, sizeof(uint32_t));
READ_STRING(surface);
stream_.read((char *)&u_long_3, sizeof(uint32_t));
stream_.read((char *)&render_flags, sizeof(uint32_t));
stream_.read((char *)&textures_count, sizeof(uint32_t));
stream_.read((char *)&transforms_count, sizeof(uint32_t));
for (int x = 0; x < textures_count; x++) {
texture_stages.push_back(std::make_shared<stage_texture>(stream_, type));
}
for (int x = 0; x < textures_count; x++) {
uint32_t uv_source;
stream_.read((char *)&uv_source, sizeof(uint32_t));
transform_stages.push_back(std::pair<uint32_t, transform_matrix>(uv_source, transform_matrix(stream_)));
}
if (type >= 10) {
texture_stages.push_back(std::make_shared<stage_texture>(stream_, type));
}
}
int main(int argc, char *argv[]) {
FILE *FptrNumDocs;
ENVIRONMENT env;
char *model_data_dir;
char pathbuf[BUFSIZ];
char *col_label_file;
int write_matlab;
int col_labels_from_file;
int rows, columns;
MODEL_PARAMS model_params;
MODEL_INFO model_info;
env.word_array = NULL;
env.word_tree = NULL;
if ( argc != 17 ) usage_and_exit( argv[0], 1 );
if ( strcmp( argv[1], "-mdir" ) != 0 ) usage_and_exit( argv[0], 1 );
model_data_dir = argv[2];
if ( strcmp( argv[3], "-matlab" ) != 0 ) usage_and_exit( argv[0], 1 );
write_matlab = atoi( argv[4] );
if ( strcmp( argv[5], "-precontext" ) != 0 ) usage_and_exit( argv[0], 1 );
pre_context_size = atoi( argv[6] );
if ( strcmp( argv[7], "-postcontext" ) != 0 ) usage_and_exit( argv[0], 1 );
post_context_size = atoi( argv[8] );
if ( strcmp( argv[9], "-rows" ) != 0 ) usage_and_exit( argv[0], 1 );
rows = atoi( argv[10] );
if ( strcmp( argv[11], "-columns" ) != 0 ) usage_and_exit( argv[0], 1 );
columns = atoi( argv[12] );
if ( strcmp( argv[13], "-col_labels_from_file" ) != 0 ) usage_and_exit( argv[0], 1 );
col_labels_from_file = atoi( argv[14] );
if ( strcmp( argv[15], "-col_label_file" ) != 0 ) usage_and_exit( argv[0], 1 );
col_label_file = argv[16];
fprintf( stderr, "model data dir is \"%s\".\n", model_data_dir );
/** Read in current model params **/
sprintf( pathbuf, "%s/%s", model_data_dir, MODEL_PARAMS_BIN_FILE );
if ( !read_model_params( pathbuf, &model_params )) {
die( "count_wordvec.c: couldn't read model data file\n" );
}
sprintf( pathbuf, "%s/%s", model_data_dir, MODEL_INFO_BIN_FILE );
if ( !read_model_info( pathbuf, &model_info )) {
die( "count_wordvec.c: couldn't read model info file\n" );
}
if (model_params.rows < rows) {
rows = model_params.rows;
} else {
model_params.rows = rows;
}
printf("count_wordvec.c: looking for %d rows\n", rows);
printf("which had better match %d\n", model_params.rows);
model_info.columns = columns;
model_info.col_labels_from_file = col_labels_from_file;
model_info.pre_context_size = pre_context_size;
model_info.post_context_size = post_context_size;
model_info.blocksize = BLOCKSIZE;
model_info.start_columns = START_COLUMNS;
message( "Reading the dictionary... ");
if( !read_dictionary( &(env.word_array), &(env.word_tree), model_data_dir ))
die( "count_wordvec.c: Can't read the dictionary.\n");
/*** read number of ducuments from file ***/
sprintf( pathbuf, "%s/%s", model_data_dir, FNUM_FILE );
if ( !my_fopen( &FptrNumDocs, pathbuf, "r" ))
die( "couldn't open filenames file" );
if( !fscanf( FptrNumDocs, "%d", &numDocs ))
die( "can't read numDocs" );
if( !my_fclose( &FptrNumDocs ))
die( "couldn't close numDocs file" );
/*****/
/* Set some initial values in the matrix, arrays etc. */
if( !initialize_row_indices( env.word_array, &(env.row_indices),
rows ))
die( "Couldn't initialize row indices.\n");
if( !initialize_column_indices( env.word_array, &(env.col_indices),
columns, col_labels_from_file, col_label_file, &(env.word_tree) ))
die( "Couldn't initialize column indices.\n");
/* Allocate memory and set everything to zero.
Defined in matrix.h */
if( !initialize_matrix( (MATRIX_TYPE***) &(env.matrix), rows, columns))
die( "Can't initialize matrix.\n");
/* Go through the wordlist, applying process_region to all regions. */
fprintf( stderr, "model data dir is \"%s\".\n", model_data_dir );
fprintf( stderr, "count_wordvec.c: about to call process_wordlist\n" );
if( !process_wordlist( is_target, advance_target,
set_region_in, set_region_out,
process_region , &env, model_data_dir))
die( "Couldn't process wordlist.\n");
/* Perform some conversion on the matrix.
E.g. some kind of normalization. We traditionally take the square root
of all entries. */
if( !transform_matrix( (MATRIX_TYPE **) (env.matrix), rows, columns))
die( "Couldn't transform matrix.\n");
/* Write the co-occurrence matrix. */
message( "Writing the co-occurrence matrix.\n");
if( !write_matrix_svd((MATRIX_TYPE **) (env.matrix),
rows, columns, model_data_dir ))
die( "count_wordvec.c: couldn't write co-occurrence "
"matrix in SVD input format.\n" );
if ( write_matlab ) {
if ( !write_matrix_matlab( (MATRIX_TYPE **)(env.matrix),
rows, columns, model_data_dir ))
die( "count_wordvec.c: couldn't write co-occurrence "
"matrix in Matlab input format.\n" );
}
sprintf( pathbuf, "%s/%s", model_data_dir, MODEL_PARAMS_BIN_FILE );
if ( !write_model_params( pathbuf, &model_params )) {
die( "count_wordvec.c: couldn't write model params file\n" );
}
sprintf( pathbuf, "%s/%s", model_data_dir, MODEL_INFO_BIN_FILE );
if ( !write_model_info( pathbuf, &model_info )) {
die( "count_wordvec.c: couldn't write model info file\n" );
}
exit( EXIT_SUCCESS);
}
int main(int argc, char** argv)
{
std::string database_path,result_path;
database_path = "PCA.txt";
result_path = "result";
double pi = 4 * atan(1);
double distance_limit = 0.001, angle_limit = pi * 0.25;
/* Note that if the model is read from a file the scale should be 1.0, otherwise if the model is calculated from a database the scale should be around 0.1 */
double scale = 1.0;
double energy_weight = 0.001;
int device = CV_CAP_OPENNI;
Registration registrator;
/* Path to the database of the model */
pcl::console::parse_argument (argc, argv, "-database", database_path);
/* Path to the file where the final result is saved */
pcl::console::parse_argument (argc, argv, "-result", result_path);
/* The distance threshold for establishing the correspondences */
pcl::console::parse_argument (argc, argv, "-distance", distance_limit);
/* The angle threshold between the normals for establishing the correspondences */
pcl::console::parse_argument (argc, argv, "-angle", angle_limit);
/* The scale to which the training set has to be brought to */
pcl::console::parse_argument (argc, argv, "-scale", scale);
/* The weight that the regularizing part of the Non-Rigid Registration should have */
pcl::console::parse_argument (argc, argv, "-energy_weight", energy_weight);
Eigen::Matrix3d transform_matrix = Eigen::Matrix3d::Identity();
Eigen::Vector3d translation = Eigen::Vector3d::Zero();
transform_matrix(0,0) = scale;
transform_matrix(1,1) = scale;
transform_matrix(2,2) = scale;
/* For some reason, the cloud returned by the Asus Xtion is always up-side down, so the model is rotated by 180 degrees along the Ox axis */
transform_matrix = Eigen::AngleAxisd(pi,Eigen::Vector3d::UnitX()) * transform_matrix;
/* This switch enables the debug mode in order to analyze in the PCLVisualizer the intermiediate steps of the registration */
bool debug = pcl::console::find_switch (argc, argv, "-debug");
if( pcl::console::find_switch (argc, argv, "-Asus") )
{
device = CV_CAP_OPENNI_ASUS;
}
registrator.setDebugMode ( debug );
/* In this if branch the target cloud is a simple snapshot from the Kinect/Xtion */
if(pcl::console::find_switch (argc, argv, "--camera"))
{
std::string xml_file("haarcascade_frontalface_alt.xml");
pcl::console::parse_argument (argc, argv, "-xml_file", xml_file);
registrator.getTargetPointCloudFromCamera(device,xml_file);
registrator.getDataForModel(database_path, transform_matrix, translation, scale);
registrator.alignModel();
registrator.calculateAlternativeRegistrations(50,energy_weight,15,100,angle_limit,distance_limit,debug);
}
/* In this if branch the target cloud is read from a pcd file */
if(pcl::console::find_switch (argc, argv, "--scan"))
{
std::string pcd_file("target.pcd");
pcl::console::parse_argument (argc, argv, "-target", pcd_file);
float x,y,z;
pcl::console::parse_argument (argc, argv, "-x", x);
pcl::console::parse_argument (argc, argv, "-y", y);
pcl::console::parse_argument (argc, argv, "-z", z);
pcl::PointXYZ face(x,y,z);
registrator.getTargetPointCloudFromFile(pcd_file, face);
registrator.getDataForModel(database_path, transform_matrix, translation, scale);
registrator.alignModel();
registrator.calculateAlternativeRegistrations(50,energy_weight,15,100,angle_limit,distance_limit,debug);
}
/* In this if branch the target cloud is read by using the KinfuTracker */
if(pcl::console::find_switch (argc, argv, "--kinfu"))
{
registrator.getDataForModel (database_path, transform_matrix, translation, scale);
registrator.calculateKinfuTrackerRegistrations (device,50,energy_weight,100,angle_limit,distance_limit);
}
registrator.writeDataToPCD(result_path);
return (0);
}
