/**
| 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");
}
}
