/** | cos¦È sin¦È 0 0| |-sin¦È cos¦È 0 0| | 0 0 1 0| | 0 0 0 1| */ void SceneObject::SetRotationZByDegrees(float angel) { float degrees = fmod((angel - mRotation.z), 360); float rad = degrees / 180 * MATH_PI; Matrix44f rotM( cos(rad), sin(rad), 0, 0, -sin(rad), cos(rad), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 ); mTransform = rotM * mTransform; mRotation.z = angel; }
cv::Mat GoodFrame::getRT(std::vector<cv::Point3f> &modelPoints_min) { cv::Mat img = this->getCapturesImage(); std::vector<cv::Point2f> imagePoints; for (size_t i = 0; i < 68; ++i) { imagePoints.push_back(cv::Point2f((float)(this->getDetected_landmarks().at<double>(i)), (float)(this->getDetected_landmarks().at<double>(i+68)))); } ///// int max_d = MAX(img.rows,img.cols); cv::Mat camMatrix = (Mat_<double>(3,3) << max_d, 0, img.cols/2.0, 0, max_d, img.rows/2.0, 0, 0, 1.0); cv::Mat ip(imagePoints); cv::Mat op(modelPoints_min); std::vector<double> rv(3), tv(3); cv::Mat rvec(rv),tvec(tv); double _dc[] = {0,0,0,0}; // std::cout << ip << std::endl << std::endl; // std::cout << op << std::endl << std::endl; // std::cout << camMatrix << std::endl << std::endl; solvePnP(op, ip, camMatrix, cv::Mat(1,4,CV_64FC1,_dc), rvec, tvec, false, CV_EPNP); double rot[9] = {0}; cv::Mat rotM(3, 3, CV_64FC1, rot); cv::Rodrigues(rvec, rotM); double* _r = rotM.ptr<double>(); // printf("rotation mat: \n %.3f %.3f %.3f\n%.3f %.3f %.3f\n%.3f %.3f %.3f\n",_r[0],_r[1],_r[2],_r[3],_r[4],_r[5],_r[6],_r[7],_r[8]); // printf("trans vec: \n %.3f %.3f %.3f\n",tv[0],tv[1],tv[2]); cv::Mat _pm(3, 4, CV_64FC1); _pm.at<double>(0,0) = _r[0]; _pm.at<double>(0,1) = _r[1]; _pm.at<double>(0,2) = _r[2]; _pm.at<double>(0,3) = tv[0]; _pm.at<double>(1,0) = _r[3]; _pm.at<double>(1,1) = _r[4]; _pm.at<double>(1,2) = _r[5]; _pm.at<double>(1,3) = tv[1]; _pm.at<double>(2,0) = _r[6]; _pm.at<double>(2,1) = _r[7]; _pm.at<double>(2,2) = _r[8]; _pm.at<double>(2,3) = tv[2]; return _pm; }
void Rect::rotate(float angle) { if(angle > FULLANGLE) { angle = angle - FULLANGLE; } PM::Matrix3 rotM( cos(angle), -sin(angle), 0, sin(angle), cos(angle), 0, 0, 0, 1 ); this->right.line.dirVec = PM::vxm(this->right.line.dirVec, rotM); // rotatoi oikean sivun suunta vektorin this->right.line.dplVec = PM::vxm(this->right.line.dplVec, rotM); // rotatoi siirtymävectorin lokaalin origon (Rect.m_Pos) suhteen. this->top.line.dirVec = PM::vxm(this->top.line.dirVec, rotM); this->top.line.dplVec = PM::vxm(this->top.line.dplVec, rotM); this->left.line.dirVec = PM::vxm(this->left.line.dirVec, rotM); this->left.line.dplVec = PM::vxm(this->left.line.dplVec, rotM); this->bottom.line.dirVec = PM::vxm(this->bottom.line.dirVec, rotM); this->bottom.line.dplVec = PM::vxm(this->bottom.line.dplVec, rotM); }
/*- render_particles から呼ばれる。粒子の数だけ繰り返し -*/ void Particles_Engine::do_render( TFlash *flash, Particle *part, TTile *tile, std::vector<TRasterFxPort *> part_ports, std::map<int, TTile *> porttiles, const TRenderSettings &ri, TDimension &p_size, TPointD &p_offset, int lastframe, std::vector<TLevelP> partLevel, struct particles_values &values, double opacity_range, int dist_frame, std::map<std::pair<int, int>, double> &partScales) { // Retrieve the particle frame - that is, the *column frame* from which we are // picking // the particle to be rendered. int ndx = part->frame % lastframe; TRasterP tileRas(tile->getRaster()); std::string levelid; double aim_angle = 0; if (values.pathaim_val) { double arctan = atan2(part->vy, part->vx); aim_angle = arctan * M_180_PI; } // Calculate the rotational and scale components we have to apply on the // particle TRotation rotM(part->angle + aim_angle); TScale scaleM(part->scale); TAffine M(rotM * scaleM); // Particles deal with dpi affines on their own TAffine scaleAff(m_parent->handledAffine(ri, m_frame)); double partScale = scaleAff.a11 * partScales[std::pair<int, int>(part->level, ndx)]; TDimensionD partResolution(0, 0); TRenderSettings riNew(ri); // Retrieve the bounding box in the standard reference TRectD bbox(-5.0, -5.0, 5.0, 5.0), standardRefBBox; if (part->level < (int)part_ports.size() && // Not the default levelless cases part_ports[part->level]->isConnected()) { TRenderSettings riIdentity(ri); riIdentity.m_affine = TAffine(); (*part_ports[part->level])->getBBox(ndx, bbox, riIdentity); // A particle's bbox MUST be finite. Gradients and such which have an // infinite bbox // are just NOT rendered. // NOTE: No fx returns half-planes or similar (ie if any coordinate is // either // (std::numeric_limits<double>::max)() or its opposite, then the rect IS // THE infiniteRectD) if (bbox == TConsts::infiniteRectD) return; } // Now, these are the particle rendering specifications bbox = bbox.enlarge(3); standardRefBBox = bbox; riNew.m_affine = TScale(partScale); bbox = riNew.m_affine * bbox; /*- 縮小済みのParticleのサイズ -*/ partResolution = TDimensionD(tceil(bbox.getLx()), tceil(bbox.getLy())); if (flash) { if (!partLevel[part->level]->frame(ndx)) { if (part_ports[0]->isConnected()) { TTile auxTile; TRaster32P tmp; tmp = TRaster32P(p_size); (*part_ports[0]) ->allocateAndCompute(auxTile, p_offset, p_size, tmp, ndx, ri); partLevel[part->level]->setFrame(ndx, TRasterImageP(auxTile.getRaster())); } } flash->pushMatrix(); const TAffine aff; flash->multMatrix(scaleM * aff.place(0, 0, part->x, part->y)); // if(curr_opacity!=1.0 || part->gencol.fadecol || part->fincol.fadecol || // part->foutcol.fadecol) { TColorFader cf(TPixel32::Red, .5); flash->draw(partLevel[part->level]->frame(ndx), &cf); } // flash->draw(partLevel->frame(ndx), 0); flash->popMatrix(); } else { TRasterP ras; std::string alias; TRasterImageP rimg; if (rimg = partLevel[part->level]->frame(ndx)) { ras = rimg->getRaster(); } else { alias = "PART: " + (*part_ports[part->level])->getAlias(ndx, riNew); if (rimg = TImageCache::instance()->get(alias, false)) { ras = rimg->getRaster(); // Check that the raster resolution is sufficient for our purposes if (ras->getLx() < partResolution.lx || ras->getLy() < partResolution.ly) ras = 0; else partResolution = TDimensionD(ras->getLx(), ras->getLy()); } } // We are interested in making the relation between scale and (integer) // resolution // bijective - since we shall cache by using resolution as a partial // identification parameter. // Therefore, we find the current bbox Lx and take a unique scale out of it. partScale = partResolution.lx / standardRefBBox.getLx(); riNew.m_affine = TScale(partScale); bbox = riNew.m_affine * standardRefBBox; // If no image was retrieved from the cache (or it was not scaled enough), // calculate it if (!ras) { TTile auxTile; (*part_ports[part->level]) ->allocateAndCompute(auxTile, bbox.getP00(), TDimension(partResolution.lx, partResolution.ly), tile->getRaster(), ndx, riNew); ras = auxTile.getRaster(); // For now, we'll just use 32 bit particles TRaster32P rcachepart; rcachepart = ras; if (!rcachepart) { rcachepart = TRaster32P(ras->getSize()); TRop::convert(rcachepart, ras); } ras = rcachepart; // Finally, cache the particle addRenderCache(alias, TRasterImageP(ras)); } if (!ras) return; // At this point, it should never happen anyway... // Deal with particle colors/opacity TRaster32P rfinalpart; double curr_opacity = part->set_Opacity(porttiles, values, opacity_range, dist_frame); if (curr_opacity != 1.0 || part->gencol.fadecol || part->fincol.fadecol || part->foutcol.fadecol) { /*- 毎フレーム現在位置のピクセル色を参照 -*/ if (values.pick_color_for_every_frame_val && values.gencol_ctrl_val && (porttiles.find(values.gencol_ctrl_val) != porttiles.end())) part->get_image_reference(porttiles[values.gencol_ctrl_val], values, part->gencol.col); rfinalpart = ras->clone(); part->modify_colors_and_opacity(values, curr_opacity, dist_frame, rfinalpart); } else rfinalpart = ras; // Now, let's build the particle transform before it is overed on the output // tile // First, complete the transform by adding the rotational and scale // components from // Particles parameters M = ri.m_affine * M * TScale(1.0 / partScale); // Then, retrieve the particle position in current reference. TPointD pos(part->x, part->y); pos = ri.m_affine * pos; // Finally, add the translational component to the particle // NOTE: p_offset is added to account for the particle relative position // inside its level's bbox M = TTranslation(pos - tile->m_pos) * M * TTranslation(bbox.getP00()); if (TRaster32P myras32 = tile->getRaster()) TRop::over(tileRas, rfinalpart, M); else if (TRaster64P myras64 = tile->getRaster()) TRop::over(tileRas, rfinalpart, M); else throw TException("ParticlesFx: unsupported Pixel Type"); } }