/**
* Generate the vtkDataSet from the objects input IMDEventWorkspace
* @param progressUpdating : Reporting object to pass progress information up the stack.
* @return fully constructed vtkDataSet.
*/
vtkDataSet* vtkSplatterPlotFactory::create(ProgressAction& progressUpdating) const
{
UNUSED_ARG(progressUpdating);
// If initialize() wasn't run, we don't have a workspace.
if(!m_workspace)
{
throw std::runtime_error("Invalid vtkSplatterPlotFactory. Workspace is null");
}
size_t nd = m_workspace->getNumDims();
Mantid::Kernel::ReadLock lock(*m_workspace);
if (nd > 3)
{
// Slice from >3D down to 3D
this->slice = true;
this->sliceMask = new bool[nd];
this->sliceImplicitFunction = new MDImplicitFunction();
// Make the mask of dimensions
// TODO: Smarter mapping
for (size_t d = 0; d < nd; d++)
this->sliceMask[d] = (d < 3);
// Define where the slice is in 4D
// TODO: Where to slice? Right now is just 0
std::vector<coord_t> point(nd, 0);
point[3] = coord_t(m_time); //Specifically for 4th/time dimension.
// Define two opposing planes that point in all higher dimensions
std::vector<coord_t> normal1(nd, 0);
std::vector<coord_t> normal2(nd, 0);
for (size_t d = 3; d < nd; d++)
{
normal1[d] = +1.0;
normal2[d] = -1.0;
}
// This creates a 0-thickness region to slice in.
sliceImplicitFunction->addPlane(MDPlane(normal1, point));
sliceImplicitFunction->addPlane(MDPlane(normal2, point));
}
else
{
// Direct 3D, so no slicing
this->slice = false;
}
// Macro to call the right instance of the
CALL_MDEVENT_FUNCTION(this->doCreate, m_workspace);
// Clean up
if (this->slice)
{
delete[] this->sliceMask;
delete this->sliceImplicitFunction;
}
// The macro does not allow return calls, so we used a member variable.
return this->dataSet;
}
BaseIF* makeVane(const Real& thick,
const RealVect& normal,
const Real& innerRadius,
const Real& outerRadius,
const Real& offset,
const Real& height)
{
RealVect zero(D_DECL(0.0,0.0,0.0));
RealVect xAxis(D_DECL(1.0,0.0,0.0));
bool inside = true;
Vector<BaseIF*> vaneParts;
// Each side of the vane (infinite)
RealVect normal1(normal);
RealVect point1(D_DECL(offset+height/2.0,-thick/2.0,0.0));
PlaneIF plane1(normal1,point1,inside);
vaneParts.push_back(&plane1);
RealVect normal2(-normal);
RealVect point2(D_DECL(offset+height/2.0,thick/2.0,0.0));
PlaneIF plane2(normal2,point2,inside);
vaneParts.push_back(&plane2);
// Make sure we only get something to the right of the origin
RealVect normal3(D_DECL(0.0,0.0,1.0));
RealVect point3(D_DECL(0.0,0.0,0.0));
PlaneIF plane3(normal3,point3,inside);
vaneParts.push_back(&plane3);
// Cut off the top and bottom
RealVect normal4(D_DECL(1.0,0.0,0.0));
RealVect point4(D_DECL(offset,0.0,0.0));
PlaneIF plane4(normal4,point4,inside);
vaneParts.push_back(&plane4);
RealVect normal5(D_DECL(-1.0,0.0,0.0));
RealVect point5(D_DECL(offset+height,0.0,0.0));
PlaneIF plane5(normal5,point5,inside);
vaneParts.push_back(&plane5);
// The outside of the inner cylinder
TiltedCylinderIF inner(innerRadius,xAxis,zero,!inside);
vaneParts.push_back(&inner);
// The inside of the outer cylinder
TiltedCylinderIF outer(outerRadius,xAxis,zero,inside);
vaneParts.push_back(&outer);
IntersectionIF* vane = new IntersectionIF(vaneParts);
return vane;
}
BaseIF* makeChamber(const Real& radius,
const Real& thick,
const Real& offset,
const Real& height)
{
RealVect zero(D_DECL(0.0,0.0,0.0));
RealVect xAxis(D_DECL(1.0,0.0,0.0));
bool inside = true;
Vector<BaseIF*> pieces;
// Create a chamber
TiltedCylinderIF chamberOut(radius + thick/2.0,xAxis,zero, inside);
TiltedCylinderIF chamberIn (radius - thick/2.0,xAxis,zero,!inside);
IntersectionIF infiniteChamber(chamberIn,chamberOut);
pieces.push_back(&infiniteChamber);
RealVect normal1(D_DECL(1.0,0.0,0.0));
RealVect point1(D_DECL(offset,0.0,0.0));
PlaneIF plane1(normal1,point1,inside);
pieces.push_back(&plane1);
RealVect normal2(D_DECL(-1.0,0.0,0.0));
RealVect point2(D_DECL(offset+height,0.0,0.0));
PlaneIF plane2(normal2,point2,inside);
pieces.push_back(&plane2);
IntersectionIF* chamber = new IntersectionIF(pieces);
return chamber;
}
NOMIPS16 f (int *p)
{
*p = normal ();
normal2 ();
staticfunc ();
return 1;
}
QImage *createRoundButtonBackground(const QMatrix &matrix) const
{
QRect scaledRect;
scaledRect = matrix.mapRect(QRect(0, 0, this->logicalSize.width(), this->logicalSize.height()));
QImage *image = new QImage(scaledRect.width(), scaledRect.height(), QImage::Format_ARGB32_Premultiplied);
image->fill(QColor(0, 0, 0, 0).rgba());
QPainter painter(image);
painter.setRenderHint(QPainter::SmoothPixmapTransform);
painter.setRenderHint(QPainter::Antialiasing);
painter.setPen(Qt::NoPen);
if (Colors::useEightBitPalette){
painter.setPen(QColor(120, 120, 120));
if (this->pressed)
painter.setBrush(QColor(60, 60, 60));
else if (this->highlighted)
painter.setBrush(QColor(100, 100, 100));
else
painter.setBrush(QColor(80, 80, 80));
}
else {
QLinearGradient outlinebrush(0, 0, 0, scaledRect.height());
QLinearGradient brush(0, 0, 0, scaledRect.height());
brush.setSpread(QLinearGradient::PadSpread);
QColor highlight(255, 255, 255, 70);
QColor shadow(0, 0, 0, 70);
QColor sunken(220, 220, 220, 30);
QColor normal1(255, 255, 245, 60);
QColor normal2(255, 255, 235, 10);
if (this->type == TextButton::PANEL){
normal1 = QColor(200, 170, 160, 50);
normal2 = QColor(50, 10, 0, 50);
}
if (pressed) {
outlinebrush.setColorAt(0.0f, shadow);
outlinebrush.setColorAt(1.0f, highlight);
brush.setColorAt(0.0f, sunken);
painter.setPen(Qt::NoPen);
} else {
outlinebrush.setColorAt(1.0f, shadow);
outlinebrush.setColorAt(0.0f, highlight);
brush.setColorAt(0.0f, normal1);
if (!this->highlighted)
brush.setColorAt(1.0f, normal2);
painter.setPen(QPen(outlinebrush, 1));
}
painter.setBrush(brush);
}
if (this->type == TextButton::PANEL)
painter.drawRect(0, 0, scaledRect.width(), scaledRect.height());
else
painter.drawRoundedRect(0, 0, scaledRect.width(), scaledRect.height(), 10, 90, Qt::RelativeSize);
return image;
}
//-------------------------法向计算------------------------------
void testApp::compute_normal(const ofVec3f *vec,const int rows,const int cols,ofVec3f *normalmap)
{
ofVec3f normal1(0),normal2(0);
const ofVec3f *line_one=vec;
const ofVec3f *line_two=vec+cols;
clock_t t1=clock();
for(int i=0;i<rows;i++)
{
for(int j=0;j<cols;j++)
{
if(j!=cols-1)
{
normal1=line_one[j+1]-line_one[j];
normal2=line_two[j]-line_one[j];
}
else if(j==cols-1)
{
normal1=line_one[j]-line_one[j-1];
normal2=line_two[j]-line_one[j];
}
int num=i*cols+j;
if(vec[num].z>0)
{
normalmap[num]=normal2.cross(normal1);
normalmap[num]=normalmap[num].normalized();
}
else
{
normalmap[num]=ofVec3f(0.0,0.0,0.0);
}
}
if(i!=rows-2)
{
line_two+=cols;
line_one+=cols;
}
}
clock_t t2=clock();
double ti=(double)(t2-t1)/CLOCKS_PER_SEC;
//cout<<ti<<endl;
}
void Ellipsoid::subDraw()
{
computeAssistVar();
getPrimitiveSetList().clear();
osg::ref_ptr<osg::Vec3Array> vertexArr = new osg::Vec3Array;
osg::ref_ptr<osg::Vec3Array> normalArr = new osg::Vec3Array;
setVertexArray(vertexArr);
setNormalArray(normalArr, osg::Array::BIND_PER_VERTEX);
osg::ref_ptr<osg::Vec4Array> colArr = new osg::Vec4Array();
colArr->push_back(m_color);
setColorArray(colArr, osg::Array::BIND_OVERALL);
bool isFull = osg::equivalent(m_angle, 2 * M_PI, GetEpsilon());
if (isFull)
{
m_angle = 2 * M_PI;
}
osg::Vec3 bottomNormal = -m_aLen;
bottomNormal.normalize();
osg::Quat localToWold;
localToWold.makeRotate(osg::Z_AXIS, -bottomNormal);
osg::Vec3 xVec = localToWold * osg::X_AXIS;
osg::Vec3 yVec = xVec ^ bottomNormal;
int hCount = m_bDivision;
double hIncAng = 2 * M_PI / hCount;
osg::Quat hQuat(hIncAng, bottomNormal);
int vCount = (int)ceil(m_angle / (2 * M_PI / m_aDivision));
if (vCount & 1) // 如果是奇数，则变成偶数
++vCount;
double vIncAng = m_angle / vCount;
double currAngle = m_angle / 2.0;
double b = m_bRadius;
double a = m_aLen.length();
osg::Vec3 vec1(b * sin(currAngle), 0, a * cos(currAngle));
vec1 = localToWold * vec1;
osg::Vec3 normal1(sin(currAngle) / b, 0, cos(currAngle) / a);
normal1 = localToWold * normal1;
normal1.normalize();
currAngle -= vIncAng;
osg::Vec3 vec2(b * sin(currAngle), 0, a * cos(currAngle));
vec2 = localToWold * vec2;
osg::Vec3 normal2(sin(currAngle) / b, 0, cos(currAngle) / a);
normal2 = localToWold * normal2;
normal2.normalize();
const GLint first = vertexArr->size();
for (int i = 0; i < vCount / 2; ++i)
{
osg::Vec3 hVec1 = vec1;
osg::Vec3 hVec2 = vec2;
osg::Vec3 hNormal1 = normal1;
osg::Vec3 hNormal2 = normal2;
const size_t hFirst = vertexArr->size();
for (int j = 0; j < hCount; ++j)
{
vertexArr->push_back(m_center + hVec1);
vertexArr->push_back(m_center + hVec2);
normalArr->push_back(hNormal1);
normalArr->push_back(hNormal2);
hVec1 = hQuat * hVec1;
hVec2 = hQuat * hVec2;
hNormal1 = hQuat * hNormal1;
hNormal2 = hQuat * hNormal2;
}
vertexArr->push_back((*vertexArr)[hFirst]);
vertexArr->push_back((*vertexArr)[hFirst + 1]);
normalArr->push_back((*normalArr)[hFirst]);
normalArr->push_back((*normalArr)[hFirst + 1]);
vec1 = vec2;
currAngle -= vIncAng;
vec2.set(b * sin(currAngle), 0, a * cos(currAngle));
vec2 = localToWold * vec2;
normal1 = normal2;
normal2.set(sin(currAngle) / b, 0, cos(currAngle) / a);
normal2 = localToWold * normal2;
normal2.normalize();
}
addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::QUAD_STRIP, first, vertexArr->size() - first));
if (!isFull && m_bottomVis)
{
const GLint first = vertexArr->size();
currAngle = m_angle / 2.0;
osg::Vec3 vec1(b * sin(currAngle), 0, a * cos(currAngle));
vec1 = localToWold * vec1;
osg::Vec3 vec2(0, 0, a * cos(currAngle));
vec2 = localToWold * vec2;
vertexArr->push_back(m_center + vec2);
normalArr->push_back(bottomNormal);
for (int i = 0; i < hCount; ++i)
{
vertexArr->push_back(m_center + vec1);
normalArr->push_back(bottomNormal);
vec1 = hQuat * vec1;
}
vertexArr->push_back((*vertexArr)[first + 1]);
normalArr->push_back(bottomNormal);
addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::TRIANGLE_FAN, first, vertexArr->size() - first));
}
}
void ShapeParabolicRectangle::generatePrimitives(SoAction *action)
{
SoPrimitiveVertex pv;
SoState *state = action->getState();
SbBool useTexFunc = ( SoTextureCoordinateElement::getType(state) ==
SoTextureCoordinateElement::FUNCTION );
const SoTextureCoordinateElement* tce = 0;
if ( useTexFunc ) tce = SoTextureCoordinateElement::getInstance(state);
SbVec3f point;
const int rows = 12; // Number of points per row
const int columns = 12; // Number of points per column
const int totalPoints = (rows)*(columns); // Total points in the grid
float vertex[totalPoints][6];
int h = 0;
double ui = 0;
double vj = 0;
for (int i = 0; i < rows; ++i )
{
ui =( 1.0 /(double)(rows-1) ) * i;
for ( int j = 0 ; j < columns ; ++j )
{
vj = ( 1.0 /(double)(columns-1) ) * j;
Point3D point = GetPoint3D(ui, vj);
NormalVector normal;
if( activeSide.getValue() == 0 ) normal = -GetNormal(ui, vj);
else normal = GetNormal(ui, vj);
vertex[h][0] = point.x;
vertex[h][1] = point.y;
vertex[h][2] = point.z;
vertex[h][3] = normal.x;
vertex[h][4] = normal.y;
vertex[h][5] = normal.z;
pv.setPoint( vertex[h][0], vertex[h][1], vertex[h][2] );
h++; //Increase h to the next point.
}
}
float u = 1;
float v = 1;
beginShape(action, QUADS );
for( int irow = 0; irow < (rows-1); ++irow )
{
for( int icolumn = 0; icolumn < (columns-1); ++icolumn )
{
int index0 = irow*columns + icolumn;
SbVec3f point0( vertex[index0][0], vertex[index0][1], vertex[index0][2] );
SbVec3f normal0(vertex[index0][3], vertex[index0][4], vertex[index0][5] );
SbVec4f texCoord0 = useTexFunc ? tce->get(point0, normal0): SbVec4f( u,v, 0.0, 1.0 );
pv.setPoint(point0);
pv.setNormal(normal0);
pv.setTextureCoords(texCoord0);
shapeVertex(&pv);
int index1 = index0 + 1;
SbVec3f point1( vertex[index1][0], vertex[index1][1], vertex[index1][2] );
SbVec3f normal1(vertex[index1][3], vertex[index1][4], vertex[index1][5] );
SbVec4f texCoord1 = useTexFunc ? tce->get(point1, normal1): SbVec4f( u,v, 0.0, 1.0 );
pv.setPoint(point1);
pv.setNormal(normal1);
pv.setTextureCoords(texCoord1);
shapeVertex(&pv);
int index3 = index0 + columns;
int index2 = index3 + 1;
SbVec3f point2( vertex[index2][0], vertex[index2][1], vertex[index2][2] );
SbVec3f normal2(vertex[index2][3], vertex[index2][4], vertex[index2][5] );
SbVec4f texCoord2 = useTexFunc ? tce->get(point2, normal2): SbVec4f( u,v, 0.0, 1.0 );
pv.setPoint(point2);
pv.setNormal(normal2);
pv.setTextureCoords(texCoord2);
shapeVertex(&pv);
SbVec3f point3( vertex[index3][0], vertex[index3][1], vertex[index3][2] );
SbVec3f normal3(vertex[index3][3], vertex[index3][4], vertex[index3][5] );
SbVec4f texCoord3 = useTexFunc ? tce->get(point3, normal3): SbVec4f( u,v, 0.0, 1.0 );
pv.setPoint(point3);
pv.setNormal(normal3);
pv.setTextureCoords(texCoord3);
shapeVertex(&pv);
}
}
endShape();
}
void ToolButton::paintEvent( QPaintEvent * _pe )
{
const bool active = isDown() || isChecked();
QPainter painter(this);
painter.setRenderHint(QPainter::SmoothPixmapTransform);
painter.setRenderHint(QPainter::Antialiasing);
painter.setPen(Qt::NoPen);
QLinearGradient outlinebrush(0, 0, 0, height());
QLinearGradient brush(0, 0, 0, height());
brush.setSpread(QLinearGradient::PadSpread);
QColor highlight(255, 255, 255, 70);
QColor shadow(0, 0, 0, 70);
QColor sunken(220, 220, 220, 30);
QColor normal1(255, 255, 245, 60);
QColor normal2(255, 255, 235, 10);
if( active )
{
outlinebrush.setColorAt(0.0f, shadow);
outlinebrush.setColorAt(1.0f, highlight);
brush.setColorAt(0.0f, sunken);
painter.setPen(Qt::NoPen);
}
else
{
outlinebrush.setColorAt(1.0f, shadow);
outlinebrush.setColorAt(0.0f, highlight);
brush.setColorAt(0.0f, normal1);
if( m_mouseOver == false )
{
brush.setColorAt(1.0f, normal2);
}
painter.setPen(QPen(outlinebrush, 1));
}
painter.setBrush(brush);
painter.drawRoundedRect( 0, 0, width(), height(), 5, 5 );
const int dd = active ? 1 : 0;
QPoint pt = QPoint( ( width() - m_img.width() ) / 2 + dd, 3 + dd );
if( s_iconOnlyMode )
{
pt.setY( ( height() - m_img.height() ) / 2 - 1 + dd );
}
painter.drawImage( pt, m_img );
if( s_iconOnlyMode == false )
{
const QString l = ( active && m_altLabel.isEmpty() == FALSE ) ?
m_altLabel : m_label;
const int w = painter.fontMetrics().width( l );
painter.setPen( Qt::black );
painter.drawText( ( width() - w ) / 2 +1+dd, height() - 4+dd, l );
painter.setPen( Qt::white );
painter.drawText( ( width() - w ) / 2 +dd, height() - 5+dd, l );
}
}
BaseIF* makePlate(const Real& height,
const Real& thick,
const Real& radius,
const int& doHoles,
const Real& holeRadius,
const Real& holeSpace)
{
RealVect zero(D_DECL(0.0,0.0,0.0));
RealVect xAxis(D_DECL(1.0,0.0,0.0));
bool inside = true;
// Create the plate without holes
Vector<BaseIF*> pieces;
RealVect normal1(D_DECL(1.0,0.0,0.0));
RealVect point1(D_DECL(height,0.0,0.0));
PlaneIF plane1(normal1,point1,inside);
pieces.push_back(&plane1);
RealVect normal2(D_DECL(-1.0,0.0,0.0));
RealVect point2(D_DECL(height+thick,0.0,0.0));
PlaneIF plane2(normal2,point2,inside);
pieces.push_back(&plane2);
TiltedCylinderIF middle(radius,xAxis,zero,inside);
pieces.push_back(&middle);
IntersectionIF plate(pieces);
// Make the drills
Vector<BaseIF*> drillBits;
// Compute how many drills are needed in each direciton - 2*num+1 -
// conservatively
int num = (int)((radius - holeRadius) / holeSpace + 1.0);
if (doHoles != 0)
{
for (int i = -num; i <= num; i++)
{
for (int j = -num; j <= num; j++)
{
RealVect center(D_DECL(0.0,i*holeSpace,j*holeSpace));
TiltedCylinderIF* drill = new TiltedCylinderIF(holeRadius,xAxis,center,inside);
drillBits.push_back(drill);
}
}
}
UnionIF drills(drillBits);
ComplementIF notDrills(drills,true);
// Drill the plate
IntersectionIF* holyPlate = new IntersectionIF(plate,notDrills);
return holyPlate;
}
void main()
{
bool patternfound = false;
bool resetAuto = false;
int nbImages = 0;
double moyFinale = 0;
bool detectionVisage = false;
int nbrLoopSinceLastDetection = 0;
int criticalValueOfLoopWithoutDetection = 15;
std::cout << "initialisation de Chehra..." << std::endl;
Chehra chehra;
std::cout << "done" << std::endl;
cv::Mat cameraMatrix, distCoeffs;
cv::Mat rvecs, tvecs;
std::vector<cv::Point2f> imagePoints;
std::vector<cv::Point2f> pointsVisage2D;
std::vector<cv::Point2f> moyPointsVisage2D;
std::vector<cv::Point3f> pointsVisage3D;
std::vector<cv::Point3f> visage;
std::vector<double> distances;
double moyDistances;
std::vector<std::vector<cv::Point2f>> images;
std::vector<cv::Mat> frames;
double s = 10.0f;
osg::Matrixd matrixS; // scale
matrixS.set(
s, 0, 0, 0,
0, s, 0, 0,
0, 0, s, 0,
0, 0, 0, 1);
pointsVisage3D.push_back(cv::Point3f(90,0,-80));
pointsVisage3D.push_back(cv::Point3f(-90,0,-80));
pointsVisage3D.push_back(cv::Point3f(0,0,0));
pointsVisage3D.push_back(cv::Point3f(600,0,600));
pointsVisage3D.push_back(cv::Point3f(0,0,600));
pointsVisage3D.push_back(cv::Point3f(-600,0,600));
/*
pointsVisage3D.push_back(cv::Point3f(13.1, -98.1,108.3)); // exterieur narine gauche
pointsVisage3D.push_back(cv::Point3f(-13.1, -98.1,108.3)); // exterieur narine droite
pointsVisage3D.push_back(cv::Point3f(0, -87.2, 124.2)); // bout du nez
pointsVisage3D.push_back(cv::Point3f(44.4, -57.9, 83.7)); // exterieur oeil gauche
pointsVisage3D.push_back(cv::Point3f(0, 55.4, 101.4)); // haut du nez, centre des yeux
pointsVisage3D.push_back(cv::Point3f(-44.4, -57.9, 83.7)); // exterieur oeil droit
*/
cv::FileStorage fs("../rsc/intrinsicMatrix.yml", cv::FileStorage::READ);
fs["cameraMatrix"] >> cameraMatrix;
fs["distCoeffs"] >> distCoeffs;
double f = (cameraMatrix.at<double>(0, 0) + cameraMatrix.at<double>(1, 1)) / 2; // NEAR = distance focale ; si pixels carrés, fx = fy -> np
//mais est généralement différent de fy donc on prend (pour l'instant) par défaut la valeur médiane
double g = 2000 * f; // je sais pas pourquoi. au pif.
fs.release();
cv::VideoCapture vcap(0);
if(!vcap.isOpened())
{
std::cout << "FAIL!" << std::endl;
return;
}
cv::Mat *frame = new cv::Mat(cv::Mat::zeros(vcap.get(CV_CAP_PROP_FRAME_HEIGHT), vcap.get(CV_CAP_PROP_FRAME_WIDTH), CV_8UC3));
do
{
vcap >> *frame;
}while(frame->empty());
osg::ref_ptr<osg::Image> backgroundImage = new osg::Image;
backgroundImage->setImage(frame->cols, frame->rows, 3,
GL_RGB, GL_BGR, GL_UNSIGNED_BYTE,
(uchar*)(frame->data),
osg::Image::AllocationMode::NO_DELETE, 1);
// read the scene from the list of file specified commandline args.
osg::ref_ptr<osg::Group> group = new osg::Group;
osg::ref_ptr<osg::Geode> cam = createHUD(backgroundImage, vcap.get(CV_CAP_PROP_FRAME_WIDTH), vcap.get(CV_CAP_PROP_FRAME_HEIGHT), cameraMatrix.at<double>(0, 2), cameraMatrix.at<double>(1, 2), f);
std::cout << "initialisation de l'objet 3D..." << std::endl;
osg::ref_ptr<osg::Node> objet3D = osgDB::readNodeFile("../rsc/objets3D/brain.obj");
//osg::ref_ptr<osg::Node> objet3D = osgDB::readNodeFile("../rsc/objets3D/dumptruck.osgt");
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
osg::Sphere* unitSphere = new osg::Sphere(osg::Vec3(0, -1000, 1000), 100.0);
osg::ShapeDrawable* unitSphereDrawable = new osg::ShapeDrawable(unitSphere);
osg::Geode* objet3D = new osg::Geode();
objet3D->addDrawable(unitSphereDrawable);
*/
//osg::StateSet* sphereStateset = unitSphereDrawable->getOrCreateStateSet();
//sphereStateset->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF);
//sphereStateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
std::cout << "done" << std::endl;
osg::StateSet* obectStateset = objet3D->getOrCreateStateSet();
obectStateset->setMode(GL_LIGHTING,osg::StateAttribute::OFF);
obectStateset->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF);
osg::ref_ptr<osg::MatrixTransform> mat = new osg::MatrixTransform();
// construct the viewer.
osgViewer::CompositeViewer compositeViewer;
osgViewer::View* viewer = new osgViewer::View;
osgViewer::View* viewer2 = new osgViewer::View;
// add the HUD subgraph.
group->addChild(cam);
mat->addChild(objet3D);
group->addChild(mat);
osg::Matrixd projectionMatrix;
projectionMatrix.makeFrustum(
-cameraMatrix.at<double>(0, 2), vcap.get(CV_CAP_PROP_FRAME_WIDTH) - cameraMatrix.at<double>(0, 2),
-cameraMatrix.at<double>(1, 2), vcap.get(CV_CAP_PROP_FRAME_HEIGHT) - cameraMatrix.at<double>(1, 2),
f, g);
osg::Vec3d eye(0.0f, 0.0f, 0.0f), target(0.0f, g, 0.0f), normal(0.0f, 0.0f, 1.0f);
// set the scene to render
viewer->setSceneData(group.get());
viewer->setUpViewInWindow(0, 0, 1920 / 2, 1080 / 2);
viewer->getCamera()->setProjectionMatrix(projectionMatrix);
viewer->getCamera()->setViewMatrixAsLookAt(eye, target, normal);
viewer2->setSceneData(group.get());
viewer2->setUpViewInWindow(1920 / 2, 0, 1920 / 2, 1080 / 2);
viewer2->getCamera()->setProjectionMatrix(projectionMatrix);
osg::Vec3d eye2(4 * f, 3 * f / 2, 0.0f), target2(0.0f, f, 0.0f), normal2(0.0f, 0.0f, 1.0f);
viewer2->getCamera()->setViewMatrixAsLookAt(eye2, target2, normal2);
compositeViewer.addView(viewer2);
compositeViewer.addView(viewer);
compositeViewer.realize(); // set up windows and associated threads.
do
{
patternfound = false;
resetAuto = false;
detectionVisage = false;
moyPointsVisage2D.clear();
pointsVisage2D.clear();
visage.clear();
moyDistances = 0;
distances.clear();
std::cout << "recherche de pattern" << std::endl
<< "nbr images sauvegardees : " << images.size() << std::endl;
vcap >> *frame;
frames.push_back(*frame);
detectionVisage = detecterVisage(frame, &chehra, &pointsVisage2D, &visage);
if(detectionVisage)
{
images.push_back(pointsVisage2D);
nbrLoopSinceLastDetection = 0;
group->addChild(mat);
}
else
nbrLoopSinceLastDetection++;
if((images.size() > NBRSAVEDIMAGES || nbrLoopSinceLastDetection > criticalValueOfLoopWithoutDetection) && !images.empty())
images.erase(images.begin());
if(images.empty())
group->removeChild(mat);
else
{
//cv::cornerSubPix(*frame, pointsVisage2D, cv::Size(5, 5), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
for(int i = 0; i < NBRFACEPOINTSDETECTED; i++)
{
cv::Point2f coordonee(0.0f, 0.0f);
for(int j = 0; j < images.size(); j++)
{
coordonee.x += images[j][i].x;
coordonee.y += images[j][i].y;
}
coordonee.x /= images.size();
coordonee.y /= images.size();
moyPointsVisage2D.push_back(coordonee);
}
cv::solvePnP(pointsVisage3D, moyPointsVisage2D, cameraMatrix, distCoeffs, rvecs, tvecs);
cv::Mat rotVec(3, 3, CV_64F);
cv::Rodrigues(rvecs, rotVec);
imagePoints = dessinerPoints(frame, pointsVisage3D, rotVec, tvecs, cameraMatrix, distCoeffs);
double t3 = tvecs.at<double>(2, 0);
double t1 = tvecs.at<double>(0, 0);
double t2 = tvecs.at<double>(1, 0) + t3 / 27.5; // and now, magic !
double r11 = rotVec.at<double>(0, 0);
double r12 = rotVec.at<double>(0, 1);
double r13 = rotVec.at<double>(0, 2);
double r21 = rotVec.at<double>(1, 0);
double r22 = rotVec.at<double>(1, 1);
double r23 = rotVec.at<double>(1, 2);
double r31 = rotVec.at<double>(2, 0);
double r32 = rotVec.at<double>(2, 1);
double r33 = rotVec.at<double>(2, 2);
osg::Matrixd matrixR; // rotation (transposee de rotVec)
matrixR.set(
r11, r21, r31, 0,
r12, r22, r32, 0,
r13, r23, r33, 0,
0, 0, 0, 1);
osg::Matrixd matrixT; // translation
matrixT.makeTranslate(t1, t2, t3);
osg::Matrixd matrix90; // rotation de repere entre opencv et osg
matrix90.makeRotate(osg::Quat(osg::DegreesToRadians(-90.0f), osg::Vec3d(1.0, 0.0, 0.0)));
mat->setMatrix(matrixS * matrixR * matrixT * matrix90);
// Calcul d'erreur de reprojection
double moy = 0;
for(int i = 0; i < pointsVisage2D.size(); i++)
{
double d = sqrt(pow(pointsVisage2D[i].y - imagePoints[i].y, 2) + pow(pointsVisage2D[i].x - imagePoints[i].x, 2));
distances.push_back(d);
moy += d;
}
moyDistances = moy / pointsVisage2D.size();
if(moyDistances > 2) // si l'ecart de reproj est trop grand, reset
resetAuto = true;
}
backgroundImage->dirty();
compositeViewer.frame();
}while(!compositeViewer.done());
}
BaseIF* makeVane(const Real& thick,
const RealVect& normal,
const Real& innerRadius,
const Real& outerRadius,
const Real& offset,
const Real& height,
const Real& angle)
{
RealVect zeroVect(D_DECL(0.0,0.0,0.0));
RealVect xAxis(D_DECL(1.0,0.0,0.0));
bool inside = true;
Vector<BaseIF*> vaneParts;
Real sinTheta = sin(angle);
#if CH_SPACEDIM == 3
Real cosTheta = cos(angle);
// Each side of the vane (infinite)
// rotate the normal around x-axis
RealVect normal1(D_DECL(normal[0],cosTheta*normal[1]-sinTheta*normal[2],sinTheta*normal[1]+cosTheta*normal[2]));
// rotate point on top of vane around x-axis
RealVect point(D_DECL(offset+height/2.0,-thick/2.0,0.0));
RealVect point1(D_DECL(point[0],cosTheta*point[1]-sinTheta*point[2],sinTheta*point[1]+cosTheta*point[2]));
PlaneIF plane1(normal1,point1,inside);
vaneParts.push_back(&plane1);
RealVect normal2(-normal1);
// rotate point on bottom (-point[2] of vane around x-axis
RealVect point2(D_DECL(point[0],-cosTheta*point[1]-sinTheta*point[2],-sinTheta*point[1]+cosTheta*point[2]));
PlaneIF plane2(normal2,point2,inside);
vaneParts.push_back(&plane2);
#endif
// Make sure we only get something to the right of the origin
RealVect normal3(D_DECL(0.0,-sinTheta,cosTheta));
RealVect point3(D_DECL(0.0,0.0,0.0));
PlaneIF plane3(normal3,point3,inside);
vaneParts.push_back(&plane3);
// Cut off the top and bottom
RealVect normal4(D_DECL(1.0,0.0,0.0));
RealVect point4(D_DECL(offset,0.0,0.0));
PlaneIF plane4(normal4,point4,inside);
vaneParts.push_back(&plane4);
RealVect normal5(D_DECL(-1.0,0.0,0.0));
RealVect point5(D_DECL(offset+height,0.0,0.0));
PlaneIF plane5(normal5,point5,inside);
vaneParts.push_back(&plane5);
// The outside of the inner cylinder
TiltedCylinderIF inner(innerRadius,xAxis,zeroVect,!inside);
vaneParts.push_back(&inner);
// The inside of the outer cylinder
TiltedCylinderIF outer(outerRadius,xAxis,zeroVect,inside);
vaneParts.push_back(&outer);
IntersectionIF* vane = new IntersectionIF(vaneParts);
return vane;
}
void Collision::modify(Particle& particle,float deltaTime) const
{
size_t index = particle.getIndex();
float radius1 = particle.getParamCurrentValue(PARAM_SIZE) * scale * 0.5f;
float m1 = particle.getParamCurrentValue(PARAM_MASS);
Group& group = *particle.getGroup();
// Tests collisions with all the particles that are stored before in the pool
for (size_t i = 0; i < index; ++i)
{
Particle& particle2 = group.getParticle(i);
float radius2 = particle2.getParamCurrentValue(PARAM_SIZE) * scale * 0.5f;
float sqrRadius = radius1 + radius2;
sqrRadius *= sqrRadius;
// Gets the normal of the collision plane
vec3 normal = particle.position();
normal -= particle2.position();
// float sqrDist = normal.getSqrNorm();
float sqrDist = glm::length2(normal);
if (sqrDist < sqrRadius) // particles are intersecting each other
{
vec3 delta = particle.velocity();
delta -= particle2.velocity();
if (dotProduct(normal,delta) < 0.0f) // particles are moving towards each other
{
float oldSqrDist = getSqrDist(particle.oldPosition(),particle2.oldPosition());
if (oldSqrDist > sqrDist)
{
// Disables the move from this frame
particle.position() = particle.oldPosition();
particle2.position() = particle2.oldPosition();
normal = particle.position();
normal -= particle2.position();
if (dotProduct(normal,delta) >= 0.0f)
continue;
}
// normal.normalize();
normal = glm::normalize(normal);
// Gets the normal components of the velocities
vec3 normal1(normal);
vec3 normal2(normal);
normal1 *= dotProduct(normal,particle.velocity());
normal2 *= dotProduct(normal,particle2.velocity());
// Resolves collision
float m2 = particle2.getParamCurrentValue(PARAM_MASS);
if (oldSqrDist < sqrRadius && sqrDist < sqrRadius)
{
// Tweak to separate particles that intersects at both t - deltaTime and t
// In that case the collision is no more considered as punctual
if (dotProduct(normal,normal1) < 0.0f)
{
particle.velocity() -= normal1;
particle2.velocity() += normal1;
}
if (dotProduct(normal,normal2) > 0.0f)
{
particle2.velocity() -= normal2;
particle.velocity() += normal2;
}
}
else
{
// Else classic collision equations are applied
// Tangent components of the velocities are left untouched
particle.velocity() -= (1.0f + (elasticity * m2 - m1) / (m1 + m2)) * normal1;
particle2.velocity() -= (1.0f + (elasticity * m1 - m2) / (m1 + m2)) * normal2;
normal1 *= ((1.0f + elasticity) * m1) / (m1 + m2);
normal2 *= ((1.0f + elasticity) * m2) / (m1 + m2);
particle.velocity() += normal2;
particle2.velocity() += normal1;
}
}
}
}
}
void QmaxButton::createButton(QPainter &painter)
{
QRect scaledRect;
scaledRect = matrix.mapRect(QRect(0,0,this->logicalSize.width(),this->logicalSize.height()));
QImage bg(this->m_strImage);
painter.setRenderHint(QPainter::SmoothPixmapTransform);
painter.setRenderHint(QPainter::Antialiasing);
painter.setPen(Qt::NoPen);
QLinearGradient brush1(0,0,0,scaledRect.height());
painter.drawImage(-2, -2, bg);
if(Colors::useEightBitPalette)
{
painter.setPen(QColor(120,120,120));
if(this->m_nPressed)
painter.setBrush(QColor(60,60,60));
else if(this->m_nHighlight)
painter.setBrush(QColor(100,100,100));
else
painter.setBrush(QColor(80,80,80));
}
else
{
QLinearGradient outlinebrush(0,0,0,scaledRect.height());
QLinearGradient brush(0,0,0,scaledRect.height());
brush.setSpread(QLinearGradient::PadSpread);
QColor highlight(255,255,255,128);
QColor shadow(0,0,0,70);
QColor sunken(220,220,220,30);
QColor normal1(255,255,245,60);
QColor normal2(255,255,235,10);
QColor normal3(200,200,200,10);
QColor normal4(255,255,250,255);
if(m_nType && m_nType != 5 )
{
normal1 = QColor(200,170,160,50);
normal2 = QColor(50,10,0,50);
}
if(m_nPressed)
{
outlinebrush.setColorAt(0.0f,shadow);
outlinebrush.setColorAt(1.0f,highlight);
brush.setColorAt(1.0f,sunken);
painter.setPen(Qt::NoPen);
}
else
{
outlinebrush.setColorAt(1.0f,shadow);
outlinebrush.setColorAt(0.0f,highlight);
brush.setColorAt(0.0f,normal1);
if(m_nHighlight)
brush.setColorAt(1.0f,normal2);
painter.setPen(QPen(outlinebrush,1));
}
if(this->isEnabled()==false )
{
outlinebrush.setColorAt(1.0f,shadow);
outlinebrush.setColorAt(0.0f,highlight);
brush.setColorAt(0.0f,normal3);
painter.setPen(QPen(outlinebrush,1));
}
if(m_nStatus )
{
outlinebrush.setColorAt(1.0f,shadow);
outlinebrush.setColorAt(0.0f,highlight);
brush.setColorAt(0.0f,normal4);
painter.setPen(QPen(outlinebrush,1));
}
painter.setBrush(brush);
}
if(m_nType == 1)
painter.drawRect(0,0,scaledRect.width(),scaledRect.height());
else if(m_nType == 0)
painter.drawRoundedRect(0,0,scaledRect.width(),scaledRect.height(),40.0,40.0,Qt::RelativeSize);
else if(m_nType == 5)
painter.drawEllipse(0,0,scaledRect.width(),scaledRect.height());
QFont font( "DejaVu Sans" );
font.setPointSize( 12 );
painter.setFont( font );
brush1.setColorAt(1.0f,QColor(255,255,255,255));
if(this->isEnabled()==false)
{
brush1.setColorAt(1.0f,QColor(200,200,200,100));
}
painter.setPen(QPen(brush1,1));
painter.setBrush(brush1);
QFontMetrics fMetrics = painter.fontMetrics();
QSize sz = fMetrics.size( Qt::TextWordWrap, m_strText );
QRectF txtRect( scaledRect.center(), sz );
int xPoint = (scaledRect.width()/2)- ((m_strText.count()/2)*10);
int yPoint = scaledRect.height()/2;
painter.drawText(xPoint,yPoint,m_strText);
}
QImage Button::createBackground() const
{
QImage image(rect().width(), rect().height(), QImage::Format_ARGB32_Premultiplied);
image.fill(QColor(0, 0, 0, 0).rgba());
QPainter painter(&image);
painter.setRenderHint(QPainter::SmoothPixmapTransform);
painter.setRenderHint(QPainter::Antialiasing);
painter.setPen(Qt::NoPen);
QLinearGradient outlinebrush(0, 0, 0, rect().height());
QLinearGradient brush(0, 0, 0, rect().height());
brush.setSpread(QLinearGradient::PadSpread);
QColor highlight(255, 255, 255, 70);
QColor shadow(0, 0, 0, 70);
QColor sunken(220, 220, 220, 30);
QColor normal1(255, 255, 245, 60);
QColor normal2(255, 255, 235, 10);
if (dark)
{
normal1 = QColor(200, 170, 160, 50);
normal2 = QColor(50, 10, 0, 50);
}
if (!isEnabled() || isDown())
{
outlinebrush.setColorAt(0.0f, shadow);
outlinebrush.setColorAt(1.0f, highlight);
brush.setColorAt(0.0f, sunken);
painter.setPen(Qt::NoPen);
}
else
{
outlinebrush.setColorAt(1.0f, shadow);
outlinebrush.setColorAt(0.0f, highlight);
brush.setColorAt(0.0f, normal1);
if (!hover) brush.setColorAt(1.0f, normal2);
painter.setPen(QPen(outlinebrush, 1));
}
painter.setBrush(brush);
if (round)
painter.drawRoundedRect(rect(), 15, rect().height());
else
painter.drawRect(0, 0, rect().width(), rect().height());
if (text() == "##Left##" || text() == "##Right##")
{
qreal xCenter = rect().width() * 0.5;
qreal yCenter = rect().height() * 0.5;
qreal xSize = 12;
qreal ySize = 6;
if (text() == "##Right##") xSize *= -1;
QPainterPath path;
path.moveTo(xCenter - xSize, yCenter);
path.lineTo(xCenter + xSize, yCenter - ySize);
path.lineTo(xCenter + xSize, yCenter + ySize);
path.lineTo(xCenter - xSize, yCenter);
painter.drawPath(path);
}
return image;
}
void main()
{
bool patternfound = false;
bool reset = false;
bool resetAuto = false;
int nbImages = 0;
double moyFinale = 0;
char key = 0;
bool detectionMire = false;
bool detectionVisage = false;
int cpt = 0, moyCpt = 0, i = 0;
std::cout << "initialisation de Chehra..." << std::endl;
Chehra chehra;
std::cout << "done" << std::endl;
cv::TermCriteria termcrit(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03);
cv::Size winSize(31, 31);
cv::Mat cameraMatrix, distCoeffs;
cv::Mat imCalib;
cv::Mat imCalibColor;
cv::Mat imCalibNext;
cv::Mat rvecs, tvecs;
cv::Mat Rc, C = cv::Mat(3, 1, CV_64F), rotVecInv;
std::vector<cv::Point2f> imagePoints;
std::vector<cv::Point3f> objectPoints;
std::vector<cv::Point3f> cubeObjectPoints;
std::vector<cv::Point3f> dessinPointsVisage;
std::vector<std::vector<cv::Point2f>> chessCornersInit(2);
std::vector<std::vector<cv::Point2f>> pointsVisageInit(2);
std::vector<cv::Point3f> chessCorners3D;
std::vector<cv::Point3f> pointsVisage3D;
std::vector<cv::Point3f> visage;
std::vector<double> distances;
double moyDistances;
// Creation des coins de la mire
for(int x = 0; x < COLCHESSBOARD; x++)
for(int y = 0; y < ROWCHESSBOARD; y++)
chessCorners3D.push_back(cv::Point3f(x * SIZEMIRE, y * SIZEMIRE, 0.0f));
// Creation des points a projeter
for(int x = 0; x < COLCHESSBOARD; x++)
for(int y = 0; y < ROWCHESSBOARD; y++)
objectPoints.push_back(cv::Point3f(x * SIZEMIRE, y * SIZEMIRE, 0.0f));
cv::FileStorage fs("../rsc/intrinsicMatrix.yml", cv::FileStorage::READ);
fs["cameraMatrix"] >> cameraMatrix;
fs["distCoeffs"] >> distCoeffs;
double f = (cameraMatrix.at<double>(0, 0) + cameraMatrix.at<double>(1, 1)) / 2; // NEAR = distance focale ; si pixels carrés, fx = fy -> np
//mais est généralement différent de fy donc on prend (pour l'instant) par défaut la valeur médiane
double g = 2000 * f; // je sais pas pourquoi. au pif.
fs.release();
cv::VideoCapture vcap(0);
if(!vcap.isOpened()){
std::cout << "FAIL!" << std::endl;
return;
}
cv::Mat *frame = new cv::Mat(cv::Mat::zeros(vcap.get(CV_CAP_PROP_FRAME_HEIGHT), vcap.get(CV_CAP_PROP_FRAME_WIDTH), CV_8UC3));
do
{
vcap >> *frame;
}while(frame->empty());
osg::ref_ptr<osg::Image> backgroundImage = new osg::Image;
backgroundImage->setImage(frame->cols, frame->rows, 3,
GL_RGB, GL_BGR, GL_UNSIGNED_BYTE,
(uchar*)(frame->data),
osg::Image::AllocationMode::NO_DELETE, 1);
// read the scene from the list of file specified commandline args.
osg::ref_ptr<osg::Group> group = new osg::Group;
osg::ref_ptr<osg::Geode> cam = createHUD(backgroundImage, vcap.get(CV_CAP_PROP_FRAME_WIDTH), vcap.get(CV_CAP_PROP_FRAME_HEIGHT), cameraMatrix.at<double>(0, 2), cameraMatrix.at<double>(1, 2), f);
std::cout << "initialisation de l'objet 3D..." << std::endl;
osg::ref_ptr<osg::Node> objet3D = osgDB::readNodeFile("../rsc/objets3D/Creature.obj");
std::cout << "done" << std::endl;
osg::StateSet* obectStateset = objet3D->getOrCreateStateSet();
obectStateset->setMode(GL_DEPTH_TEST,osg::StateAttribute::OFF);
osg::ref_ptr<osg::MatrixTransform> mat = new osg::MatrixTransform();
osg::ref_ptr<osg::PositionAttitudeTransform> pat = new osg::PositionAttitudeTransform();
// construct the viewer.
osgViewer::CompositeViewer compositeViewer;
osgViewer::View* viewer = new osgViewer::View;
osgViewer::View* viewer2 = new osgViewer::View;
// add the HUD subgraph.
group->addChild(cam);
mat->addChild(objet3D);
pat->addChild(mat);
group->addChild(pat);
pat->setScale(osg::Vec3d(3, 3, 3));
osg::Matrixd projectionMatrix;
projectionMatrix.makeFrustum(
-cameraMatrix.at<double>(0, 2), vcap.get(CV_CAP_PROP_FRAME_WIDTH) - cameraMatrix.at<double>(0, 2),
-cameraMatrix.at<double>(1, 2), vcap.get(CV_CAP_PROP_FRAME_HEIGHT) - cameraMatrix.at<double>(1, 2),
f, g);
osg::Vec3d eye(0.0f, 0.0f, 0.0f), target(0.0f, g, 0.0f), normal(0.0f, 0.0f, 1.0f);
// set the scene to render
viewer->setSceneData(group.get());
viewer->setUpViewInWindow(0, 0, 1920 / 2, 1080 / 2);
viewer->getCamera()->setProjectionMatrix(projectionMatrix);
viewer->getCamera()->setViewMatrixAsLookAt(eye, target, normal);
viewer2->setSceneData(group.get());
viewer2->setUpViewInWindow(1920 / 2, 0, 1920 / 2, 1080 / 2);
viewer2->getCamera()->setProjectionMatrix(projectionMatrix);
osg::Vec3d eye2(4 * f, 3 * f / 2, 0.0f), target2(0.0f, f, 0.0f), normal2(0.0f, 0.0f, 1.0f);
viewer2->getCamera()->setViewMatrixAsLookAt(eye2, target2, normal2);
compositeViewer.addView(viewer);
compositeViewer.addView(viewer2);
compositeViewer.realize(); // set up windows and associated threads.
do
{
group->removeChild(pat);
patternfound = false;
resetAuto = false;
detectionMire = false;
detectionVisage = false;
imagePoints.clear();
chessCornersInit[0].clear();
chessCornersInit[1].clear();
pointsVisageInit[0].clear();
pointsVisageInit[1].clear();
pointsVisage3D.clear();
dessinPointsVisage.clear();
visage.clear();
moyDistances = 0;
distances.clear();
imCalibNext.release();
std::cout << "recherche de pattern" << std::endl;
time_t start = clock();
double timer = 0;
do
{
start = clock();
vcap >> *frame;
backgroundImage->dirty();
//detectionMire = detecterMire(frame, &chessCornersInit[1], &imCalibNext);
detectionVisage = detecterVisage(frame, &chehra, &pointsVisageInit[1], &visage, &pointsVisage3D, &imCalibNext);
cpt++;
double duree = (clock() - start)/(double) CLOCKS_PER_SEC;
timer += duree;
if(timer >= 1){
std::cout << cpt << " fps" << std::endl;
moyCpt += cpt;
timer = 0;
duree = 0;
i++;
cpt = 0;
start = clock();
}
compositeViewer.frame();
}while(!detectionMire && !detectionVisage && !compositeViewer.done());
if(compositeViewer.done())
break;
std::cout << "pattern detectee" << std::endl << std::endl;
group->addChild(pat);
do
{
start = clock();
vcap >> *frame;
cv::Mat rotVec = trackingMire(frame, &imCalibNext, &pointsVisageInit, &pointsVisage3D, &cameraMatrix, &distCoeffs, &tvecs);
//cv::Mat rotVec = trackingMire(frame, &imCalibNext, &chessCornersInit, &chessCorners3D, &cameraMatrix, &distCoeffs, &tvecs);
//imagePoints = dessinerPoints(frame, objectPoints, rotVec, tvecs, cameraMatrix, distCoeffs);
imagePoints = dessinerPoints(frame, pointsVisage3D, rotVec, tvecs, cameraMatrix, distCoeffs);
double r11 = rotVec.at<double>(0, 0);
double r21 = rotVec.at<double>(1, 0);
double r31 = rotVec.at<double>(2, 0);
double r32 = rotVec.at<double>(2, 1);
double r33 = rotVec.at<double>(2, 2);
osg::Matrixd matrixR;
matrixR.makeRotate(
atan2(r32, r33), osg::Vec3d(1.0, 0.0, 0.0),
-atan2(-r31, sqrt((r32 * r32) + (r33 * r33))), osg::Vec3d(0.0, 0.0, 1.0),
atan2(r21, r11), osg::Vec3d(0.0, 1.0, 0.0));
mat->setMatrix(matrixR);
pat->setPosition(osg::Vec3d(tvecs.at<double>(0, 0), tvecs.at<double>(2, 0), -tvecs.at<double>(1, 0)));
//std::cout << "x = " << tvecs.at<double>(0, 0) << " - y = " << tvecs.at<double>(1, 0) << " - z = " << tvecs.at<double>(2, 0) << std::endl;
// Calcul d'erreur de reprojection
double moy = 0;
for(int j = 0; j < pointsVisageInit[1].size() ; j++)
{
double d = sqrt(pow(pointsVisageInit[0][j].y - imagePoints[j].y, 2) + pow(pointsVisageInit[0][j].x - imagePoints[j].x, 2));
distances.push_back(d);
moy += d;
}
moyDistances = moy / pointsVisageInit[1].size();
if(moyDistances > 1) // si l'ecart de reproj est trop grand, reset
resetAuto = true;
double duree = (clock() - start)/(double) CLOCKS_PER_SEC;
std::cout << (int)(1/duree) << " fps" << std::endl;
moyCpt += (int)(1/duree);
duree = 0;
i++;
backgroundImage->dirty();
compositeViewer.frame();
}while(!compositeViewer.done() && !resetAuto);
}while(!compositeViewer.done());
std::cout << std::endl << "Moyenne des fps : " << moyCpt/i << std::endl;
std::system("PAUSE");
}
void QmaxButton::createArrowBackground(QPainter &painter)
{
// printf("Arrow BackGround2\n");
QRect scaledRect;
scaledRect = matrix.mapRect(QRect(0, 0, this->logicalSize.width(), this->logicalSize.height()));
QImage image(scaledRect.width(), scaledRect.height(), QImage::Format_ARGB32_Premultiplied);
image.fill(QColor(0, 0, 0, 0).rgba());
//QPainter painter(image);
painter.setRenderHint(QPainter::SmoothPixmapTransform);
painter.setRenderHint(QPainter::Antialiasing);
painter.setPen(Qt::NoPen);
painter.drawImage(0, 0, image);
if(Colors::useEightBitPalette)
{
painter.setPen(QColor(120,120,120));
if(this->m_nPressed)
painter.setBrush(QColor(60,60,60));
else if(this->m_nHighlight)
painter.setBrush(QColor(100,100,100));
else
painter.setBrush(QColor(80,80,80));
}
else
{
QLinearGradient outlinebrush(0,0,0,scaledRect.height());
QLinearGradient brush(0,0,0,scaledRect.height());
brush.setSpread(QLinearGradient::PadSpread);
QColor highlight(255,255,255,128);
QColor shadow(0,0,0,70);
QColor sunken(220,220,220,30);
QColor normal1(88,88,89,255);
QColor normal2(88,88,89,255);
QColor normal3(0,0,200,10);
QColor normal4(255,255,250,255);
if(m_nType==3 || m_nType == 4)
{
normal1 = QColor(100,180,189,55);
normal2 = QColor(100,180,189,255);
}
if(m_nPressed)
{
outlinebrush.setColorAt(0.0f,shadow);
outlinebrush.setColorAt(1.0f,highlight);
brush.setColorAt(1.0f,sunken);
painter.setPen(Qt::NoPen);
}
else
{
outlinebrush.setColorAt(0.75f,shadow);
outlinebrush.setColorAt(0.0f,highlight);
brush.setColorAt(0.0f,normal2);
if(m_nHighlight)
brush.setColorAt(1.0f,normal1);
painter.setPen(QPen(outlinebrush,1));
}
if(this->isEnabled()==false )
{
outlinebrush.setColorAt(1.0f,shadow);
outlinebrush.setColorAt(0.0f,highlight);
brush.setColorAt(0.0f,normal3);
painter.setPen(QPen(outlinebrush,2));
}
if(m_nStatus )
{
outlinebrush.setColorAt(1.0f,shadow);
outlinebrush.setColorAt(0.0f,highlight);
brush.setColorAt(0.0f,normal2);
painter.setPen(QPen(outlinebrush,1));
}
painter.setBrush(brush);
}
// painter.drawRect(0, 0, scaledRect.width(), scaledRect.height());
float xOff = scaledRect.width() / 2;
float yOff = scaledRect.height() / 2;
float sizex = 5.0f * matrix.m11();
float sizey = 3.5f * matrix.m22();
if (m_nType == 3)
sizey *= -1;
QPainterPath path;
path.moveTo(xOff, yOff + (5 * sizey));
path.lineTo(xOff - (4 * sizex), yOff - (3 * sizey));
path.lineTo(xOff + (4 * sizex), yOff - (3 * sizey));
path.lineTo(xOff, yOff + (5 * sizey));
painter.drawPath(path);
}
