void rotateRow( int x, int y, int z, int v, int s)
{
// rotate all objects matching x, y, z mask with value v
// s verse of rotation
int j, i, ix, iy, iz;
// animation loop 0-90 deg
for(j = 0; j <= 90; j+= STEP)
{
// clear painting scren
gClearScreen(); // clear the hidden screen
// search all objects for matching pattern
for( iz=0; iz<3; iz++)
for( iy=0; iy<3; iy++)
for(ix=0; ix<3; ix++)
{
if ( ix*iy*iz == 1) continue; // ignore c[1][1][1]
i = c[ix][iy][iz];
// check if requires rotation
if (( (ix+1)*x == v+1) || ((iy+1)*y == v+1) || ((iz+1)*z == v+1))
{ // rotate the corresponding axis
// transform and draw the object
initMatrix( mo, x*s*j, y*s*j, z*s*j, 0, 0, 0);
drawObject( mo, i);
// if final rotation
if ( j == 90)
rotateObject( mo, i);
}
else
{
// transform and draw the object
initMatrix( mo, 0, 0, 0, 0, 0, 0);
drawObject( mo, i);
}
}
// update visible screen
copyV();
} // for j steps
// update pointer matrix c[][][]
rotateC( x, y, z, v, s);
} // animate rotation of one row/face
void rotateCube( int alpha, int beta, int gamma)
{
int j, i, ar, br, gr;
for(j = 0; j <= 90; j+= STEP)
{
// clear painting screen
gClearScreen(); // clear the hidden screen
// rotate the corresponding axis
ar = alpha * j;
br = beta * j;
gr = gamma * j ;
for( i=objc; i<MAXOBJ; i++)
{
// transform and draw the object
initMatrix( mo, ar, br, gr, 0, 0, 0);
drawObject( mo, i);
// if final rotation
if ( j == 90)
rotateObject( mo, i);
}
// update visible screen
copyV();
} // for j steps
// update pointer matrix c[][][]
for ( i=0; i<3; i++)
{
if (( alpha<0)||( beta<0)||( gamma<0))
rotateC( ( alpha!=0), ( beta!=0), ( gamma!=0), i, -1);
else
rotateC( ( alpha!=0), ( beta!=0), ( gamma!=0), i, +1);
}
} // animate rotation of entire cube
void initVideo( void)
{
gClearScreen();
InitGraph();
// define HRES x VRES window
SetReg(0x04,(HRES-1)>>8);
SetReg(0x05,(HRES-1));
SetReg(0x08,0x00);
SetReg(0x09,(VRES-1));
copyV();
BacklightOn();
// 4. Timer3 on, prescaler 1:8, internal clock, period
//OpenTimer3( T3_ON | T3_PS_1_256 | T3_SOURCE_INT, FRAME_T-1);
// 9. Enable Timer3 Interrupts
// set the priority level 6
//mT3SetIntPriority( 6);
//mT3IntEnable( 1);
} // initVideo
// ************************************************************
int main( void)
{
// double t;
short v, i, j;
char start;
char roll;
char s[32], kj;
// init offsets
x0 = 125;
y0 = 100;
z0 = 200;
// hardware and video initialization
MMBInit();
initVideo();
initMatrix( mw, AOFFS, BOFFS, GOFFS, x0, y0, z0);
// 3. main loop
while( 1)
{
roll = 1;
start = 0;
gClearScreen();
// splash screen
setColor( 2); // red
AT(8, 1); putsV( "Rubik's Cube Demo");
AT(8, 3); putsV( " LDJ v1.0");
AT(8, 5); putsV( " for PIC32MX4 MMB");
copyV(); // update visible screen
while( !MMBReadKey());
srand( ReadCoreTimer());
// clear all objects
objc = MAXOBJ;
pyc = MAXPOLY;
pc = MAXP;
// init all objects angles
for( i=0; i<MAXOBJ; i++)
a[i] = b[i] = g[i] = 0;
// create the rubik cube
newCube();
// init cursor and define grid
qx = 1; qy = 1;
initGrid();
while ( roll)
{
// paint cube in current position (with cursor)
gClearScreen(); // clear the hidden screen
for( i=objc; i<MAXOBJ; i++)
{
initMatrix( mo, 0, 0, 0, 0, 0, 0);
drawObject( mo, i);
}
drawCursor( qx, qy);
copyV();
// read the joystick and rotate center rows
kj = MMBGetKey();
if ( kj & 0x80) // long pressure
{
switch( kj & 0x7F){
case JOY_RIGHT: // rotate whole cube
rotateCube( 0, +1, 0);
break;
case JOY_LEFT: // rotate whole cube
rotateCube( 0, -1, 0);
break;
case JOY_UP: // rotate whole cube
rotateCube( -1, 0, 0);
break;
case JOY_DOWN: // rotate whole cube
rotateCube( +1, 0, 0);
break;
case JOY_SELECT: // rotate face counter clockwise
rotateRow( 0, 0, 1, 2, +1);
break;
}
}
else // short pressure
{
switch( kj){
case JOY_RIGHT: // rotate only the current row
if (qx == 2)
rotateRow( 0, 1, 0, qy, +1);
else qx++;
break;
case JOY_LEFT: // rotate only the current row
if (qx == 0)
rotateRow( 0, 1, 0, qy, -1);
else qx--;
break;
case JOY_UP: // rotate only the current row
if (qy == 2)
rotateRow( 1, 0, 0, qx, -1);
else qy++;
break;
case JOY_DOWN: // rotate only the current row
if (qy == 0)
rotateRow( 1, 0, 0, qx, +1);
else qy--;
break;
case JOY_SELECT: // rotate face clockwise
rotateRow( 0, 0, 1, 2, -1);
break;
} // switch
} // short pressure
} //roll
} // main loop
} // main
void FluidSimulator::project(){
/*for (int j = simulationGrid->height - 3; j >=1; j--){
for (int i = simulationGrid->width - 3; i >= 1; i--){
if (simulationGrid->getCellType(i, j) == FLUID){
std::cout << "i" << i << "j" << j << ": " << simulationGrid->cells[i-1][j].u << " " << simulationGrid->cells[i][j-1].v << " " << simulationGrid->cells[i][j].u << " " << simulationGrid->cells[i][j].v << " " << simulationGrid->cells[i + 1][j].u << " " << simulationGrid->cells[i][j + 1].v << "\n";
}
}
}*/
int fluidCellsCount = simulationGrid->getFluidCellCount();
std::cout <<"N" << fluidCellsCount << "\n";
int index = 0;
for (int i = 0; i < simulationGrid->width; i++){
for (int j = 0; j < simulationGrid->height; j++){
if (simulationGrid->getCellType(i, j) == FLUID){
indices.insert(std::make_pair(std::make_pair(i, j), index++));
}
}
}
std::cout << indices.size();
pressureCoefficients = new double*[fluidCellsCount];
double * rhs = calculateNegativeDivergence();
for (int i = 0; i < fluidCellsCount; i++){ // return if pressure is already divergent free
if (rhs[i] != 0)
break;
if (i + 1 == fluidCellsCount){
indices.clear();
std::cout << "divFree\n";
return;
}
}
double * Adiag = new double[fluidCellsCount]();
double * Aplusi = new double[fluidCellsCount]();
double * precon = new double[fluidCellsCount]();
double * Aplusj = new double[fluidCellsCount]();
double * Aprevi = new double[fluidCellsCount]();
double * Aprevj = new double[fluidCellsCount]();
double scale = dt / (DENSITY*simulationGrid->getCellSize()*simulationGrid->getCellSize());
for (int i = 0; i < simulationGrid->width; i++){ //compute A... where A*pressure(unknown) = negative divergence... pressure makes the field divergent free
for (int j = 0; j < simulationGrid->height; j++){
if (simulationGrid->getCellType(i, j) != FLUID) continue;
int place = indices[std::make_pair(i, j)];
if (simulationGrid->getCellType(i, j) == FLUID && simulationGrid->getCellType(i + 1, j) == FLUID){
Adiag[place] += scale;
Aplusi[place] = -scale;
}
else if (simulationGrid->getCellType(i, j) == FLUID && simulationGrid->getCellType(i + 1, j) == FREE){
Adiag[place] += scale;
}
if (simulationGrid->getCellType(i, j) == FLUID && simulationGrid->getCellType(i, j + 1) == FLUID){
Adiag[place] += scale;
Aplusj[place] = -scale;
}
else if (simulationGrid->getCellType(i, j) == FLUID && simulationGrid->getCellType(i, j + 1) == FREE)
Adiag[place] += scale;
if (simulationGrid->getCellType(i, j) == FLUID && simulationGrid->getCellType(i - 1, j) == FLUID){
Adiag[place] += scale;
Aprevi[place] = -scale;
}
else if (simulationGrid->getCellType(i, j) == FLUID && simulationGrid->getCellType(i - 1, j) == FREE){
Adiag[place] += scale;
}
if (simulationGrid->getCellType(i, j) == FLUID && simulationGrid->getCellType(i, j-1) == FLUID){
Adiag[place] += scale;
Aprevj[place] = -scale;
}
else if (simulationGrid->getCellType(i, j) == FLUID && simulationGrid->getCellType(i, j-1) == FREE){
Adiag[place] += scale;
}
/* if (simulationGrid->getCellType(i, j) == FREE && simulationGrid->getCellType(i, j + 1) == FLUID)
Adiag[indices[std::make_pair(i, j + 1)]] += scale;
if (simulationGrid->getCellType(i, j) == FREE && simulationGrid->getCellType(i + 1, j) == FLUID)
Adiag[indices[std::make_pair(i + 1, j)]] += scale;*/
}
}
/*for (int i = 15; i < simulationGrid->width; i=i+9){
for (int j = 0; j < simulationGrid->height; j++){
if (simulationGrid->getCellType(i, j) != FLUID) continue;
int place = indices[std::make_pair(i, j)];
std::cout << "***place:" << i << " " << j << " " << place << " Adiag: " << Adiag[place] / scale << "\n";
std::cout << " Aprevi: " << Aprevi[place] / scale << " " << getIndex(i-1, j) << " " << indices.count(std::make_pair(i-1, j)) << "\n";
std::cout << " Aprevj: " << Aprevj[place] / scale << " " << getIndex(i, j-1) << " " << indices.count(std::make_pair(i, j - 1)) << "\n";
std::cout << " Aplusi: " << Aplusi[place] / scale << " " << getIndex(i + 1, j) << " " << indices.count(std::make_pair(i + 1, j)) << "\n";
std::cout << " Aplusj: " << Aplusj[place] / scale << " " << getIndex(i, j+1) << " " << indices.count(std::make_pair(i, j + 1)) << "\n";
}
}*/
double tau = 0.97, beta = 0.25;
for (int i = 0; i < simulationGrid->width; i++){ //compute preconditioner... initial guess for solving the system
for (int j = 0; j < simulationGrid->height; j++){
if (simulationGrid->getCellType(i, j) == FLUID){
int place = indices[std::make_pair(i, j)];
double e = Adiag[place];
if (indices.count(std::make_pair(i - 1, j)) > 0)
e -= square(Aprevi[place] * precon[getIndex(i-1,j)]);
if (indices.count(std::make_pair(i, j - 1)) > 0)
e -= square(Aprevj[place] * precon[getIndex(i, j - 1)]);
if (indices.count(std::make_pair(i - 1, j)) > 0)
e -= tau*(Aplusi[getIndex(i - 1, j)] * Aplusj[getIndex(i - 1, j)] * square(precon[getIndex(i - 1, j)]));
if (indices.count(std::make_pair(i, j - 1)) > 0)
e -= tau*(Aplusj[getIndex(i, j - 1)] * Aplusi[getIndex(i, j - 1)] * square(precon[getIndex(i, j - 1)]));
if (e < beta * Adiag[place])
e = Adiag[place];
precon[place] = 1 / sqrt(e+0.000001);
}
}
}
double * p = new double[fluidCellsCount]();
double * r = copyV(rhs, fluidCellsCount);
double * z = applyPreconditioner(Adiag, Aplusi, Aplusj, Aprevi, Aprevj, precon, r);
double * s = copyV(z, fluidCellsCount);
double dp = dotproduct(z, r, fluidCellsCount);
bool done = false;
double maxR;
int iterations = 0;
while (!done && iterations < MAX_ITERATIONS){
z = apply(Adiag, Aplusi, Aplusj, Aprevi, Aprevj, s, fluidCellsCount);
double den = dotproduct(z, s, fluidCellsCount);
if (den == 0)
break;
double alph = dp / den;
maxR = 0;
for (int i = 0; i < fluidCellsCount; i++){
p[i] = p[i] + alph*s[i];
r[i] = r[i] - alph*z[i];
if (r[i] > maxR)
maxR = r[i];
}
if (abs(maxR) <= TOLERANCE){
done = true;
//std::cout << "MX" << maxR << "#";
break;
}
z = applyPreconditioner(Adiag, Aplusi, Aplusj, Aprevi, Aprevj, precon, r);
double dpNew = dotproduct(z, r, fluidCellsCount);
double beeta = dpNew / dp;
//std::cout << "beeta" << beeta << "dp" << dp << "dpnew" << dpNew;
for (int i = 0; i < fluidCellsCount; i++)
s[i] = z[i] + beeta*s[i];
dp = dpNew;
iterations++;
//if (iterations + 1 == MAX_ITERATIONS || done) std::cout << "MX" << maxR << "#";
}
for (int i = 0; i < fluidCellsCount; i++)
p[i] *= dt / (DENSITY * simulationGrid->getCellSize());
double * res = apply(Adiag, Aplusi, Aplusj, Aprevi, Aprevj, p, fluidCellsCount);
for (int i = 0; i < simulationGrid->width; i++){
for (int j = 0; j < simulationGrid->height; j++){
if (simulationGrid->getCellType(i, j) != FLUID) continue;
int place = indices[std::make_pair(i, j)];
if (simulationGrid->getCellType(i, j) == FLUID && ( simulationGrid->getCellType(i - 1, j)!= FLUID || simulationGrid->getCellType(i + 1, j) != FLUID)){
std::cout << dt / (DENSITY * simulationGrid->getCellSize()) << "p " << p[place] << " ";
std::cout << i << " " << j << "\n";
//std::cout << "^" << rhs[place] << "\n";
}
}
}
//std::cout << "uterations: " << iterations << "\n";
/*for (int i = 0; i < fluidCellsCount; i++)
std::cout << "p: " << p[i] << "\n";*/
for (int i = 0; i < simulationGrid->width; i++){
for (int j = 0; j < simulationGrid->height; j++){
double sc = 1;
if (simulationGrid->getCellType(i, j) == FLUID){
int place = indices[std::make_pair(i, j)];
simulationGrid->cells[i][j].u -= sc*p[place];
simulationGrid->cells[i + 1][j].u += sc*p[place];
simulationGrid->cells[i][j].v -= sc*p[place];
simulationGrid->cells[i][j + 1].v += sc*p[place];
}
}
}
for (int i = 0; i < simulationGrid->width; i++){
for (int j = 0; j < simulationGrid->height; j++){
if (simulationGrid->getCellType(i, j) == SOLID){
simulationGrid->cells[i][j].u = 0;
if (i + 1 <simulationGrid->width)
simulationGrid->cells[i + 1][j].u = 0;
simulationGrid->cells[i][j].v = 0;
if (j + 1 <simulationGrid->height)
simulationGrid->cells[i][j + 1].v = 0;
}
}
}
/*for (int i = 0; i < simulationGrid->width; i++){ //compute preconditioner... initial guess for solving the system
for (int j = 0; j < simulationGrid->height; j++){
if (simulationGrid->getCellType(i, j) != FLUID) continue;
int place = indices[std::make_pair(i, j)];
if (simulationGrid->getCellType(i, j) == FLUID && (simulationGrid->getCellType(i - 1, j) != FLUID || simulationGrid->getCellType(i + 1, j) != FLUID)){
//std::cout << "u " << simulationGrid->cells[i][j].u << " v" << simulationGrid->cells[i][j].v ;
std::cout << i << " " << j << "\n";
if (simulationGrid->getCellType(i - 1, j) == FLUID)
std::cout << "* " << (simulationGrid->cells[i + 1][j].u + simulationGrid->cells[i][j].u) / 2 << " " << (simulationGrid->cells[i][j].v + simulationGrid->cells[i][j + 1].v) / 2 << "\n";
if (simulationGrid->getCellType(i + 1, j) == FLUID)
std::cout << "* " << (simulationGrid->cells[i + 1][j].u + simulationGrid->cells[i][j].u) / 2 << " " << (simulationGrid->cells[i][j].v + simulationGrid->cells[i][j+1].v) / 2 <<"\n";
//std::cout << "^" << rhs[place] << "\n";
}
}
}*/
indices.clear();
delete[] rhs;
delete[] Adiag;
delete[] Aplusi;
delete[] precon;
delete[] Aplusj;
delete[] Aprevi;
delete[] Aprevj;
std::cout << "aft" << indices.size();
}
