static List<Pixel> line(Pixel a,Pixel b) {
//printf("a\n");
List<Pixel> ls;
if (a==b) {
ls.add(a);
//printf("b\n");
return ls;
}
Pixel diff=b-a;
if (abs(diff.x)>abs(diff.y)) {
int diffm=abs(diff.x);
for (int i=0;i<=diffm;i++) {
// printf("%i/%i\n",i,diffm);
// Pixel p=a+diff*((float)i/(float)diffm);
Pixel p=Pixel(a.x+mysgn(diff.x)*i,a.y+diff.y*((float)i/(float)diffm));
ls.add(p);
}
} else {
int diffm=abs(diff.y);
for (int i=0;i<=diffm;i++) {
// printf(" %i/%i\n",i,diffm);
// Pixel p=a+diff*((float)i/(float)diffm);
Pixel p=Pixel(a.x+diff.x*((float)i/(float)diffm),a.y+mysgn(diff.y)*i);
ls.add(p);
}
}
//printf("c\n");
return ls;
}
void thicklinev (V2d a, V2d b, float w, CT c) {
V2d p=V2d(w/2.0,0);
V2d a2b=V2d(0,w/2.0)*mysgn(b.y-a.y);
filltri(a-a2b+p,a-a2b-p,b+a2b+p,c);
filltri(a-a2b-p,b+a2b+p,b+a2b-p,c);
// error("Writeable::vthickline not cast before write");
}
void thicklineh (V2d a, V2d b, float w, CT c) {
V2d p=V2d(0,w/2.0+1);
V2d a2b=V2d(w/2.0-1,0)*mysgn(b.x-a.x);
filltri(a-a2b+p,a-a2b-p,b+a2b+p,c);
filltri(a-a2b-p,b+a2b+p,b+a2b-p,c);
// error("Writeable::hthickline not cast before write");
}
/* Move a star according to acting forcefields */
void stars_move(int p) {
float nx,ny;
float x=cx[p];
float y=cy[p];
if (fon[1]) {
x = x * var[1]; y = y * var[1];
}
if (fon[2]) {
nx=x*cos(var[2])+y*sin(var[2]);
ny=-x*sin(var[2])+y*cos(var[2]);
x=nx;
y=ny;
}
if (fon[3]) {
y=y*var[3];
}
if (fon[4]) {
x=(x-1.0)*var[3]+1.0;
}
if (fon[5]) {
x=x+var[5]*x;
}
if (fon[6]) {
x = mysgn(x) * pow(fabs(x),var[6]);
y = mysgn(y) * pow(fabs(y),var[6]);
}
if (fon[7]) {
if (fon[0]) {
if (fon[9]) {
x=x+var[7]*(-1.0+2.0*(float)(p%2));
} else {
x=x+var[7]*(-1.0+2.0*(float)((p%50)/49.0));
}
}
}
if (fon[8]) {
if (fon[0]) {
if (fon[9]) {
y=y+var[8]*(-1.0+2.0*(float)(p%2));
} else {
y=y+var[8]*(-1.0+2.0*(float)((p%50)/49.0));
}
}
}
cx[p]=x;
cy[p]=y;
}
int merge(uchar x,uchar y,float amount) {
int d=myabs(y-x);
int s=mysgn(y-x);
if (d>128) {
d=256-d;
s=-s;
}
return (uchar)(x+s*d*amount);
}
void main() {
allegrosetup(scrwid,scrhei);
makepalette(&greypalette);
mypalette(255,0,0,0);
mypalette(0,0,0,0);
mypalette(255,1,1,1);
mypalette(128,0,0,0);
randomise();
for (int i=0;i<=5;i++) {
waves+Wave();
//waves.num(i).display();
}
for (int r=0;r<8;r++) {
for (int g=0;g<8;g++) {
int palc=r+g*8;
mypalette(palc,(r==0?0:0.1+0.6*r/7.0),(g==0?0:0.1+0.2*g/7.0),(g==0?0:0.1+0.7*g/7.0));
// Good colours: mypalette(palc,(r==0?0:0.3+0.4*r/7.0),(g==0?0:0.1+0.2*g/7.0),(g==0?0:0.3+0.5*g/7.0));
}
}
V3d vel=V3d(0,0,0);
V3d acc=V3d(0,0,0);
Ori ori;
float droll=0;
float dyaw=0;
float roll=0;
float yaw=0;
float pitch=0;
int frame=0;
List<V3d> particles=List<V3d>(gridsize*gridsize*gridsize);
/* for (int i=1;i<gridsize*gridsize*gridsize;i++) {
particles+gridsize*V3d(floatrnd(-1,1),floatrnd(-1,1),floatrnd(-1,1));
}*/
for (float thru=0;thru<1.0;thru+=0.01) {
particles+getpos(thru);
//v.print();
}
starttimer();
float thru=0;
V3d pos=getpos(thru);
do {
thru+=0.001;
V3d last=pos;
V3d pos=getpos(thru);
V3d newz=pos-last;
ori.forcez(newz);
frame++;
float pd=1.6+1.3*sin(2*pi*frame/1000.0);
PPsetup(scrwid,scrhei,pd);
left.clear(0);
right.clear(0);
// b.fadeby(16);
V3d off=V3d(mymod(pos.x,gridsize),mymod(pos.y,gridsize),mymod(pos.z,gridsize));
int ioff=-mydiv(pos.x,gjump);
int joff=-mydiv(pos.y,gjump);
int koff=-mydiv(pos.z,gjump);
ori.quickorisetup();
// Need to find largest axis, and do loops with that one outside
int ks=mysgn(ori.qz.z);
int js=-mysgn(ori.qz.y);
int is=-mysgn(ori.qz.x);
for (int i=1;i<=particles.len;i++) {
int x,y;
V3d p=particles.num(i);
//V3d cen=p-off;
V3d cen=p+pos;
cen=V3d(mymod2(cen.x,-gridsize,gridsize),mymod2(cen.y,-gridsize,gridsize),mymod2(cen.z,-gridsize,gridsize));
cen=V3d::qorientate(cen,ori);
int c=7.0-chop(6.0*(cen.z+gridsize)/(float)(gridsize*2),0,6);
plotsphere(cen,c);
}
for (int i=0;i<scrwid;i++) {
for (int j=0;j<scrhei;j++) {
b.bmp[j][i]=(left.bmp[j][i] | right.bmp[j][i]);
}
}
b.writetoscreen();
// acc=hang(acc,V3d::origin,0.95,0.01);
// vel=hang(vel,(ori.qz/5.0).neg(),0.9,0)+acc;
// pos=pos+vel;
// droll=hang(droll,0,0.9,0.01);
// dyaw=hang(dyaw,0,0.95,0.01);
// roll=hang(roll,0,0.95,0)+droll;
// yaw=hang(yaw,0,0.92,0)+dyaw;
// pitch=hang(pitch,0,0.999,0.01);
// pos=pos+ori.qz*4;
// ori.roll(roll/5.0);
// ori.yaw(yaw/5.0);
// pos=pos-ori.z()*4;
framedone();
} while (!key[KEY_SPACE] && !key[KEY_ESC]);
savetimer();
allegro_exit();
displayframespersecond();
}
void main() {
allegrosetup(scrwid,scrhei);
makepalette(&greypalette);
mypalette(255,0,0,0);
mypalette(0,0,0,0);
mypalette(255,1,1,1);
mypalette(128,0,0,0);
for (int r=0;r<8;r++) {
for (int g=0;g<8;g++) {
int palc=r+g*8;
mypalette(palc,(r==0?0:0.3+0.4*r/7.0),(g==0?0:0.1+0.2*g/7.0),(g==0?0:0.3+0.5*g/7.0));
}
}
// randomise();
V3d pos=V3d(0,0,0);
V3d vel=V3d(0,0,0);
V3d acc=V3d(0,0,0);
Ori ori;
float droll=0;
float dyaw=0;
float roll=0;
float yaw=0;
float pitch=0;
int frame=0;
starttimer();
do {
frame++;
float pd=2.0+1.8*sin(2*pi*frame/1000.0);
PPsetup(scrwid,scrhei,pd);
left.clear(0);
right.clear(0);
// b.fadeby(16);
V3d off=V3d(mymod(pos.x,gjump),mymod(pos.y,gjump),mymod(pos.z,gjump));
int ioff=-mydiv(pos.x,gjump);
int joff=-mydiv(pos.y,gjump);
int koff=-mydiv(pos.z,gjump);
ori.quickorisetup();
// Need to find largest axis, and do loops with that one outside
int ks=mysgn(ori.qz.z);
int js=-mysgn(ori.qz.y);
int is=-mysgn(ori.qz.x);
for (int k=ks*gridsize;k!=-ks*gridsize;k=k-ks*gjump)
for (int i=-is*gridsize;i!=is*gridsize;i=i+is*gjump)
for (int j=-js*gridsize;j!=js*gridsize;j=j+js*gjump) {
int x,y;
V3d cen=V3d(i,j,k)-off;
cen=V3d::qorientate(cen,ori);
int c=7-chop(7*(cen.z+pd+1)/(gridsize+2),0,7);
plotsphere(cen,c);
}
for (int i=0;i<scrwid;i++) {
for (int j=0;j<scrhei;j++) {
b.bmp[j][i]=(left.bmp[j][i] | right.bmp[j][i]);
}
}
b.writetoscreen();
acc=hang(acc,V3d::origin,0.95,0.01);
vel=hang(vel,ori.qz/10,0.94,0)+acc;
pos=pos+vel;
droll=hang(droll,0,0.9,0.01);
dyaw=hang(dyaw,0,0.95,0.01);
roll=hang(roll,0,0.95,0)+droll;
yaw=hang(yaw,0,0.92,0)+dyaw;
pitch=hang(pitch,0,0.999,0.01);
pos=pos+ori.qz*4;
ori.roll(roll/5.0);
ori.yaw(yaw/5.0);
pos=pos-ori.z()*4;
framedone();
} while (!key[KEY_SPACE] && !key[KEY_ESC]);
savetimer();
allegro_exit();
displayframespersecond();
}
void main() {
allegrosetup(scrwid,scrhei);
makepalette(&greypalette);
mypalette(255,0,0,0);
mypalette(0,0,0,0);
mypalette(255,1,1,1);
mypalette(128,0,0,0);
randomise();
for (int i=0;i<=15;i++) {
waves+Wave();
// waves.num(i).display();
}
for (int r=0;r<16;r++) {
for (int g=0;g<16;g++) {
int palc=r+g*16;
mypalette(palc,(r==0?0:brightness+0.2+change*0.5*r/15.0),(g==0?0:brightness+change*0.1+0.2*g/15.0),(g==0?0:brightness+change*0.2+0.6*g/15.0));
// Groovy colours mypalette(palc,(r==0?0:0.1+0.6*r/15.0),(g==0?0:0.1+0.2*g/15.0),(g==0?0:0.1+0.7*g/15.0));
// Good colours: mypalette(palc,(r==0?0:0.3+0.4*r/15.0),(g==0?0:0.1+0.2*g/15.0),(g==0?0:0.3+0.5*g/15.0));
}
}
V3d vel=V3d(0,0,0);
V3d acc=V3d(0,0,0);
Ori ori;
float droll=0;
float dyaw=0;
float dpitch=0;
int frame=0;
List<V3d> particles=List<V3d>(gridsize*gridsize*gridsize);
/* for (int i=1;i<gridsize*gridsize*gridsize;i++) {
particles+gridsize*V3d(floatrnd(-1,1),floatrnd(-1,1),floatrnd(-1,1));
}*/
int ps=1;
float rad=0;
for (float thru=0;thru<1.0;thru+=0.0016) {
V3d here=getpos(thru);
V3d forward=getpos(thru+0.00001)-here;
V3d up=V3d::normcross(V3d::crazy,forward);
V3d right=V3d::normcross(forward,up);
for (int i=0;i<ps;i++) {
float t=2*pi*(float)i/(float)ps;
float s=sin(t);
float c=cos(t);
particles+(here+rad*(s*up+c*right));
}
// v.print();
}
starttimer();
float thru=0;
float marker=0;
V3d pos=getpos(marker);
V3d *tail=new V3d[taillen];
int tailpos=0;
do {
thru+=0.001;
// V3d last=V3d(pos.x,pos.y,pos.z);
// V3d pos=getpos(thru);
// V3d next=getpos(thru+0.00001);
// V3d newz=next-pos;
// ori.forcez(newz);
frame++;
// float pd=1.6+1.3*sin(2*pi*frame/1000.0);
float pd=2.0;
PPsetup(scrwid,scrhei,pd);
left.clear(0);
right.clear(0);
// b.fadeby(16);
int ioff=-mydiv(pos.x,gjump);
int joff=-mydiv(pos.y,gjump);
int koff=-mydiv(pos.z,gjump);
ori.quickorisetup();
// Need to find largest axis, and do loops with that one outside
int ks=mysgn(ori.qz.z);
int js=-mysgn(ori.qz.y);
int is=-mysgn(ori.qz.x);
for (int i=1;i<=particles.len;i++) {
int x,y;
V3d p=particles.num(i);
V3d cen=p-pos+0.0*ori.y;
cen=V3d::disorientate(cen,ori);
int c=15.0-chop(14.0*(cen.z+gridsize)/(float)(gridsize*2),0,14);
// int c=7.0-chop(6.0*(i/particles.len),0,6);
plotsphere(cen,c);
}
// Plot and move marker
V3d m;
for (int i=1;i<=15;i++) {
m=V3d::disorientate(getpos(marker)-pos,ori);
if (m.mod()<markerrange)
marker+=0.0002;
}
float u=PPgetunitnoadd(m);
bool plot=false;
if (u) {
int x,y;
float rad=0.08*u;
PPgetscrposnoadd(m,PPlefteye,&x,&y);
if (left.inimage(x,y)) {
plot=true;
left.opencircle(x,y,rad,15);
}
PPgetscrposnoadd(m,PPrighteye,&x,&y);
if (right.inimage(x,y)) {
plot=true;
right.opencircle(x,y,rad,15*16);
}
}
if (!plot) {
V2d v=scrwid*2*V2d(m.x,m.y).norm();
if (abs(v.x)>scrwid/2) {
float change=(float)scrwid/2.0/abs(v.x);
v=change*v;
}
if (abs(v.y)>scrhei/2) {
float change=(float)scrhei/2.0/abs(v.y);
v=change*v;
}
v=v+V2d(scrwid/2,scrhei/2);
left.opencircle(v,5,15);
right.opencircle(v,5,15*16);
}
// Pull player towards marker
// if (m.mod()>markerrange+.1) {
V3d pulldir=getpos(marker)-pos;
// vel=vel+pulldir*0.02;
// float amount=(V3d::normdot(ori.z(),pulldir)+5.0)/6.0;
float amount=chop(0.99-(m.mod()-markerrange)/5.0,0,1);
V3d newz=ori.z()*amount+(1.0-amount)*pulldir;
ori.forcez(newz);
// vel=vel*amount;
//
//}
// Draw and update tail
V3d last=(tail[tailpos]-pos).disorientate(ori);
for (int k=1;k<taillen;k++) {
int j=mymod(tailpos+k,taillen);
V3d next=(tail[j]-pos).disorientate(ori);
plotline(last,next,(float)k/(float)taillen);
last=next;
}
if ((frame % 5)==0) {
tail[tailpos]=pos-ori.qz;
tailpos=mymod(tailpos+1,taillen);
}
// Or screens
for (int i=0;i<scrwid;i++) {
for (int j=0;j<scrhei;j++) {
b.bmp[j][i]=(left.bmp[j][i] | right.bmp[j][i]);
}
}
b.writetoscreen();
// acc=hang(acc,V3d::origin,0.95,0.01);
// Movement
float angvel=turnability;
if (key[KEY_LCONTROL])
vel=vel+ori.z()*forcevel;
else
angvel=turnability*2.0;
if (key[KEY_UP])
dpitch=dpitch+angvel;
if (key[KEY_DOWN])
dpitch=dpitch-angvel;
if (key[KEY_LEFT])
if (key[KEY_ALT])
droll=droll-angvel;
else
dyaw=dyaw-angvel;
if (key[KEY_RIGHT])
if (key[KEY_ALT])
droll=droll+angvel;
else
dyaw=dyaw+angvel;
vel=hang(vel,V3d::o,0.91,0);
pos=pos+vel;
droll=hang(droll,0,0.9,0);
dyaw=hang(dyaw,0,0.9,0);
dpitch=hang(dpitch,0,0.9,0);
ori.roll(droll/5.0);
ori.yaw(dyaw/5.0);
ori.pitch(dpitch/5.0);
framedone();
} while (!key[KEY_SPACE] && !key[KEY_ESC]);
savetimer();
allegro_exit();
displayframespersecond();
}
void main() {
allegrosetup(scrwid,scrhei);
makepalette(&greypalette);
mypalette(255,0,0,0);
mypalette(0,0,0,0);
mypalette(255,1,1,1);
mypalette(128,0,0,0);
randomise();
for (int i=0;i<=10;i++) {
waves+Wave();
//waves.num(i).display();
}
for (int r=0;r<8;r++) {
for (int g=0;g<8;g++) {
int palc=r+g*8;
mypalette(palc,(r==0?0:0.2+0.5*r/7.0),(g==0?0:0.1+0.2*g/7.0),(g==0?0:0.2+0.6*g/7.0));
// Groovy colours mypalette(palc,(r==0?0:0.1+0.6*r/7.0),(g==0?0:0.1+0.2*g/7.0),(g==0?0:0.1+0.7*g/7.0));
// Good colours: mypalette(palc,(r==0?0:0.3+0.4*r/7.0),(g==0?0:0.1+0.2*g/7.0),(g==0?0:0.3+0.5*g/7.0));
}
}
V3d vel=V3d(0,0,0);
V3d acc=V3d(0,0,0);
Ori ori;
float droll=0;
float dyaw=0;
float dpitch=0;
int frame=0;
List<V3d> particles=List<V3d>(gridsize*gridsize*gridsize);
/* for (int i=1;i<gridsize*gridsize*gridsize;i++) {
particles+gridsize*V3d(floatrnd(-1,1),floatrnd(-1,1),floatrnd(-1,1));
}*/
int ps=1;
float rad=0;
for (float thru=0;thru<1.0;thru+=0.002) {
V3d here=getpos(thru);
V3d forward=getpos(thru+0.00001)-here;
V3d up=V3d::normcross(V3d::crazy,forward);
V3d right=V3d::normcross(forward,up);
for (int i=0;i<ps;i++) {
float t=2*pi*(float)i/(float)ps;
float s=sin(t);
float c=cos(t);
particles+(here+rad*(s*up+c*right));
}
//v.print();
}
starttimer();
float thru=0;
V3d pos=getpos(thru);
do {
thru+=0.001;
// V3d last=V3d(pos.x,pos.y,pos.z);
// V3d pos=getpos(thru);
// V3d next=getpos(thru+0.00001);
// V3d newz=next-pos;
// ori.forcez(newz);
frame++;
// float pd=1.6+1.3*sin(2*pi*frame/1000.0);
float pd=2.0;
PPsetup(scrwid,scrhei,pd);
left.clear(0);
right.clear(0);
// b.fadeby(16);
int ioff=-mydiv(pos.x,gjump);
int joff=-mydiv(pos.y,gjump);
int koff=-mydiv(pos.z,gjump);
ori.quickorisetup();
// Need to find largest axis, and do loops with that one outside
int ks=mysgn(ori.qz.z);
int js=-mysgn(ori.qz.y);
int is=-mysgn(ori.qz.x);
float maxdist=100000;
int maxp;
for (int i=1;i<=particles.len;i++) {
int x,y;
V3d p=particles.num(i);
V3d cen=p-pos+0.0*ori.y;
if (cen.mod()<maxdist) {
maxdist=cen.mod();
maxp=i;
}
cen=V3d::disorientate(cen,ori);
int c=7.0-chop(6.0*(cen.z+gridsize)/(float)(gridsize*2),0,6);
// int c=7.0-chop(6.0*(i/particles.len),0,6);
plotsphere(cen,c);
}
plotline(V3d(-0.1,0,0),V3d(0,0,0.5));
plotline(V3d(0.1,0,0),V3d(0,0,0.5));
plotline(V3d(0,-0.1,0),V3d(0,0,0.5));
plotline(V3d(0,0.1,0),V3d(0,0,0.5));
for (int i=0;i<scrwid;i++) {
for (int j=0;j<scrhei;j++) {
b.bmp[j][i]=(left.bmp[j][i] | right.bmp[j][i]);
}
}
// Simple crosshair
// b.hline(scrwid/2-10,scrhei/2,scrwid/2+10,255);
// b.vline(scrwid/2,scrhei/2-10,scrhei/2+10,255);
b.writetoscreen();
// acc=hang(acc,V3d::origin,0.95,0.01);
if (key[KEY_UP])
dpitch=dpitch+angvel;
if (key[KEY_DOWN])
dpitch=dpitch-angvel;
if (key[KEY_LEFT])
if (key[KEY_ALT])
droll=droll-angvel;
else
dyaw=dyaw-angvel;
if (key[KEY_RIGHT])
if (key[KEY_ALT])
droll=droll+angvel;
else
dyaw=dyaw+angvel;
if (key[KEY_LCONTROL])
vel=vel+ori.z()*forcevel;
V3d pulldir=particles.num(maxp)-pos;
vel=hang(vel,V3d::o,0.98,0)+pulldir*0.02;
// pos=hang(pos,particles.num(maxp),0.9,0)+vel;
pos=pos+vel;
droll=hang(droll,0,0.9,0);
dyaw=hang(dyaw,0,0.9,0);
dpitch=hang(dpitch,0,0.9,0);
ori.roll(droll/5.0);
ori.yaw(dyaw/5.0);
ori.pitch(dpitch/5.0);
V3d at=particles.num(mymod2(maxp+10,1,particles.len+1))-pos;
V3d newz=ori.z()*.99+.01*at;
ori.forcez(newz);
framedone();
} while (!key[KEY_SPACE] && !key[KEY_ESC]);
savetimer();
allegro_exit();
displayframespersecond();
}
void orth_all(int start,int end,int M,int N,double* A,double* P,double* Q,int MlsN,
int* cond,double* tol_orth,int iteration,double last_ave_orth)
{
int i,j,k,w;
double c,ele1,ele2,se,cs,sn,t,tmp;
if(start >= end) return;
for(i=start; i<end; i++)
for(j=i+1; j<=end; j++)
{
c = 0.0;
for(k=0; k<N; k++)
c = c + (*(A+i*N+k))*(*(A+j*N+k));
if(myabs(c) > tol)
*cond = 1; //计算能否跳出循环
else
{
*tol_orth = *tol_orth + myabs(c);
continue;
}
if(iteration==0)
{
*tol_orth = *tol_orth + myabs(c);
continue;
}
if(myabs(c) < last_ave_orth)
{
*tol_orth = *tol_orth + myabs(c);
continue;
}
ele1 = 0.0;
ele2 = 0.0;
for(w=0; w<N; w++)
{
ele1 = ele1 + (*(A+i*N+w))*(*(A+i*N+w));
ele2 = ele2 + (*(A+j*N+w))*(*(A+j*N+w));
}
se = (ele1 - ele2)/(2*c);
t = mysgn(se)/(myabs(se)+sqrt(1+se*se));
cs = 1/sqrt(1+t*t);
sn = cs * t;
//更新AT的第i,j列
for(k=0; k<N; k++)
{
tmp = *(A+i*N+k);
*(A+i*N+k) = cs*tmp + sn*(*(A+j*N+k));
*(A+j*N+k) = -sn*tmp + cs*(*(A+j*N+k));
}
if(MlsN==0)
{
//更新右奇异矩阵Q
for(k=0; k<M; k++)
{
tmp = *(Q+i*M+k);
*(Q+i*M+k) = cs*tmp + sn*(*(Q+j*M+k));
*(Q+j*M+k) = -sn*tmp + cs*(*(Q+j*M+k));
}
}
else
{
//更新右奇异矩阵P
for(k=0; k<M; k++)
{
tmp = *(P+i*M+k);
*(P+i*M+k) = cs*tmp + sn*(*(P+j*M+k));
*(P+j*M+k) = -sn*tmp + cs*(*(P+j*M+k));
}
}
}//end for for
}
void moremap() {
float rx,ry,nrx,nry,px,py;
int f,i,j,k,c,x,y,ix,iy,displayloop;
// Generate some more of the map
for (maploop=1; maploop<scrwid*scrhei/20; maploop++) {
rx=(float)mmx/scrwid*2-1;
ry=(float)(mmy-scrhei/2)/scrwid*2;
/* From QB later
SUB move (x, y)
IF fon%(1) THEN
x = x * var(1): y = y * var(1)
END IF
IF fon%(6) THEN
x = var(1) * SGN(x) * ABS(x) ^ var(6)
y = var(1) * SGN(y) * ABS(y) ^ var(6)
END IF
IF fon%(2) THEN
nx = x * COS(var(2)) + y * SIN(var(2))
ny = -x * SIN(var(2)) + y * COS(var(2))
x = nx
y = ny
END IF
IF fon%(3) THEN
'y = y - .01 * (y - 1)
'y = 1 + .99 * (y - 1)
'y = (y + 1) * .7 - 1
y = y * var(3)
END IF
IF fon%(4) THEN
y = (y - 1) * var(4) + 1
END IF
IF fon%(5) THEN
x = x + var(5) * x
END IF
IF fon%(7) THEN
IF fon%(10) THEN
IF fon%(9) THEN
x = x + var(7) * (-1 + 2 * (p% MOD 2))
ELSE
x = x + var(7) * (-1 + 2 * (p% MOD 50) / 49)
END IF
ELSE
x = x + var(7)
END IF
END IF
IF fon%(8) THEN
IF fon%(10) THEN
IF fon%(9) THEN
y = y + var(8) * (-1 + 2 * (p% MOD 2))
ELSE
y = y + var(8) * (-1 + 2 * (p% MOD 50) / 49)
END IF
ELSE
y = y + var(8)
END IF
END IF
END SUB
*/
if (fon[0]) {
rx = mysgn(rx)/var[7]*mypow(myabs(rx),1/var[0]);
ry = mysgn(ry)/var[7]*mypow(myabs(ry),1/var[0]);
}
if (fon[1]) {
rx = rx / var[1];
ry = ry / var[1];
}
if (fon[2]) {
nrx = rx * cos(var[2]) + ry * sin(var[2]);
nry = -rx * sin(var[2]) + ry * cos(var[2]);
rx = nrx;
ry=nry;
}
if (fon[3]) {
ry = ry - mysgn(ry) * sin(var[6]*pi*myabs(ry)) * var[3];
}
if (fon[4]) {
ry = ((myabs(ry) - 1) / var[4] + 1) * mysgn(ry);
}
if (fon[5]) {
rx = rx - mysgn(rx) * sin(var[6]*pi*myabs(rx)) * var[5];
}
px=(rx+1)/2*scrwid;
py=scrhei/2+(ry)/2*scrwid;
ix=(int)px;
iy=(int)py;
if (ix<0 || ix>=scrwid-1 || iy<0 || iy>=scrhei-1) {
ix=px;
iy=py;
}
amount[mmx][mmy][0][0][makingmap]=((float)ix+1-(float)px)*((float)(iy+1)-(float)py);
amount[mmx][mmy][1][0][makingmap]=((float)px-(float)ix)*((float)(iy+1)-(float)py);
amount[mmx][mmy][0][1][makingmap]=((float)ix+1-(float)px)*((float)py-(float)iy);
amount[mmx][mmy][1][1][makingmap]=((float)px-(float)ix)*((float)py-(float)iy);
pix[mmx][mmy][makingmap]=ix;
piy[mmx][mmy][makingmap]=iy;
if (ix<0 || ix>=scrwid-1 || iy<0 || iy>=scrhei-1) {
pix[mmx][mmy][makingmap]=scrwid/2;
piy[mmx][mmy][makingmap]=scrhei/2;
for (i=0; i<=1; i++) {
for (j=0; j<=1; j++) {
amount[mmx][mmy][i][j][makingmap]=0;
}
}
}
mmx++;
if (mmx>=scrwid) {
mmx=0;
mmy++;
if (mmy>=scrhei) {
mmy=0;
tmpmap=usingmap;
usingmap=makingmap;
makingmap=tmpmap;
for (f=0; f<fs; f++) {
perturb(f);
}
}
}
}
}
void main() {
allegrosetup(scrwid,scrhei);
makepalette(&greypalette);
mypalette(255,0,0,0);
randomise();
V3d pos=V3d(0,0,0);
V3d vel=V3d(0,0,0);
V3d acc=V3d(0,0,0);
Ori ori;
float droll=0;
float dyaw=0;
float roll=0;
float yaw=0;
float pitch=0;
for (int i=1;i<1000;i++) {
acc=hang(acc,V3d::origin,0.92,0.01);
vel=hang(vel,ori.qz/5,0.99,0)+acc;
pos=pos+vel;
droll=hang(droll,0,0.9,0.01);
dyaw=hang(dyaw,0,0.95,0.01);
roll=hang(roll,0,0.95,0)+droll;
yaw=hang(yaw,0,0.92,0)+dyaw;
pitch=hang(pitch,0,0.999,0.01);
pos=pos+ori.qz*4;
ori.roll(roll/5.0);
ori.yaw(yaw/5.0);
pos=pos-ori.z()*4;
}
JBmp b=JBmp(scrwid,scrhei);
int frame=0;
starttimer();
do {
frame++;
float pd=2.0-1.5*cos(2*pi*frame/200.0);
PPsetup(scrwid,scrhei,pd);
b.clear(255);
// b.fadeby(16);
V3d off=V3d(mymod(pos.x,gjump),mymod(pos.y,gjump),mymod(pos.z,gjump));
int ioff=-mydiv(pos.x,gjump);
int joff=-mydiv(pos.y,gjump);
int koff=-mydiv(pos.z,gjump);
ori.quickorisetup();
// Need to find largest axis, and do loops with that one outside
int ks=mysgn(ori.qz.z);
int js=-mysgn(ori.qz.y);
int is=-mysgn(ori.qz.x);
for (int k=ks*gridsize;k!=-ks*gridsize;k=k-ks*gjump)
for (int i=-is*gridsize;i!=is*gridsize;i=i+is*gjump)
for (int j=-js*gridsize;j!=js*gridsize;j=j+js*gjump) {
int x,y;
V3d cen=V3d(i,j,k)-off;
cen=V3d::qorientate(cen,ori);
if (PPgetscrpos(cen,&x,&y)) {
int c=255-ucharchop(255*(cen.z+pd+1)/(gridsize+2));
c=quantiseto(64,c+63);
b.filledcircle(x,y,0.1*PPgetunit(cen),c);
}
}
b.writetoscreen();
b.saveframe();
acc=hang(acc,V3d::origin,0.92,0.01);
vel=hang(vel,ori.qz/5,0.99,0)+acc;
pos=pos+vel;
droll=hang(droll,0,0.9,0.01);
dyaw=hang(dyaw,0,0.95,0.01);
roll=hang(roll,0,0.95,0)+droll;
yaw=hang(yaw,0,0.92,0)+dyaw;
pitch=hang(pitch,0,0.999,0.01);
pos=pos+ori.qz*4;
ori.roll(roll/5.0);
ori.yaw(yaw/5.0);
pos=pos-ori.z()*4;
framedone();
} while (!key[KEY_SPACE] && !key[KEY_ESC]);
savetimer();
allegro_exit();
displayframespersecond();
}
int main(void) {
int f,i,j,k,c,x,y,ix,iy,displayloop;
int usingmap,makingmap,mmx,mmy,tmpmap,maploop;
float rx,ry,nrx,nry,px,py;
RGB rgb;
FILE *fp;
img=(uchar **)calloc(scrhei,sizeof(uchar *));
for (y=0; y<scrhei; y++) {
img[y]=(uchar *)calloc(scrwid,sizeof(uchar));
for (x=0; x<scrwid; x++) {
img[y][x]=myrnd()*255;
}
}
img2=(uchar **)calloc(scrhei,sizeof(uchar *));
for (y=0; y<scrhei; y++) {
img2[y]=(uchar *)calloc(scrwid,sizeof(uchar));
for (x=0; x<scrwid; x++) {
img2[y][x]=myrnd()*255;
}
}
srand((int)time(NULL));
usingmap=0;
makingmap=1;
mmx=0;
mmy=0;
/* Originals from QB
op[0] = 1; damp[0] = .999; force[0] = .005;
op[1] = 1.02; damp[1] = .999; force[1] = .002;
op[2] = 0; damp[2] = .999; force[2] = .002;
op[3] = 1; damp[3] = .999; force[3] = .005;
op[4] = 1; damp[4] = .999; force[4] = .005;
op[5] = 0; damp[5] = .999; force[5] = .002;
*/
// 0 Accelerate
op[0] = 1;
damp[0] = .999;
force[0] = .005;
// 1 Velocity
op[1] = 1.02;
damp[1] = .999;
force[1] = .01;
// 2 Rotation
op[2] = 0;
damp[2] = .999;
force[2] = .05;
// 3 Drip
op[3] = 1;
damp[3] = .999;
force[3] = .03;
// 4 Dribble
op[4] = 1;
damp[4] = .999;
force[4] = .01;
// 5 Slide
op[5] = 0;
damp[5] = .999;
force[5] = .01;
for (f=0; f<fs; f++) {
var[f] = op[f];
fon[f]=1;
}
allegro_init ();
install_keyboard ();
install_timer ();
set_gfx_mode (GFX_AUTODETECT, scrwid, scrhei, 0, 0);
set_pallete (desktop_palette);
_farsetsel(screen->seg);
for (c=0; c<=255; c++) {
rgb.r=saw(0,c);
rgb.g=saw(256/3,c);
rgb.b=saw(2*256/3,c);
set_color(c,&rgb);
}
while(!key[KEY_ESC]) {
// Generate some more of the map
for (maploop=1; maploop<scrwid*scrhei/15; maploop++) {
rx=(float)mmx/scrwid*2-1;
ry=(float)(mmy-scrhei/2)/scrwid*2;
if (fon[1]) {
rx = rx / var[1];
ry = ry / var[1];
}
if (fon[0]) {
rx = mysgn(rx)/var[1]*mypow(myabs(rx),1/var[6]);
ry = mysgn(ry)/var[1]*mypow(myabs(ry),1/var[6]);
}
if (fon[2]) {
nrx = rx * cos(var[2]) + ry * sin(var[2]);
nry = -rx * sin(var[2]) + ry * cos(var[2]);
rx = nrx;
ry=nry;
}
if (fon[3]) {
ry = ry / var[3];
}
if (fon[4]) {
ry = ((myabs(ry) - 1) / var[4] + 1) * mysgn(ry);
}
if (fon[5]) {
rx = rx + var[5] * mysgn(rx);
}
px=(rx+1)/2*scrwid;
py=scrhei/2+(ry)/2*scrwid;
ix=(int)px;
iy=(int)py;
amount[mmx][mmy][0][0][makingmap]=((float)ix+1-(float)px)*((float)(iy+1)-(float)py);
amount[mmx][mmy][1][0][makingmap]=((float)px-(float)ix)*((float)(iy+1)-(float)py);
amount[mmx][mmy][0][1][makingmap]=((float)ix+1-(float)px)*((float)py-(float)iy);
amount[mmx][mmy][1][1][makingmap]=((float)px-(float)ix)*((float)py-(float)iy);
pix[mmx][mmy][makingmap]=ix;
piy[mmx][mmy][makingmap]=iy;
if (ix<0 || ix>=scrwid-1 || iy<0 || iy>=scrhei-1) {
pix[mmx][mmy][makingmap]=scrwid/2;
piy[mmx][mmy][makingmap]=scrhei/2;
for (i=0; i<=1; i++) {
for (j=0; j<=1; j++) {
amount[mmx][mmy][i][j][makingmap]=0;
}
}
}
mmx++;
if (mmx>=scrwid) {
mmx=0;
mmy++;
if (mmy>=scrhei) {
mmy=0;
tmpmap=usingmap;
usingmap=makingmap;
makingmap=tmpmap;
for (f=0; f<fs; f++) {
perturb(f);
}
for (i=0; i<4; i++) {
f = myrnd() * fs;
if (myrnd()<.8) {
if (myrnd()<.5)
fon[f] = 1;
else
fon[f]=0;
}
}
}
}
}
// Animate
for (x=0; x<scrwid; x++) {
for (y=0; y<scrhei; y++) {
c=0;
for (i=0; i<=1; i++) {
for (j=0; j<=1; j++) {
c=c+amount[x][y][i][j][usingmap]*img[piy[x][y][usingmap]+j][pix[x][y][usingmap]+i];
}
}
c--;
img2[y][x]=c;
}
}
/* for (y=0;y<scrhei;y++) {
for (x=0;x<scrwid;x++) {
_farpokeb(screen->seg, (unsigned long)screen->line[y]+x, img2[y][x]);
}
}*/
for (y=0; y<scrhei; y++) {
movedata(_my_ds(), img2[y], screen->seg, bmp_write_line(screen,y), scrwid);
}
for (f=0; f<fs; f++) {
if (fon[f]) {
hline(screen, scrwid/2, f*2, scrwid/2+(var[f] - op[f]) * scrwid * 4, 0);
}
}
imgtmp=img;
img=img2;
img2=imgtmp;
for (i=1; i<=5; i++) {
mycircle(myrnd()*scrwid,myrnd()*scrhei,2+myrnd()*8,myrnd()*255);
}
}
}
void orth_row_e(int i, int j,int M,int N, double* A,double* P,double* Q,int MlsN,int* cond, double* tol_orth,int iteration,double last_ave_orth,double* ele)
{
int k,w;
double c,ele1,ele2,se,cs,sn,t,tmp;
c = 0.0;
for(k=0; k<N; k++)
c = c + (*(A+i*N+k))*(*(A+j*N+k));
if(myabs(c) > tol)
*cond = 1; //计算能否跳出循环
else
{
*tol_orth = *tol_orth + myabs(c);
return;
}
if(iteration==0)
{
*tol_orth = *tol_orth + myabs(c);
return;
}
if(myabs(c) < last_ave_orth)
{
*tol_orth = *tol_orth + myabs(c);
return;
}
ele1 = *(ele+i);
ele2 = *(ele+j);
se = (ele1 - ele2)/(2*c);
t = mysgn(se)/(myabs(se)+sqrt(1+se*se));
cs = 1/sqrt(1+t*t);
sn = cs * t;
//更新ele_i ele_j
*(ele+i) = cs*cs*ele1 + sn*sn*ele2 + 2*cs*sn*c;
*(ele+j) = sn*sn*ele1 + cs*cs*ele2 - 2*cs*sn*c;
//更新AT的第i,j列
for(k=0; k<N; k++)
{
tmp = *(A+i*N+k);
*(A+i*N+k) = cs*tmp + sn*(*(A+j*N+k));
*(A+j*N+k) = -sn*tmp + cs*(*(A+j*N+k));
}
if(MlsN==0)
{
//更新右奇异矩阵Q
for(k=0; k<M; k++)
{
tmp = *(Q+i*M+k);
*(Q+i*M+k) = cs*tmp + sn*(*(Q+j*M+k));
*(Q+j*M+k) = -sn*tmp + cs*(*(Q+j*M+k));
}
}
else
{
//更新右奇异矩阵P
for(k=0; k<M; k++)
{
tmp = *(P+i*M+k);
*(P+i*M+k) = cs*tmp + sn*(*(P+j*M+k));
*(P+j*M+k) = -sn*tmp + cs*(*(P+j*M+k));
}
}
}
int newgesvd(int M,int N,double tol,double *A,double *D,
double *Q,double *P,int *INFO)
{
int i,j,k,w,q;
double a,b,c;
double s,t,se;
double tmp,max;
double cs,sn;
int cond = 0;
double ele1, ele2;
int iteration = 0;
double* orth;
int* link; //匹配
int id;
*INFO = 0;
if(M < 0) *INFO = -1;
else if(N < 0) *INFO = -2;
if(*INFO < 0)
{
printf("error:%d\n",*INFO);
exit(0);
}
omp_set_num_threads(NUM_THREADS);
if(M<=N)
{
assert(N%8==0);
orth = (double*)malloc(N*N*sizeof(double));
link = (int*)malloc(3*N*sizeof(int));
if((orth==NULL)||(link==NULL))
{
printf("can't alloc the space for orth and link\n");
return -2;
}
//初始化P为单位阵
#pragma omp parallel private(id,i)
{
id = omp_get_thread_num();
for(i=id*N/4; i<(id+1)/4; i++)
*(P+i*N+i) = 1.0;
}//end for pragma
//orth = (double*)malloc(N*N*sizeof(double));
//link = (int*)malloc(3*N*sizeof(int));
while(1)
{
cond = 0;
//求出A所有列向量之间内积，存放在orth中
#pragma omp parallel reduction(+:cond) private(id,i,k,j)
{
id = omp_get_thread_num();
for(i=id*N/4; i<(id+1)*N/4; i++)
for(k=i+1; k<(id+3)*N/4,k<N;k++)
{
*(orth+i*N+k) = 0; // orth 为对称阵
for(j=0; j<M; j++)
*(orth+i*N+k) = *(orth+i*N+k) + *(A+j*N+i)*(*(A+j*N+k));
*(orth+k*N+i) = *(orth+i*N+k);
if(*(orth+i*N+k)>is_zero||*(orth+i*N+k)<-is_zero)
cond = 1; //存在不正交的向量
for(j=0; j<3; j++)
*(link+j*N+i) = 0; // 初始化匹配数组
}
}//end for pragma
if(cond==0) break; // 都已经正交
//局部贪心匹配
w = 0;
for(i=0; i<N; i++)
{
max = 0;
if(*(link+i)==0)
{
for(j=0;j<N;j++)
if((i!=j)&&(*(link+j)==0))
if(max<myabs(*(orth+i*N+j)))
{
max = myabs(*(orth+i*N+j));
k = j;
}
*(link+N+w) = i;
*(link+2*N+w) = k;
*(link+i) = 1;
*(link+k) = 1;
w++;
}
}
#pragma omp parallel private(id,i,j,q,k,w,c,ele1,ele2,se,t,cs,sn,tmp)
{
id = omp_get_thread_num();
for(q=id*N/8; q<(id+1)*N/8; q++)
{
i = *(link+N+q);
j = *(link+2*N+q);
assert(i!=j);
//对A的第i,j列做正交化
c = 0.0;
for(k=0; k<M; k++)
c = c + (*(A+k*N+i))*(*(A+k*N+j));
//每次旋转之前判断第i列和第j列的范数，把较大的调在前面，以保证奇异值从大到小排列
ele1 = 0.0;
ele2 = 0.0;
for(w=0; w<M; w++)
{
ele1 = ele1 + (*(A+w*N+i))*(*(A+w*N+i));
ele2 = ele2 + (*(A+w*N+j))*(*(A+w*N+j));
}
if(ele1<ele2) //交换第i,j列
{
double exech;
for(w=0; w<M; w++)
{
exech = *(A+w*N+i);
*(A+w*N+i) = *(A+w*N+j);
*(A+w*N+j) = exech;
}
for(w=0; w<N; w++)
{
exech = *(P+w*N+i);
*(P+w*N+i) = *(P+w*N+j);
*(P+w*N+j) = exech;
}
exech = ele1;
ele1 = ele2;
ele2 = exech;
}
se = (ele1 - ele2)/(2*c);
t = mysgn(se)/(myabs(se)+sqrt(1+se*se));
cs = 1/sqrt(1+t*t);
sn = cs * t;
//更新A的第i,j列
for(k=0; k<M; k++)
{
tmp = *(A+k*N+i);
*(A+k*N+i) = cs*tmp + sn*(*(A+k*N+j));
*(A+k*N+j) = -sn*tmp + cs*(*(A+k*N+j));
}
//更新右奇异矩阵P
for(k=0; k<N; k++)
{
tmp = *(P+k*N+i);
*(P+k*N+i) = cs*tmp + sn*(*(P+k*N+j));
*(P+k*N+j) = -sn*tmp + cs*(*(P+k*N+j));
}
}
}//end for pragma
if((++iteration) > ITERATION) {printf("the iteration is over\n"); break;}
}//end for while
assert(M%4==0);
//用于调试
/*
for(i=0; i<M; i++)
{
for(j=0; j<N; j++)
printf("%f ",*(A+i*N+j));
printf("\n");
}
*/
//计算A每列的F范数，得到奇异值:该奇异值可能为0不是最终奇异值
#pragma omp parallel private(id,i,j)
{
id = omp_get_thread_num();
for(i=id*M/4; i<(id+1)*M/4; i++)
{
*(D+i) = 0.0;
for(j=0; j<M; j++)
*(D+i) = *(D+i) + (*(A+j*N+i))*(*(A+j*N+i));
*(D+i) = sqrt(*(D+i));
}
}//end for pragma
#pragma omp parallel private(id,i,j)
{
id = omp_get_thread_num();
//A的各列为相应奇异值的左奇异变量
for(i=id*M/4; i<(id+1)*M/4; i++)
{
if((*(D+i)>-is_zero)&&(*(D+i)<is_zero))
;
else
for(j=0;j<M;j++)
*(Q+j*M+i) = *(A+j*N+i)/(*(D+i));
}
}// end for pragma
free(orth);
free(link);
}//end if(M<=N)===================================================================================
//=================================================================================================
else
{
//初始化Q为单位阵
for(i=0; i<M; i++)
*(Q+i*M+i) = 1.0;
//主计算过程
while(cond==0)
{
cond = 1;
//对于每个i<j迭代计算
for(i=0; i<M-1; i++)
for(j=i+1; j<M; j++)
{
c = 0.0;
for(k=0; k<N; k++)
c = c + (*(A+i*N+k))*(*(A+j*N+k));
if(myabs(c) > tol) cond = 0; //计算能否跳出循环
else continue;
//每次旋转之前判断第i列和第j列的范数，把较大的调在前面，以保证奇异值从大到小排列
ele1 = 0.0;
ele2 = 0.0;
for(w=0; w<N; w++)
{
ele1 = ele1 + (*(A+i*N+w))*(*(A+i*N+w));
ele2 = ele2 + (*(A+j*N+w))*(*(A+j*N+w));
}
if(ele1<ele2) //交换AT第i,j列
{
double exech;
for(w=0; w<N; w++)
{
exech = *(A+i*N+w);
*(A+i*N+w) = *(A+j*N+w);
*(A+j*N+w) = exech;
}
for(w=0; w<M; w++)
{
exech = *(Q+i*M+w);
*(Q+i*M+w) = *(Q+j*M+w);
*(Q+j*M+w) = exech;
}
exech = ele1;
ele1 = ele2;
ele2 = exech;
}
se = (ele1 - ele2)/(2*c);
t = mysgn(se)/(myabs(se)+sqrt(1+se*se));
cs = 1/sqrt(1+t*t);
sn = cs * t;
//更新AT的第i,j列
for(k=0; k<N; k++)
{
tmp = *(A+i*N+k);
*(A+i*N+k) = cs*tmp + sn*(*(A+j*N+k));
*(A+j*N+k) = -sn*tmp + cs*(*(A+j*N+k));
}
//更新右奇异矩阵Q
for(k=0; k<M; k++)
{
tmp = *(Q+i*M+k);
*(Q+i*M+k) = cs*tmp + sn*(*(Q+j*M+k));
*(Q+j*M+k) = -sn*tmp + cs*(*(Q+j*M+k));
}
}
if((++iteration) > ITERATION) {printf("the iteration is over\n"); break;}
}// end while
//计算A每列的F范数，得到奇异值:该奇异值可能为0不是最终奇异值
for(i=0; i<N; i++)
{
*(D+i) = 0.0;
for(j=0; j<N; j++)
*(D+i) = *(D+i) + (*(A+i*N+j))*(*(A+i*N+j));
*(D+i) = sqrt(*(D+i));
}
//A的各列为相应奇异值的左奇异变量
for(i=0; i<N; i++)
{
if((*(D+i)>-is_zero)&&(*(D+i)<is_zero))
;
else
for(j=0;j<N;j++)
*(P+i*N+j) = *(A+i*N+j)/(*(D+i));
}
}//end else
return 0;
}
int gesvd(int M,int N,real tol,real *A,real *D,
real *Q,real *P,int *INFO)
{
int i,j,k,w;
real a,b,c;
real s,t,se;
real tmp;
real cs,sn;
int cond = 0;
real ele1, ele2;
int iteration = 0;
*INFO = 0;
if(M < 0) *INFO = -1;
else if(N < 0) *INFO = -2;
if(*INFO < 0)
{
printf("error:%d\n",*INFO);
exit(0);
}
if(M<=N)
{
//初始化P为单位阵
for(i=0; i<N; i++)
*(P+i*N+i) = 1.0;
//主计算过程
while(cond==0)
{
cond = 1;
//对于每个i<j迭代计算
for(i=0; i<N-1; i++)
for(j=i+1; j<N; j++)
{
//每次旋转之前判断第i列和第j列的范数，把较大的调在前面，以保证奇异值从大到小排列
ele1 = 0.0;
ele2 = 0.0;
for(w=0; w<M; w++)
{
ele1 = ele1 + (*(A+w*N+i))*(*(A+w*N+i));
ele2 = ele2 + (*(A+w*N+j))*(*(A+w*N+j));
}
if(ele1<ele2) //交换第i,j列
{
real exech;
for(w=0; w<M; w++)
{
exech = *(A+w*N+i);
*(A+w*N+i) = *(A+w*N+j);
*(A+w*N+j) = exech;
}
for(w=0; w<N; w++)
{
exech = *(P+w*N+i);
*(P+w*N+i) = *(P+w*N+j);
*(P+w*N+j) = exech;
}
exech = ele1;
ele1 = ele2;
ele2 = exech;
}
//jacobi旋转
/*
a = 0.0;
b = 0.0;
c = 0.0;
for(k=0; k<M; k++)
a = a + (*(A+k*N+i))*(*(A+k*N+i));
for(k=0; k<M; k++)
b = b + (*(A+k*N+j))*(*(A+k*N+j));
*/
c = 0.0;
for(k=0; k<M; k++)
c = c + (*(A+k*N+i))*(*(A+k*N+j));
a = ele2;
b = ele1;
if(myabs(c) > tol*sqrt(a*b)) cond = 0; //计算能否跳出循环
else continue;
se = (b - a)/(2.0*c);
t = mysgn(se)/(myabs(se)+sqrt(1.0+se*se));
cs = 1.0/sqrt(1.0+t*t);
sn = cs * t;
//更新A的第i,j列
for(k=0; k<M; k++)
{
tmp = *(A+k*N+i);
*(A+k*N+i) = cs*tmp + sn*(*(A+k*N+j));
*(A+k*N+j) = -sn*tmp + cs*(*(A+k*N+j));
}
//更新右奇异矩阵P
for(k=0; k<N; k++)
{
tmp = *(P+k*N+i);
*(P+k*N+i) = cs*tmp + sn*(*(P+k*N+j));
*(P+k*N+j) = -sn*tmp + cs*(*(P+k*N+j));
}
}
if((++iteration) > ITERATION) break;
}// end while
//计算A每列的F范数，得到奇异值:该奇异值可能为0不是最终奇异值
for(i=0; i<N; i++)
{
*(D+i) = 0.0;
for(j=0; j<M; j++)
*(D+i) = *(D+i) + (*(A+j*N+i))*(*(A+j*N+i));
*(D+i) = sqrt(*(D+i));
}
//A的各列为相应奇异值的左奇异变量
for(i=0,k=0; i<M,i<N; i++,k++)
{
if((*(D+i)>-is_zero)&&(*(D+i)<is_zero))
k--;
else
for(j=0;j<M;j++)
*(Q+j*M+k) = *(A+j*N+i)/(*(D+i));
}
}//end if(M<=N)===================================================================================
//=================================================================================================
else
{
//初始化Q为单位阵
for(i=0; i<M; i++)
*(Q+i*M+i) = 1.0;
//主计算过程
while(cond==0)
{
cond = 1;
//对于每个i<j迭代计算
for(i=0; i<M-1; i++)
for(j=i+1; j<M; j++)
{
//每次旋转之前判断第i列和第j列的范数，把较大的调在前面，以保证奇异值从大到小排列
ele1 = 0.0;
ele2 = 0.0;
for(w=0; w<N; w++)
{
ele1 = ele1 + (*(A+i*N+w))*(*(A+i*N+w));
ele2 = ele2 + (*(A+j*N+w))*(*(A+j*N+w));
}
if(ele1<ele2) //交换AT第i,j列
{
real exech;
for(w=0; w<N; w++)
{
exech = *(A+i*N+w);
*(A+i*N+w) = *(A+j*N+w);
*(A+j*N+w) = exech;
}
for(w=0; w<M; w++)
{
exech = *(Q+i*M+w);
*(Q+i*M+w) = *(Q+j*M+w);
*(Q+j*M+w) = exech;
}
exech = ele1;
ele1 = ele2;
ele2 = exech;
}
//jacobi旋转
/*
a = 0.0;
b = 0.0;
c = 0.0;
for(k=0; k<M; k++)
a = a + (*(A+k*N+i))*(*(A+k*N+i));
for(k=0; k<M; k++)
b = b + (*(A+k*N+j))*(*(A+k*N+j));
*/
c = 0.0;
for(k=0; k<N; k++)
c = c + (*(A+i*N+k))*(*(A+j*N+k));
a = ele2;
b = ele1;
if(myabs(c) > tol*sqrt(a*b)) cond = 0; //计算能否跳出循环
else continue;
se = (b - a)/(2.0*c);
t = mysgn(se)/(myabs(se)+sqrt(1.0+se*se));
cs = 1.0/sqrt(1.0+t*t);
sn = cs * t;
//更新AT的第i,j列
for(k=0; k<N; k++)
{
tmp = *(A+i*N+k);
*(A+i*N+k) = cs*tmp + sn*(*(A+j*N+k));
*(A+j*N+k) = -sn*tmp + cs*(*(A+j*N+k));
}
//更新右奇异矩阵Q
for(k=0; k<M; k++)
{
tmp = *(Q+i*M+k);
*(Q+i*M+k) = cs*tmp + sn*(*(Q+j*M+k));
*(Q+j*M+k) = -sn*tmp + cs*(*(Q+j*M+k));
}
}
if((++iteration) > ITERATION) break;
}// end while
//计算A每列的F范数，得到奇异值:该奇异值可能为0不是最终奇异值
for(i=0; i<M; i++)
{
*(D+i) = 0.0;
for(j=0; j<N; j++)
*(D+i) = *(D+i) + (*(A+i*N+j))*(*(A+i*N+j));
*(D+i) = sqrt(*(D+i));
}
//A的各列为相应奇异值的左奇异变量
for(i=0,k=0; i<M,i<N; i++,k++)
{
if((*(D+i)>-is_zero)&&(*(D+i)<is_zero))
k--;
else
for(j=0;j<N;j++)
*(P+k*N+j) = *(A+i*N+j)/(*(D+i));
}
}//end else
return 0;
}
float mysquaresgn(float f) {
return mysgn(f)*f*f;
}
int odd_even_gesvd_optim(int M,int N,double tol,double *A,double *D,
double *Q,double *P,int *INFO)
{
int i,j,k,w,q;
double a,b,c;
double s,t,se;
double tmp;
double cs,sn;
int cond = 0;
double ele1, ele2;
int iteration = 0;
int* index; //索引
int id;
int st_index,en_index;
int exe_count;
int count_odd_even = 0;
double tol_orth = 0;
double last_ave_orth = 0;
*INFO = 0;
if(M < 0) *INFO = -1;
else if(N < 0) *INFO = -2;
if(*INFO < 0)
{
printf("error:%d\n",*INFO);
exit(0);
}
omp_set_num_threads(NUM_THREADS);
if(M<=N)
{
assert(N%8==0);
index = (int*)malloc(N*sizeof(int));
if(index==NULL)
{
printf("can't alloc the space of index\n");
return -2;
}
#pragma omp parallel default(shared) private(id,i)
{
id = omp_get_thread_num();
//初始化P为单位阵
for(i=id*N/4; i<(id+1)*N/4; i++)
{
*(P+i*N+i) = 1.0;
*(index+i) = i;
}
}// end for pragma
while(1)
{
cond = 0;
exe_count = 0;
count_odd_even = 0;
last_ave_orth = 2*tol_orth/(N*(N-1));
tol_orth = 0;
#pragma omp parallel reduction(||:cond) reduction(+:tol_orth) default(shared) private(id,i,j,q,k,w,c,ele1,ele2,se,t,cs,sn,tmp,st_index,en_index)
{
id = omp_get_thread_num();
while(exe_count < N)
{
if(count_odd_even == 0) //处于偶数次
{
st_index = id*N/4;
en_index = (id+1)*N/4;
}
else
{
st_index = id*N/4+1;
en_index = (id+1)*N/4;
}
for(q=st_index; q<en_index; q=q+2)
{
if(q+1>=N) break;
i = *(index+q);
j = *(index+q+1);
*(index+q) = j; // 交换index
*(index+q+1) = i;
//对A的第i,j列做正交化
c = 0.0;
for(k=0; k<M; k++)
c = c + (*(A+k*N+i))*(*(A+k*N+j));
//tol_orth = tol_orth + myabs(c);
if(myabs(c) > tol)
cond = 1; //不正交
else
{
tol_orth = tol_orth + myabs(c);
continue; //正交
}
if(iteration==0)
{
tol_orth = tol_orth + myabs(c);
continue;
}
if(myabs(c) < last_ave_orth)
{
tol_orth = tol_orth + myabs(c);
continue;
}
//每次旋转之前判断第i列和第j列的范数，把较大的调在前面，以保证奇异值从大到小排列
ele1 = 0.0;
ele2 = 0.0;
for(w=0; w<M; w++)
{
ele1 = ele1 + (*(A+w*N+i))*(*(A+w*N+i));
ele2 = ele2 + (*(A+w*N+j))*(*(A+w*N+j));
}
se = (ele1 - ele2)/(2*c);
t = mysgn(se)/(myabs(se)+sqrt(1+se*se));
cs = 1/sqrt(1+t*t);
sn = cs * t;
//更新A的第i,j列
for(k=0; k<M; k++)
{
tmp = *(A+k*N+i);
*(A+k*N+i) = cs*tmp + sn*(*(A+k*N+j));
*(A+k*N+j) = -sn*tmp + cs*(*(A+k*N+j));
}
//更新右奇异矩阵P
for(k=0; k<N; k++)
{
tmp = *(P+k*N+i);
*(P+k*N+i) = cs*tmp + sn*(*(P+k*N+j));
*(P+k*N+j) = -sn*tmp + cs*(*(P+k*N+j));
}
}//end for for
#pragma omp master
{
exe_count++;
if(count_odd_even == 0) count_odd_even = 1;
else count_odd_even = 0;
}
#pragma omp barrier
}//end for while
}//end for pragma
if(cond == 0) break;
if((++iteration) > ITERATION) {printf("the iteration is over\n"); break;}
}//end for while
assert(M%4==0);
//计算A每列的F范数，得到奇异值:该奇异值可能为0不是最终奇异值
#pragma omp parallel default(shared) private(id,i,j)
{
id = omp_get_thread_num();
for(i=id*M/4; i<(id+1)*M/4; i++)
{
*(D+i) = 0.0;
for(j=0; j<M; j++)
*(D+i) = *(D+i) + (*(A+j*N+i))*(*(A+j*N+i));
*(D+i) = sqrt(*(D+i));
}
//A的各列为相应奇异值的左奇异变量
for(i=id*M/4; i<(id+1)*M/4; i++)
{
if((*(D+i)>-is_zero)&&(*(D+i)<is_zero))
;
else
for(j=0;j<M;j++)
*(Q+j*M+i) = *(A+j*N+i)/(*(D+i));
}
}// end for pragma
free(index);
}//end if(M<=N)===================================================================================
//=================================================================================================
else
{
assert(M%8==0);
index = (int*)malloc(M*sizeof(int));
if(index==NULL)
{
printf("can't alloc the space of index\n");
return -2;
}
#pragma omp parallel default(shared) private(id,i)
{
id = omp_get_thread_num();
//初始化Q为单位阵
for(i=id*M/4; i<(id+1)*M/4; i++)
{
*(Q+i*M+i) = 1.0;
*(index+i) = i;
}
}// end for pragma
while(1)
{
cond = 0;
exe_count = 0;
count_odd_even = 0;
last_ave_orth = 2*tol_orth/(M*(M-1));
tol_orth = 0;
#pragma omp parallel reduction(||:cond) reduction(+:tol_orth) default(shared) private(id,i,j,q,k,w,c,ele1,ele2,se,t,cs,sn,tmp,st_index,en_index)
{
id = omp_get_thread_num();
while(exe_count < M)
{
if(count_odd_even == 0) //处于偶数次
{
st_index = id*M/4;
en_index = (id+1)*M/4;
}
else
{
st_index = id*M/4+1;
en_index = (id+1)*M/4;
}
for(q=st_index; q<en_index; q=q+2)
{
if(q+1>=M) break;
i = *(index+q);
j = *(index+q+1);
*(index+q) = j; // 交换index
*(index+q+1) = i;
c = 0.0;
for(k=0; k<N; k++)
c = c + (*(A+i*N+k))*(*(A+j*N+k));
if(myabs(c) > tol)
cond = 1; //计算能否跳出循环
else
{
tol_orth = tol_orth + myabs(c);
continue;
}
if(iteration==0)
{
tol_orth = tol_orth + myabs(c);
continue;
}
if(myabs(c) < last_ave_orth)
{
tol_orth = tol_orth + myabs(c);
continue;
}
//每次旋转之前判断第i列和第j列的范数，把较大的调在前面，以保证奇异值从大到小排列
ele1 = 0.0;
ele2 = 0.0;
for(w=0; w<N; w++)
{
ele1 = ele1 + (*(A+i*N+w))*(*(A+i*N+w));
ele2 = ele2 + (*(A+j*N+w))*(*(A+j*N+w));
}
se = (ele1 - ele2)/(2*c);
t = mysgn(se)/(myabs(se)+sqrt(1+se*se));
cs = 1/sqrt(1+t*t);
sn = cs * t;
//更新AT的第i,j列
for(k=0; k<N; k++)
{
tmp = *(A+i*N+k);
*(A+i*N+k) = cs*tmp + sn*(*(A+j*N+k));
*(A+j*N+k) = -sn*tmp + cs*(*(A+j*N+k));
}
//更新右奇异矩阵Q
for(k=0; k<M; k++)
{
tmp = *(Q+i*M+k);
*(Q+i*M+k) = cs*tmp + sn*(*(Q+j*M+k));
*(Q+j*M+k) = -sn*tmp + cs*(*(Q+j*M+k));
}
}//end for for
#pragma omp master
{
exe_count++;
if(count_odd_even == 0) count_odd_even = 1;
else count_odd_even = 0;
}
#pragma omp barrier
}// end for while
}// end for pragma
if(cond == 0) break;
if((++iteration) > ITERATION) {printf("the iteration is over\n"); break;}
}// end for while
assert(N%4==0);
//计算A每列的F范数，得到奇异值:该奇异值可能为0不是最终奇异值
#pragma omp parallel default(shared) private(id,i,j)
{
id = omp_get_thread_num();
for(i=id*N/4; i<(id+1)*N/4; i++)
{
*(D+i) = 0.0;
for(j=0; j<N; j++)
*(D+i) = *(D+i) + (*(A+i*N+j))*(*(A+i*N+j));
*(D+i) = sqrt(*(D+i));
}
//A的各列为相应奇异值的左奇异变量
for(i=id*N/4; i<(id+1)*N/4; i++)
{
if((*(D+i)>-is_zero)&&(*(D+i)<is_zero))
;
else
for(j=0;j<N;j++)
*(P+i*N+j) = *(A+i*N+j)/(*(D+i));
}
}//end for pragma
free(index);
}//end else
return 0;
}
int main(void) {
int f,i,j,k,c,x,y,ix,iy,displayloop;
int usingmap,makingmap,mmx,mmy,tmpmap,maploop;
float rx,ry,nrx,nry,px,py,thru,ctmp;
RGB rgb;
FILE *fp;
srand((int)time(NULL));
usingmap=0; makingmap=1; mmx=0; mmy=0;
img=(uchar **)calloc(scrhei,sizeof(uchar *));
img2=(uchar **)calloc(scrhei,sizeof(uchar *));
for (y=0;y<scrhei;y++) {
img[y]=(uchar *)calloc(scrwid,sizeof(uchar));
img2[y]=(uchar *)calloc(scrwid,sizeof(uchar));
for (x=0;x<scrwid;x++) {
img[y][x]=255*y/scrhei;
img2[y][x]=myrnd()*255;
if (x<scrwid-1 && y<scrhei-1) {
pix[x][y][usingmap]=x;
piy[x][y][usingmap]=y;
for (i=0;i<=1;i++)
for (j=0;j<=1;j++)
amount[x][y][i][j][usingmap]=(float)1/4;
}
}
}
/* Originals from QB
op[0] = 1; damp[0] = .999; force[0] = .005;
op[1] = 1.02; damp[1] = .999; force[1] = .002;
op[2] = 0; damp[2] = .999; force[2] = .002;
op[3] = 1; damp[3] = .999; force[3] = .005;
op[4] = 1; damp[4] = .999; force[4] = .005;
op[5] = 0; damp[5] = .999; force[5] = .002;
*/
// 0 Accelerate
op[0] = 1; damp[0] = .999; force[0] = .005;
// 1 Velocity
op[1] = 1.02; damp[1] = .999; force[1] = .01;
// 2 Rotation
op[2] = 0; damp[2] = .995; force[2] = .03;
// 3 y splurge
op[3] = 0; damp[3] = .999; force[3] = .01;
// 4 Dribble
op[4] = 1; damp[4] = 0; force[4] = .01;
// 5 x splurge
op[5] = 0; damp[5] = .999; force[5] = .01;
op[6]=2;damp[6]=.9999;force[6]=.01;
op[7]=1;damp[7]=.999;force[7]=.01;
for (f=0;f<fs;f++) {
var[f] = op[f];
fon[f]=1;
}
allegrosetup(scrwid,scrhei);
_farsetsel(screen->seg);
starttimer();
while(!key[KEY_ESC]) {
// Generate some more of the map
for (maploop=1;maploop<scrwid*scrhei/20;maploop++) {
rx=(float)mmx/scrwid*2-1;
ry=(float)(mmy-scrhei/2)/scrwid*2;
if (fon[0]) {
rx = mysgn(rx)/var[7]*mypow(myabs(rx),1/var[0]);
ry = mysgn(ry)/var[7]*mypow(myabs(ry),1/var[0]);
}
if (fon[1]) {
rx = rx / var[1]; ry = ry / var[1];
}
if (fon[2]) {
nrx = rx * cos(var[2]) + ry * sin(var[2]);
nry = -rx * sin(var[2]) + ry * cos(var[2]);
rx = nrx; ry=nry;
}
if (fon[3]) {
ry = ry - mysgn(ry) * sin(var[6]*pi*myabs(ry)) * var[3];
}
if (fon[4]) {
ry = ((myabs(ry) - 1) / var[4] + 1) * mysgn(ry);
}
if (fon[5]) {
rx = rx - mysgn(rx) * sin(var[6]*pi*myabs(rx)) * var[5];
}
px=(rx+1)/2*scrwid;
py=scrhei/2+(ry)/2*scrwid;
ix=(int)px;
iy=(int)py;
if (ix<0 || ix>=scrwid-1 || iy<0 || iy>=scrhei-1) {
ix=px; iy=py;
}
amount[mmx][mmy][0][0][makingmap]=((float)ix+1-(float)px)*((float)(iy+1)-(float)py);
amount[mmx][mmy][1][0][makingmap]=((float)px-(float)ix)*((float)(iy+1)-(float)py);
amount[mmx][mmy][0][1][makingmap]=((float)ix+1-(float)px)*((float)py-(float)iy);
amount[mmx][mmy][1][1][makingmap]=((float)px-(float)ix)*((float)py-(float)iy);
pix[mmx][mmy][makingmap]=ix;
piy[mmx][mmy][makingmap]=iy;
if (ix<0 || ix>=scrwid-1 || iy<0 || iy>=scrhei-1) {
pix[mmx][mmy][makingmap]=scrwid/2;
piy[mmx][mmy][makingmap]=scrhei/2;
for (i=0;i<=1;i++) {
for (j=0;j<=1;j++) {
amount[mmx][mmy][i][j][makingmap]=0;
}
}
}
mmx++;
if (mmx>=scrwid) {
mmx=0;
mmy++;
if (mmy>=scrhei) {
mmy=0;
tmpmap=usingmap;
usingmap=makingmap;
makingmap=tmpmap;
for (f=0;f<fs;f++) {
perturb(f);
}
}
}
}
// Animate
for (x=0; x<scrwid; x++) {
for (y=0; y<scrhei; y++) {
c=0;
for (i=0;i<=1;i++) {
for (j=0;j<=1;j++) {
c=c+amount[x][y][i][j][usingmap]*img[piy[x][y][usingmap]+j][pix[x][y][usingmap]+i];
}
}
c--;
img2[y][x]=c;
}
}
/* for (y=0;y<scrhei;y++) {
for (x=0;x<scrwid;x++) {
_farpokeb(screen->seg, (unsigned long)screen->line[y]+x, img2[y][x]);
}
}*/
for (y=0; y<scrhei; y++) {
movedata(_my_ds(), img2[y], screen->seg, bmp_write_line(screen,y), scrwid);
}
for (f=0;f<fs;f++) {
if (fon[f]) {
hline(screen, scrwid/2, f*2, scrwid/2+(var[f] - op[f]) * scrwid * 4, 0);
}
}
toff=toff-(float)1/128;
for (c=0;c<=255;c++) {
thru=saw((float)c/255-toff);
rgb.r=huefor(thru,(float)0);
rgb.g=huefor(thru,(float)1/3);
rgb.b=huefor(thru,(float)2/3);
set_color(c,&rgb);
}
imgtmp=img;
img=img2;
img2=imgtmp;
for (i=1;i<=5;i++) {
mycircle(myrnd()*scrwid,myrnd()*scrhei,2+myrnd()*8,myrnd()*255);
}
framedone();
}
allegro_exit();
displayframespersecond();
}
