void Matrix::readMatrixFromFile(std::string name)
{
//otworz plik
std::ifstream fin;
fin.open(name.c_str());
if (!fin.is_open()) {
throw "Brak pliku z danymi";
}
fin >> verticles;
std::cout << verticles;
edges = ((getVerticles() * (getVerticles() - 1)) / 2);
M = new int*[getVerticles()];
for (int i = 0; i < getVerticles(); i++)
M[i] = new int[getVerticles()];
cleanMatrix();
for (int i = 0; i < getVerticles(); i++)
{
for (int j = 0; j < getVerticles(); j++)
{
fin >> M[i][j];
}
}
fin.close();
printMatrix();
}
void Matrix::readFromFile(std::string name)
{
//otworz plik
std::ifstream fin;
fin.open(name.c_str());
if (!fin.is_open()) {
throw "Brak pliku z danymi";
}
fin >> verticles;
std::cout << verticles;
edges = ((getVerticles() * (getVerticles() - 1)) / 2);
M = new int*[getVerticles()];
for (int i = 0; i < getVerticles(); i++)
M[i] = new int[getVerticles()];
cleanMatrix();
int start, end, weight;
for (int i = 0; i < getEdges() ; i++) {
fin >> start;
fin >> end;
fin >> weight;
M[start][end] = weight;
M[end][start] = weight;
}
fin.close();
printMatrix();
}
Matrix::Matrix(int verticles)
{
this->verticles = verticles;
edges = ((getVerticles() * (getVerticles() - 1)) / 2);
M = new int*[getVerticles()];
for (int i = 0; i < getVerticles(); i++)
M[i] = new int[getVerticles()];
cleanMatrix();
for (int i = 0; i < getVerticles(); i++)
{
for (int j = 0; j < getVerticles(); j++)
{
M[i][j] = rand() % 100;
}
}
}
MMatrix sgHair_controlJoint::getAngleWeightedMatrix( const MMatrix& targetMtx, double weight )
{
MMatrix mtx;
if( m_bStaticRotation )
{
mtx = MMatrix() * ( weight-1 ) + targetMtx * weight;
cleanMatrix( mtx );
}
else
{
MVector vUpDefault( 0, 1, 0 );
MVector vCrossDefault( 0,0,1 );
MVector vUpInput( targetMtx(1,0), targetMtx(1,1), targetMtx(1,2) );
double angleUp = vUpInput.angle( vUpDefault ) * weight;
if( vUpInput.x == 0 && vUpInput.z == 0 ) vUpInput.x = 1;
MVector direction( vUpInput.x, 0, vUpInput.z );
direction.normalize();
MVector vUp( sin( angleUp ) * direction.x, cos( angleUp ), sin( angleUp ) * direction.z );
double dot = vUp * MVector( 0.0, 0.0, 1.0 );
MVector vCross( 0.0, -dot, (dot+1) );
MVector vAim = vUp ^ vCross;
vAim.normalize();
vUp.normalize();
vCross = vAim ^ vUp;
mtx( 0, 0 ) = vAim.x;
mtx( 0, 1 ) = vAim.y;
mtx( 0, 2 ) = vAim.z;
mtx( 1, 0 ) = vUp.x;
mtx( 1, 1 ) = vUp.y;
mtx( 1, 2 ) = vUp.z;
mtx( 2, 0 ) = vCross.x;
mtx( 2, 1 ) = vCross.y;
mtx( 2, 2 ) = vCross.z;
}
return mtx;
}
/* Bayesian GMM adaptation */
AUD_Error gmm_adapt( void *hGmmHandle, AUD_Matrix *pAdaptData )
{
AUD_ASSERT( hGmmHandle && pAdaptData );
AUD_ASSERT( pAdaptData->dataType == AUD_DATATYPE_INT32S );
AUD_ASSERT( pAdaptData->rows > 0 );
GmmModel *pState = (GmmModel*)hGmmHandle;
AUD_Int32s ret = 0;
AUD_Int32s i, j, k, iter;
AUD_ASSERT( pAdaptData->cols == pState->width );
AUD_Double *pW = (AUD_Double*)calloc( pState->numMix * sizeof(AUD_Double), 1 );
AUD_ASSERT( pW );
AUD_Matrix mean;
mean.rows = pState->numMix;
mean.cols = pState->width;
mean.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &mean );
AUD_ASSERT( ret == 0 );
// second order moment
// AUD_Matrix sndOrderMnt;
// sndOrderMnt.rows = pState->numMix;
// sndOrderMnt.cols = pState->width;
// sndOrderMnt.dataType = AUD_DATATYPE_DOUBLE;
// ret = createMatrix( &sndOrderMnt );
// AUD_ASSERT( ret == 0 );
AUD_Vector compProb;
compProb.len = pState->numMix;
compProb.dataType = AUD_DATATYPE_DOUBLE;
ret = createVector( &compProb );
AUD_ASSERT( ret == 0 );
AUD_Double gamma = 16.;
for ( iter = 0; iter < 5; iter++ )
{
ret = cleanMatrix( &mean );
AUD_ASSERT( ret == 0 );
memset( pW, 0, pState->numMix * sizeof(AUD_Double) );
for ( i = 0; i < pAdaptData->rows; i++ )
{
AUD_Double llr = 0.;
AUD_Int32s *pDataRow = pAdaptData->pInt32s + i * pAdaptData->cols;
llr = gmm_llr( hGmmHandle, pAdaptData, i, &compProb );
// AUDLOG( "component probability:\n" );
for ( j = 0; j < mean.rows; j++ )
{
AUD_Double *pMeanRow = mean.pDouble + j * mean.cols;
AUD_Double prob;
// AUD_Double *pSndOrderMntRow = sndOrderMnt.pDouble + j * sndOrderMnt.cols;
prob = exp( compProb.pDouble[j] - llr );
// AUDLOG( "%f, ", prob );
pW[j] += prob;
for ( k = 0; k < mean.cols; k++ )
{
pMeanRow[k] += prob * pDataRow[k];
// pSndOrderMntRow[k] += prob * (AUD_Double)pDataRow[k] * (AUD_Double)pDataRow[k];
// AUD_ASSERT( pSndOrderMntRow[k] > 0 );
}
}
// AUDLOG( "\n" );
}
// adaptation procedure
// AUD_Double sumWeight = 0.;
for ( j = 0; j < pState->numMix; j++ )
{
AUD_Double alpha = 1. / ( pW[j] + gamma );
// AUDLOG( "alpha: %f\n", alpha );
AUD_Double *pMeanRow = mean.pDouble + j * mean.cols;
// AUD_Double *pSndOrderMntRow = sndOrderMnt.pDouble + j * sndOrderMnt.cols;
AUD_Int32s *pGmmMeanRow = pState->means.pInt32s + j * pState->means.cols;
// AUD_Int64s *pGmmVarRow = pState->cvars.pInt64s + j * pState->cvars.cols;
// pState->weights.pDouble[j] =( alpha * pW[j] / pAdaptData->rows + ( 1 - alpha ) * pState->weights.pDouble[j] ) * gamma;
// sumWeight += pState->weights.pDouble[j];
for ( k = 0; k < pState->width; k++ )
{
AUD_Double tmpMean;
// AUD_Double tmpVar;
tmpMean = alpha * pMeanRow[k] + alpha * gamma * pGmmMeanRow[k];
// tmpVar = alpha * pSndOrderMntRow[k] - tmpMean * tmpMean + ( 1 - alpha ) * ( (AUD_Double)pGmmVarRow[k] ) + ( 1 - alpha ) * pGmmMeanRow[k] * pGmmMeanRow[k];
// AUD_ASSERT( tmpVar > 0 );
pGmmMeanRow[k] = (AUD_Int32s)round( tmpMean );
// pGmmVarRow[k] = (AUD_Int64s)tmpVar;
// AUDLOG( "%f: %f : %d : %lld, ", tmpMean, tmpVar, pGmmMeanRow[k], pGmmVarRow[k] );
}
// AUDLOG( "\n" );
}
// re-normalize weightes
// for ( j = 0; j < pState->numMix; j++ )
// {
// pState->weights.pDouble[j] /= sumWeight;
// }
}
free( pW );
pW = NULL;
ret = destroyVector( &compProb );
AUD_ASSERT( ret == 0 );
ret = destroyMatrix( &mean );
AUD_ASSERT( ret == 0 );
// ret = destroyMatrix( &sndOrderMnt );
// AUD_ASSERT( ret == 0 );
return AUD_ERROR_NONE;
}
MStatus sgHair_controlJoint::setGravityJointPositionWorld()
{
MStatus status;
m_mtxArrGravityAdd = m_mtxArrBase;
if( m_weightGravity == 0 ) return MS::kSuccess;
if( !m_bStaticRotation )
{
double minParam = m_paramGravity - m_rangeGravity;
double maxParam = m_paramGravity;
double divRate = maxParam - minParam;
if( divRate == 0 ) divRate = 0.0001;
if( minParam > m_mtxArrBase.length()-1 ) return MS::kSuccess;
MDoubleArray dArrGravityWeights;
dArrGravityWeights.setLength( m_mtxArrBase.length() );
double beforeWeight = 1.0;
for( int i= m_mtxArrBase.length()-1; i > minParam, i >= 0; i-- )
{
double paramRate = ( i - minParam ) / divRate;
if( paramRate > 1 ) paramRate = 1.0;
else if( paramRate < 0 ) paramRate = 0.0;
double cuRate = beforeWeight - paramRate;
if( cuRate < 0 ) cuRate = 0;
dArrGravityWeights[i] = cuRate * m_weightGravity;
beforeWeight = paramRate;
}
MMatrix mtxDefault;
MMatrix mtxMult;
for( int i= m_mtxArrBase.length()-1; i > minParam, i >= 0; i-- )
{
if( dArrGravityWeights[i] == 0 ) continue;
double weight = dArrGravityWeights[i];
mtxDefault( 3,0 ) = m_mtxArrBase[i]( 3,0 );
mtxDefault( 3,1 ) = m_mtxArrBase[i]( 3,1 );
mtxDefault( 3,2 ) = m_mtxArrBase[i]( 3,2 );
mtxMult = getAngleWeightedMatrix( m_mtxGravityOffset, weight );
mtxMult( 3,0 ) = m_mtxArrBase[i]( 3,0 );
mtxMult( 3,1 ) = m_mtxArrBase[i]( 3,1 );
mtxMult( 3,2 ) = m_mtxArrBase[i]( 3,2 );
mtxMult = mtxDefault.inverse() * mtxMult;
for( int j=i; j< m_mtxArrBase.length(); j++ )
{
m_mtxArrGravityAdd[j] *= mtxMult;
}
}
}
else
{
double minParam = m_paramGravity - m_rangeGravity;
double maxParam = m_paramGravity;
double divRate = maxParam - minParam;
if( divRate == 0 ) divRate = 0.0001;
MDoubleArray dArrGravityWeights;
dArrGravityWeights.setLength( m_mtxArrBase.length() );
double weight;
for( int i= 0; i < m_mtxArrBase.length(); i++ )
{
if( i < minParam )weight=0;
else weight = (i-minParam)/divRate;
if( weight > 1 ) weight = 1;
m_mtxArrGravityAdd[i] = m_mtxArrBase[i];
double invWeight = 1-weight;
MMatrix mtx = weight * m_mtxGravityOffset*m_mtxArrBase[i] + invWeight * m_mtxArrBase[i];
cleanMatrix( mtx );
m_mtxArrGravityAdd[i] = mtx;
m_mtxArrGravityAdd[i]( 3,0 ) = m_mtxArrBase[i]( 3,0 );
m_mtxArrGravityAdd[i]( 3,1 ) = m_mtxArrBase[i]( 3,1 );
m_mtxArrGravityAdd[i]( 3,2 ) = m_mtxArrBase[i]( 3,2 );
}
cout << endl;
}
return MS::kSuccess;
}
int main(void)
{
sf::Window App(sf::VideoMode(800, 600, 32), "SFML OpenGL");
sf::Clock Clock;
SDL_Surface *w = NULL;
SDL_Surface *p = NULL;
SDL_Rect pos;
std::list<Button*> buttonList;
int matrixBin[480][640];
buttonList.push_back(new Button(new coords(320, 350, 100), 30));
buttonList.push_back(new Button(new coords(350, 380, 100), 30));
buttonList.push_back(new Button(new coords(380, 410, 100), 30));
buttonList.push_back(new Button(new coords(410, 440, 100), 30));
buttonList.push_back(new Button(new coords(440, 470, 100), 30));
p = SDL_CreateRGBSurface(SDL_HWSURFACE, 1, 1, 32, 0, 0, 0, 0);
SDL_Init(SDL_INIT_VIDEO);
w = SDL_SetVideoMode(800, 600, 32, SDL_HWSURFACE);
device = &freenect.createDevice<MyFreenectDevice>(0);
device->startDepth();
// Set color and depth clear value
glClearDepth(1.f);
glClearColor(0.f, 0.f, 0.f, 0.f);
// Enable Z-buffer read and write
glEnable(GL_DEPTH_TEST);
glDepthMask(GL_TRUE);
// Setup a perspective projection
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(90.f, 1.f, 1.f, 2500.f); App.SetActive();
int x, y, z;
x = -250;
y = 100;
z = -270;
int r = 45;
bool b = false;
while (App.IsOpened())
{
int tmpx = 0, tmpy = 0;
while (tmpy < 480)
{
tmpx = 0;
while (tmpx < 640)
{
matrixBin[tmpy][tmpx] = 0;
matrixDetectOrder[tmpy][tmpx] = 0;
++tmpx;
}
++tmpy;
}
sf::Event Event;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glTranslatef(x, y, z);
glRotated(180, 1, 0, 0);
glBegin(GL_POINTS);
glPointSize(1.0);
kk = 0;
std::list<Button*>::iterator itBut = buttonList.begin();
fingers.clear();
while (App.GetEvent(Event))
{
if (Event.Type == sf::Event::Closed)
{
device->stopDepth();
exit(0);
}
if ((Event.Type == sf::Event::KeyPressed) && (Event.Key.Code == sf::Key::Escape))
{
device->stopDepth();
exit(0);
}
if ((Event.Type == sf::Event::KeyPressed))
{
if ((Event.Key.Code == sf::Key::Up))
{
r += 10;
device->setTiltDegrees(r);
}
if ((Event.Key.Code == sf::Key::Down))
{
r -= 10;
device->setTiltDegrees(r);
}
if ((Event.Key.Code == sf::Key::W))
{z+=10;
}
if ((Event.Key.Code == sf::Key::S))
{z-=10;}
if ((Event.Key.Code == sf::Key::A))
{x+=10;
}
if ((Event.Key.Code == sf::Key::D))
{x-=10;}
if ((Event.Key.Code == sf::Key::E))
{y+=10;}
if ((Event.Key.Code == sf::Key::Q))
{y-=10;}
}
}
if (device->m_new_depth_frame && b == false)
resetView(b);
else if (device->m_new_depth_frame && b == true)
{
bool toDetect = false;
std::list<coords*>::iterator it;
std::list<coords*>::iterator it2;
std::list<coords*>::iterator itTmp;
it = coordsList.begin();
it2 = device->switchedTable.begin();
while (it != coordsList.end())
{
if ((*it)->y > 200)
{
glColor3ub(255, 255, 255);
matrixBin[(*it)->y][(*it)->x] = 0;
if ((*it)->x == (*it2)->x &&
(*it)->y == (*it2)->y &&
(*it2)->z < (*it)->z - 100 && (*it)->z < 9000)
toDetect = true;
if ((*it)->x == (*it2)->x &&
(*it)->y == (*it2)->y && toDetect == true)
{
if ((*it2)->z < (*it)->z - 10 && (*it2)->z > (*it)->z - 20)
{
glColor3ub(0, 0, 0);
matrixBin[(*it)->y][(*it)->x] = 1;
matrixDetectOrder[(*it)->y][(*it)->x] = 1;
glVertex3f((*it2)->x, (*it2)->y, (*it2)->z);
}
}
}
glColor3ub(255, 255, 255);
itBut = buttonList.begin();
while (itBut != buttonList.end())
{
(*itBut)->draw(*it);
++itBut;
}
glVertex3f((*it)->x, (*it)->y, (*it)->z);
++it;
++it2;
}
}
SDL_FillRect(w, NULL, SDL_MapRGB(w->format, 0, 0, 0));
int xx = 0, yy = 0;
cleanMatrix(matrixDetectOrder);
while (yy < 480)
{
xx = 0;
while (xx < 640)
{
if (matrixDetectOrder[yy][xx] == 1)
{
// addFinger(matrixDetectOrder, yy, xx, finger);
// // std::cout << finger << std::endl;
// ++finger;
int size;
// size = check_square_size(matrixBin, yy, xx);
size = checkSize(matrixBin, yy, xx);
// itBut = buttonList.begin();
if (size > 4)
{
SDL_FillRect(p, NULL, SDL_MapRGB(w->format, 255, 255, 255));
pos.x = xx;
pos.y = yy;
// // while (itBut != buttonList.end())
// // {
// // (*itBut)->checkCoords(xx, yy);
// // ++itBut;
// // }
SDL_BlitSurface(p, NULL, w, &pos);
// checkIfIsFinger(matrixBin, &yy, &xx, p, pos, w);
}
}
++xx;
}
++yy;
}
// xx = 0;
// yy = 0;
// while (yy < 480)
// {
// xx = 0;
// while (xx < 640)
// {
// if (matrixDetectOrder[yy][xx] == 1)
// {
// // if (matrixDetectOrder[yy][xx] == 1)
// // {
// SDL_FillRect(p, NULL, SDL_MapRGB(w->format, 255, 255, 255));
// pos.x = xx;
// pos.y = yy;
// SDL_BlitSurface(p, NULL, w, &pos);
// // }
// // else if (matrixDetectOrder[yy][xx] == 2)
// // {
// // SDL_FillRect(p, NULL, SDL_MapRGB(w->format, 255, 0, 0));
// // pos.x = xx;
// // pos.y = yy;
// // SDL_BlitSurface(p, NULL, w, &pos);
// // // }
// // // else if (matrixDetectOrder[yy][xx] == 3)
// // // {
// // SDL_FillRect(p, NULL, SDL_MapRGB(w->format, 0, 0, 255));
// // pos.x = xx;
// // pos.y = yy;
// // SDL_BlitSurface(p, NULL, w, &pos);
// // }
// }
// ++xx;
// }
// ++yy;
// }
SDL_Flip(w);
glEnd();
App.Display();
}
return 0;
}
void Castle::regenerateWall()
{
cleanMatrix();
addRectangles();
getPerimeter();
}
