/*!
Enable to set the position of the 3D plane relative to the virtual camera.
\param _cMt : The pose of the plane relative to the virtual camera.
*/
void
vpImageSimulator::setCameraPosition(const vpHomogeneousMatrix &_cMt)
{
cMt = _cMt;
vpRotationMatrix R;
cMt.extract(R);
normal_Cam = R * normal_obj;
visible_result = vpColVector::dotProd(normal_Cam,focal);
for(int i = 0; i < 4; i++)
pt[i].track(cMt);
vpColVector e1(3) ;
vpColVector e2(3) ;
vpColVector facenormal(3) ;
e1[0] = pt[1].get_X() - pt[0].get_X() ;
e1[1] = pt[1].get_Y() - pt[0].get_Y() ;
e1[2] = pt[1].get_Z() - pt[0].get_Z() ;
e2[0] = pt[2].get_X() - pt[1].get_X() ;
e2[1] = pt[2].get_Y() - pt[1].get_Y() ;
e2[2] = pt[2].get_Z() - pt[1].get_Z() ;
facenormal = vpColVector::crossProd(e1,e2) ;
double angle = pt[0].get_X()*facenormal[0] + pt[0].get_Y()*facenormal[1] + pt[0].get_Z()*facenormal[2] ;
if (angle > 0)
visible=true;
else
visible=false;
if(visible)
{
for(int i = 0; i < 4; i++)
{
project(X[i],cMt,X2[i]);
pt[i].track(cMt);
}
vbase_u = X2[1]-X2[0];
vbase_v = X2[3]-X2[0];
distance = vpColVector::dotProd(normal_Cam,X2[1]);
if(distance < 0)
{
visible = false;
return;
}
for(int i = 0; i < 3; i++)
{
normal_Cam_optim[i] = normal_Cam[i];
X0_2_optim[i] = X2[0][i];
vbase_u_optim[i] = vbase_u[i];
vbase_v_optim[i] = vbase_v[i];
}
vpImagePoint iPa[4];
for(int i = 0; i < 4; i++)
{
iPa[i].set_j(X2[i][0]/X2[i][2]);
iPa[i].set_i(X2[i][1]/X2[i][2]);
}
T1.buildFrom(iPa[0],iPa[1],iPa[3]);
T2.buildFrom(iPa[2],iPa[1],iPa[3]);
}
}
/*!
Enable to set the position of the 3D plane relative to the virtual camera.
\param cMt_ : The pose of the plane relative to the virtual camera.
*/
void
vpImageSimulator::setCameraPosition(const vpHomogeneousMatrix &cMt_)
{
cMt = cMt_;
vpRotationMatrix R;
cMt.extract(R);
needClipping = false;
normal_Cam = R * normal_obj;
visible_result = vpColVector::dotProd(normal_Cam,focal);
for(unsigned int i = 0; i < 4; i++)
pt[i].track(cMt);
vpColVector e1(3) ;
vpColVector e2(3) ;
vpColVector facenormal(3) ;
e1[0] = pt[1].get_X() - pt[0].get_X() ;
e1[1] = pt[1].get_Y() - pt[0].get_Y() ;
e1[2] = pt[1].get_Z() - pt[0].get_Z() ;
e2[0] = pt[2].get_X() - pt[1].get_X() ;
e2[1] = pt[2].get_Y() - pt[1].get_Y() ;
e2[2] = pt[2].get_Z() - pt[1].get_Z() ;
facenormal = vpColVector::crossProd(e1,e2) ;
double angle = pt[0].get_X()*facenormal[0] + pt[0].get_Y()*facenormal[1] + pt[0].get_Z()*facenormal[2] ;
if (angle > 0){
visible=true;
}
else {
visible=false;
}
if(visible)
{
for(unsigned int i = 0; i < 4; i++)
{
project(X[i],cMt,X2[i]);
pt[i].track(cMt);
if(pt[i].get_Z() < 0)
needClipping = true;
}
vbase_u = X2[1]-X2[0];
vbase_v = X2[3]-X2[0];
distance = vpColVector::dotProd(normal_Cam,X2[1]);
if(distance < 0)
{
visible = false;
return;
}
for(unsigned int i = 0; i < 3; i++)
{
normal_Cam_optim[i] = normal_Cam[i];
X0_2_optim[i] = X2[0][i];
vbase_u_optim[i] = vbase_u[i];
vbase_v_optim[i] = vbase_v[i];
}
std::vector<vpPoint> *ptPtr = &pt;
if(needClipping){
vpPolygon3D::getClippedPolygon(pt,ptClipped,cMt,vpPolygon3D::NEAR_CLIPPING);
ptPtr = &ptClipped;
}
listTriangle.clear();
for(unsigned int i = 1 ; i < (*ptPtr).size()-1 ; i++){
vpImagePoint ip1, ip2, ip3;
ip1.set_j((*ptPtr)[0].get_x());
ip1.set_i((*ptPtr)[0].get_y());
ip2.set_j((*ptPtr)[i].get_x());
ip2.set_i((*ptPtr)[i].get_y());
ip3.set_j((*ptPtr)[i+1].get_x());
ip3.set_i((*ptPtr)[i+1].get_y());
vpTriangle tri(ip1,ip2,ip3);
listTriangle.push_back(tri);
}
}
}
