static bool insert(const struct avl *t, void **pp, void **px)
{
if (*pp == NULL)
return insert_here(t, pp, px);
int cmp = t->cmp(*px, *pp);
if (cmp == 0)
return insert_here(t, pp, px);
struct avl_node *p = node(t, *pp);
bool dir = cmp > 0;
int idir = dir ? 1 : -1;
bool grow = insert(t, &p->child[dir], px);
p->balance += idir * grow;
if (!grow || p->balance == 0)
return false;
if (p->balance == idir)
return true;
if (node(t, p->child[dir])->balance == idir)
rotate(t, pp, dir);
else
rotate2(t, pp, dir);
return false;
}
int main( int argc, char* argv[] ) {
const int N = 100;
std::vector<int> a(N), b(N);
std::vector<int> offsets(3);
offsets[0] = 3;
offsets[1] = 94;
offsets[2] = 50;
{
for (int j = 0; j < (int)offsets.size(); ++j) {
for (int i = 0; i < N; ++i) {
a[i] = i;
b[i] = (i + offsets[j]) % N;
}
rotate1(a, offsets[j]);
eq(j, a, b);
}
}
{
for (int j = 0; j < (int)offsets.size(); ++j) {
for (int i = 0; i < N; ++i) {
a[i] = i;
b[i] = (i + offsets[j]) % N;
}
rotate2(a, offsets[j]);
eq(j, a, b);
}
}
}
int main()
{
int n=13;
int nums[13]={1,2,3,4,5,6,7,8,9,10,11,12,13};
int k=17;
rotate2(nums,n,k);
for(int i=0;i<n;i++)
std::cout<<nums[i]<<std::endl;
return 0;
}
// Ini juga cuma buat ngeliat dari sudut yang lain
void mouse(int button, int state, int x, int y){
if(button == GLUT_LEFT_BUTTON && state==GLUT_DOWN){
rotate1(-10.0);
}
else if(button == GLUT_RIGHT_BUTTON && state==GLUT_DOWN){
rotate2(-10.0);
}
glFlush();
glutPostRedisplay();
}
static inline bool remove_fix(struct avl *t, void **pp, bool dir, bool shrink)
{
struct avl_node *p = node(t, *pp);
int idir = dir ? 1 : -1;
p->balance -= idir * shrink;
if (!shrink || p->balance == -idir)
return false;
if (p->balance == 0)
return true;
if (node(t, p->child[!dir])->balance == idir) {
rotate2(t, pp, !dir);
return true;
} else {
rotate(t, pp, !dir);
return node(t, *pp)->balance == 0;
}
}
int main(){
char str[13];
while(gets(str)){
char c1[7],c2[7];
bool found=false;
strncpy(c1,str,6);
strncpy(c2,str+6,6);
c1[6]=c2[6]='\0';
int i,j,k;
for(i=1;i<=4&&!found;i++){
for(j=1;j<=4&&!found;j++){
for(k=1;k<=4&&!found;k++){
if(strcmp(c1,c2)==0)
found=true;
rotate1(c1);}
rotate2(c1);}
rotate3(c1);}
if(found)
puts("TRUE");
else
puts("FALSE");
}
return 0;
}
void NPC::idle() {
int temp;
double i1,i2;
unit::idle();
airplane::idle();
accelate(true);
coordinate mc;
if(locked==true) {
mc.x=mi->center.x;
mc.y=mi->center.y;
mc.z=mi->center.z;
mc.x-=center.x;
mc.y-=center.y;
mc.z-=center.z;
mc.unital();
if((mc.x*head.x+mc.y*head.y+mc.z*head.z)!=0) {
if((mc.x*head.x+mc.y*head.y+mc.z*head.z)>0.95 || (mc.x*head.x+mc.y*head.y+mc.z*head.z)<-0.95) {
rotate2(true);
}
else {
i1=top.x*mc.x+top.y*mc.y+top.z*mc.z;
i2=right.x*mc.x+right.y*mc.y+right.z*mc.z;
if(i1<0)
i1*=-1;
if(i2<0)
i2*=-1;
if(i1>i2) {
i1=top.x*mc.x+top.y*mc.y+top.z*mc.z;
if(i1>0) {
rotate2(false);
}
else {
rotate2(true);
}
}
else {
i2=right.x*mc.x+right.y*mc.y+right.z*mc.z;
if(i2>0) {
rotate1(true);
}
else {
rotate1(false);
}
}
}
}
}
else {
}
temp=rand();
/* if(temp%100==0)
rotate1(true);
else if(temp%100==1)
rotate2(true);
else if(temp%100==2)
rotate1(false);
else if(temp%100==3)
rotate2(false);
*/ move();
fule=1000;
if(fule<=0) {
stop();
}
}
void missile::idle() {
double t1,temp,d1,d2;
coordinate tar,ts;
unit *st;
unit::idle();
if(fired && liv==lif && target->getfloor()!=lif) {
set->sub(this);
cerr<<"target dead"<<endl;
return;
}
if(fired && liv==lif && target->getfloor()==lif) {
st=set->find(center);
target->setlock(true,this);
if(st->getfloor()==lif
&& st!=target
&& ::distance(center,master->center)>1
&& ::distance(center,st->center)<0.1) {
st->hited(damage);
set->sub(this);
target->setlock(false,this);
cerr<<"crashed"<<endl;
return;
}
tar.x=target->getcoordinate().x;
tar.y=target->getcoordinate().y;
tar.z=target->getcoordinate().z;
if(target->getfloor()==lif && ::distance(tar,center)<0.1) {
target->hited(damage);
master->hit();
cerr<<"hit"<<endl;
set->sub(this);
target->setlock(false,this);
return;
}
else {
tar.x-=center.x;
tar.y-=center.y;
tar.z-=center.z;
tar.unital();
t1=tar.x*head.x+tar.y*head.y+tar.z*head.z;
if(t1>=range && ::distance(tar,center)<100000) {
//좌,우 어느쪽으로 방향을 틀지 정함.
//tar을 head-right평면에 정사영;
d1=tar.x*top.x+tar.y*top.y+tar.z*top.z;
ts.x=top.x*d1;
ts.y=top.y*d1;
ts.z=top.z*d1;
ts.x=tar.x-ts.x;
ts.y=tar.y-ts.y;
ts.z=tar.z-ts.z;
//ts와head사이의 각도
ts.unital();
d2=ts.x*head.x+ts.y*head.y+ts.z*head.z;
if(d2>=1.0)
d2=1.0;
temp=d2;
temp=acos(temp);
if(temp>maxturnrate) {
temp=maxturnrate;
}
flyingUnit::sett(temp);
d2=tar.x*right.x+tar.y*right.y+tar.z*right.z;
if(d2>=0)
rotate1(true);
else
rotate1(false);
//위, 아래 어느쪽으로 방향을 틀지 정함.
temp=tar.x*head.x+tar.y*head.y+tar.x*head.z;
if(temp>=1.0)
temp=1.0;
temp=acos(temp);
if(temp>maxturnrate) {
temp=maxturnrate;
}
flyingUnit::sett(temp);
d2=tar.x*top.x+tar.y*top.y+tar.z*top.z;
if(d2>=0)
rotate2(false);
else
rotate2(true);
}
else {
set->sub(this);
target->setlock(false,this);
cerr<<"target lost"<<endl;
return;
}
move();
if(fule<=0) {
}
}
}
}
/*************************************
fiddle()
**************************************/
int fiddle(int argc, char *argv[], int retc, char *retv[])
{
int pwr,cblock,res,dc_correct=TRUE;
register int i,ntval;
dpointers inblock;
float a,b,c,d,denom;
int ocount;
/* initialization bits */
if (i_fiddle(argc,argv))
ABORT;
dc_correct=dccorr;
pwr = fnpower(fn0);
max=0.0;
cfcount=0;
count=0;
firstrefint=0.0;
phasetweek=0.0;
degtorad=3.141592654/180.0;
ocount=0;
ntval = 1;
if (!P_getreal(PROCESSED, "nt", &tmp, 1))
{
ntval = (int) (tmp + 0.5);
if (ntval < 1)
ntval = 1;
}
disp_status("IN3 ");
/* check range of transforms */
if (startno>=fidhead.nblocks) startno=fidhead.nblocks-1;
if (startno<0) {
startno=0;
finishno=fidhead.nblocks;
stepno=1;
}
if (finishno>fidhead.nblocks) finishno=fidhead.nblocks;
if (stepno==0) stepno=1;
/* setup destination fidfile and/or correction function file if requested
*/
if (writefg) setupwritefile();
if (writecfflg) setupwritecf();
/* start of main loop */
incno=0;
t1=0;
rp0=rp;
for (cblock = startno; cblock < finishno; cblock+=stepno)
{
if ( (res = D_getbuf(D_DATAFILE, fidhead.nblocks, cblock, &inblock)) )
{
D_error(res);
freall(TRUE);
}
if ( (inblock.head->status & (S_DATA|S_SPEC|S_FLOAT|S_COMPLEX)) ==
(S_DATA|S_SPEC|S_FLOAT|S_COMPLEX) )
{
disp_index(cblock+1);
incno+=1;
if (verbose) Wscrprintf("Increment no. %d \n",incno);
if (!aphflg) incrementrp();
inp = (float *)inblock.data;
/* DEBUGGING ONLY
if (verbose) Wscrprintf("Real part of 1st point of original fid
%f \n",
inp[0]);
if (verbose) Wscrprintf("Imag part of 1st point of original fid
%f \n",
inp[1]);
*/
/* dc (not normally used!)
if (dc_correct)
{
disp_status("DC ");
cmplx_dc(inp, &lvl_re, &lvl_im, &tlt_re, &tlt_im, np0/2,
CMPLX_DC);
vvrramp(inp, 2, inp, 2, lvl_re, tlt_re, np0/2);
vvrramp(inp+1, 2, inp+1, 2, lvl_im, tlt_im, np0/2);
} */
/* phase correct spectrum */
finalph=0.0;
rotate2(inp,np0/2,lp,rp);
if (aphflg)
{
faph(inp,leftpos,rightpos,&finalph);
rotate2(inp,np0/2,0.0,finalph);
}
/* if baseline then zero imag. */
if (baseline||hilbert) fiddle_zeroimag();
/* move first to data1, ready for ift, n.b. fn0==np0 */
transpmove(inp,data1);
/* do zeroing for reference region */
if (solvent) solventextract();
else
{
for (i=0;i<leftpos;i++)
{
inp[i]=0.0;
}
for (i=rightpos+2;i<np0;i++)
{
inp[i]=0.0;
}
if (ldcflag) baseline_correct(inp,leftpos,rightpos);
if (extrap) extrapolate();
} /* !solvent */
transpmove(inp,data2);
/* now do the ift's */
if (!noift)
{
disp_status("IFT1 ");
fft(data1,fn0/2,pwr,0,COMPLEX,COMPLEX,1.0,FTNORM/ntval,np0/2);
disp_status("IFT2 ");
fft(data2,fn0/2,pwr,0,COMPLEX,COMPLEX,1.0,FTNORM/ntval,np0/2);
}
if (makereffg) makeideal();
/* need to weight data3 to create ideal fid */
if (makereffg&&!noift)
{
disp_status("WT ");
weightfid(wtfunc,data3,np0w/2,FALSE,COMPLEX);
}
if (stopflag<1||stopflag>3)
{
/* DEBUGGING ONLY
if (verbose) Wscrprintf("Real part of 1st point of
original fid %f \n",
data1[0]);
if (verbose) Wscrprintf("Imag part of 1st point of
original fid %f \n",
data1[1]);
*/
/* divide (3) by (2) */
disp_status("DIV ");
for (i=0;i<npi;i+=2)
{
a=data3[i];
b=data3[i+1];
c=data2[i];
d=data2[i+1];
denom=c*c+d*d;
data2[i]=(a*c+b*d)/denom;
data2[i+1]=(b*c-a*d)/denom;
}
if (writecfflg) writeoutcf();
if (readcfflg) readincf();
/* and multiply by (1) */
/* DEBUGGING ONLY
if (verbose) Wscrprintf("Real part of 1st point of
correction function %f \n",
data2[0]);
if (verbose) Wscrprintf("Imag part of 1st point of
correction function %f \n",
data2[1]);
*/
disp_status("MUL ");
if (!corfunc)
{
for (i=0;i<npi;i+=2)
{
a=data2[i];
b=data2[i+1];
c=data1[i];
d=data1[i+1];
inp[i]=(a*c-b*d);
inp[i+1]=(b*c+a*d);
}
}
/* DEBUGGING ONLY
if (verbose) Wscrprintf("Real part of 1st point of
corrected fid %f \n",
inp[0]);
if (verbose) Wscrprintf("Imag part of 1st point of
corrected fid %f \n",
inp[1]);
*/
/* Halve first point of corrected fid */
/* hang about, does this do anything??? */
if (halffg) /* default true */
{
data1[0]=0.5*(data1[0]);
data1[1]=0.5*(data1[1]);
}
if (npi<np0)
for (i=npi;i<np0;i++)
inp[i]=0.0;
} /* stopflag not 1 - 3 */
if (firstfg&&!nosub)
movmem((char *)inp,(char *)data4,sizeof(float)*np0,1,4);
if (secondfg&&!nosub)
{
disp_status("SUB ");
submem(inp,data4,np0);
if (invert) invertmem(inp,np0);
}
/* inp contains result fid - write out and/or FT! */
if (writefg&&!firstfg) writeoutresult();
if (!noftflag)
{
disp_status("FT ");
fft(inp,fn0/2,pwr,0,COMPLEX,COMPLEX,-1.0,FTNORM/ct,np0/2);
}
/* move intermediate result for display if requested */
if (stopflag||corfunc)
{
disp_status("MOVE ");
switch (stopflag)
{
case 1:
p=data1;
break;
case 2:
p=data2;
break;
case 4:
p=data4;
break;
default:
p=data3;
break;
}
if (corfunc) p=data2;
if (difffg&&(stopflag<4))
{
if (firstfg) movmem((char *)p,(char
*)data4,sizeof(float)*np0,1,4);
if (!firstfg)
{
submem(p,data4,np0);
if (invert) invertmem(p,np0);
}
}
if (noift)
transpmove(p,inp);
else
movmem((char *)p,(char *)inp,sizeof(float)*np0,1,4);
}
/* re-phase back to rp,lp */
disp_status("PHASE ");
if (!(writefg&&!firstfg)) /* if not both writing and subsequent
fid */
{
rotate2(inp,np0/2,-lp,-rp);
}
else rotate2(inp,np0/2,-lp,0.0);
if (difffg)
{
secondfg=!secondfg;
firstfg=!firstfg;
}
if (udifffg)
{
secondfg=TRUE;
firstfg=FALSE;
}
if (secondfg||oflag)
makereffg=FALSE;
else makereffg=TRUE;
/* oflag increment */
if (oflag)
{
rp+=odat[ocount];
ocount++;
if (ocount>3) ocount=0;
}
/* release result */
if ( (res=D_markupdated(D_DATAFILE,cblock)) )
{
D_error(res);
disp_status(" ");
freall(TRUE);
}
if ( (res = D_release(D_DATAFILE, cblock)) )
{
D_error(res);
disp_status(" ");
freall(TRUE);
}
if (interuption) /* ? not fully working ? */
{
Werrprintf("Fiddle processing halted");
freall(TRUE);
ABORT;
}
if (flag2d)
{
if (altfg)
{
if ((incno % 2)==0) t1=t1+1/sw1;
}
else
{
t1=t1+1/sw1;
}
}
} /* end of if ( (inblock.head->status &c at start of main loop */
} /* end of main loop */
start_from_ft=TRUE;
releasevarlist();
appendvarlist("cr");
Wsetgraphicsdisplay("ds");
/* free memory */
freall(FALSE);
disp_status(" ");
disp_index(0);
RETURN;
}
void MeshEnergy::GetNormalsTangentPlane( const std::vector<int>& C, const std::vector<double>& phi,
std::valarray<double>& ne1, std::valarray<double>& ne2,
MeshData* vtkNotUsed(meshdata))
{
vtkPoints* verts = meshdata->polydata->GetPoints();
for( ::size_t k_ = 0; k_ < C.size(); k_++ )
{
int k = C[k_];
std::vector<double> nhat(3);
nhat[0] = meshdata->nx[k]; // these are normal to surface; the nx_ are the contour normals
nhat[1] = meshdata->ny[k];
nhat[2] = meshdata->nz[k];
// step 1. create the rotation matrix that orients the current normal as [0,0,1]'.
double phiang = atan2( nhat[0], nhat[1] );
std::vector<double> rotate1(9);
rotate1[0] = cos(phiang); rotate1[1] = -sin(phiang); rotate1[2] = 0;
rotate1[3] = sin(phiang); rotate1[4] = cos(phiang); rotate1[5] = 0;
rotate1[6] = 0; rotate1[7] = 0; rotate1[8] = 1.0;
std::vector<double> nhat_a(3);
pkmult( nhat, rotate1, nhat_a );
double ytilde = nhat_a[1];
double theta = M_PI_2 - atan2(nhat[2],ytilde);
std::vector<double> rotate2(9);
rotate2[0] = 1.0; rotate2[1] = 0; rotate2[2] = 0;
rotate2[3] = 0; rotate2[4] = cos(theta); rotate2[5] = -sin(theta);
rotate2[6] = 0; rotate2[7] = sin(theta); rotate2[8] = cos(theta);
std::vector<double> nhat_b(3);
pkmult( nhat_a, rotate2, nhat_b );
// nhat_b should now be [0 0 1]'
double thispt[3];
verts->GetPoint( k, thispt );
// apply rotate2 * rotate1 to each *translated* neighbor of this k-th point
::size_t num_neigh = meshdata->adj[k].myNeighbs.size();
double vec[3];
std::vector<double> vv(3);
std::vector<double> vv_(3);
std::valarray<double> xdata(num_neigh);
std::valarray<double> ydata(num_neigh);
std::valarray<double> zdata(num_neigh);
// step 2. create temporary set of std::vectors as copies of neighboring points
// translated to origin
// step 3. apply the rotation to all these points
for ( ::size_t i = 0; i < num_neigh; i++ )
{
int idx = meshdata->adj[k].myNeighbs[i];
verts->GetPoint( idx, vec );
vv[0] = vec[0] - thispt[0];
vv[1] = vec[1] - thispt[1];
vv[2] = vec[2] - thispt[2];
pkmult( vv, rotate1, vv_ );
pkmult( vv_, rotate2, vv );
xdata[i] = vv[0];
ydata[i] = vv[1];
zdata[i] = phi[idx] - phi[k]; //vv[2];
// zero reference phi at the vertex where we are forming tangent plane
}
/*if( abs(zdata).min() < 1e-6 )
continue;*/
// step 4. find least-squares fit for H(x,y) = ax + by
std::valarray<double> RHS(2);
std::valarray<double> ATA(4);
ATA[0] = (xdata * xdata).sum();
ATA[1] = (xdata * ydata).sum();
ATA[2] = ATA[1];
ATA[3] = (ydata * ydata).sum();
RHS[0] = (xdata * zdata).sum();
RHS[1] = (ydata * zdata).sum();
int maxits = 1000;
std::valarray<double> ab = RHS; // initial guess
std::valarray<double> LHS(2);
pkmult2( ab, ATA, LHS );
double res = sqrt( ( (LHS - RHS)*(LHS - RHS) ).sum() );
double tol = 1e-8;
int iter = 0;
while( iter < maxits && res > tol )
{
iter++;
ab[0] = (RHS[0] - ( ab[1]*ATA[1] ) )/ ATA[0];
ab[1] = (RHS[1] - ( ab[0]*ATA[2] ) )/ ATA[3];
pkmult2( ab, ATA, LHS );
res = sqrt( ( (LHS - RHS)*(LHS - RHS) ).sum() );
}
ne1[k_] = ab[0] / sqrt( (ab*ab).sum() );
ne2[k_] = ab[1] / sqrt( (ab*ab).sum() );
// step 5. differentiate the plane along principal directions
}
}
void MeshEnergy::GetKappa( const std::vector<int>& C, const std::vector<double>& phi,
std::valarray<double>& kappa)
{
// kappa: divergence of normal
// dy^2 * dxx - 2dxdydxy + dx^2dyy / ( dx^2 + dy^2 )^(3/2)
vtkPoints* verts = meshdata->polydata->GetPoints();
for( ::size_t k_ = 0; k_ < C.size(); k_++ )\
{
int k = C[k_];
std::vector<double> nhat(3);
nhat[0] = meshdata->nx[k]; // these are normal to surface; the nx_ are the contour normals
nhat[1] = meshdata->ny[k];
nhat[2] = meshdata->nz[k];
// step 1. create the rotation matrix that orients the current normal as [0,0,1]'.
double phiang = atan2( nhat[0], nhat[1] );
std::vector<double> rotate1(9);
rotate1[0] = cos(phiang); rotate1[1] = -sin(phiang); rotate1[2] = 0;
rotate1[3] = sin(phiang); rotate1[4] = cos(phiang); rotate1[5] = 0;
rotate1[6] = 0; rotate1[7] = 0; rotate1[8] = 1.0;
std::vector<double> nhat_a(3);
pkmult( nhat, rotate1, nhat_a );
double ytilde = nhat_a[1];
double theta = M_PI_2 - atan2(nhat[2],ytilde);
std::vector<double> rotate2(9);
rotate2[0] = 1.0; rotate2[1] = 0; rotate2[2] = 0;
rotate2[3] = 0; rotate2[4] = cos(theta); rotate2[5] = -sin(theta);
rotate2[6] = 0; rotate2[7] = sin(theta); rotate2[8] = cos(theta);
std::vector<double> nhat_b(3);
pkmult( nhat_a, rotate2, nhat_b );
// nhat_b should now be [0 0 1]'
double thispt[3];
verts->GetPoint( k, thispt );
// apply rotate2 * rotate1 to each *translated* neighbor of this k-th point
::size_t num_neigh = meshdata->adj[k].myNeighbs.size();
double vec[3];
std::vector<double> vv(3);
std::vector<double> vv_(3);
std::valarray<double> xdata(num_neigh);
std::valarray<double> ydata(num_neigh);
std::valarray<double> zdata(num_neigh);
// step 2. create temporary set of std::vectors as copies of neighboring points
// translated to origin
// step 3. apply the rotation to all these points
for (::size_t i = 0; i < num_neigh; i++ )
{
int idx = meshdata->adj[k].myNeighbs[i];
verts->GetPoint( idx, vec );
vv[0] = vec[0] - thispt[0];
vv[1] = vec[1] - thispt[1];
vv[2] = vec[2] - thispt[2];
pkmult( vv, rotate1, vv_ );
pkmult( vv_, rotate2, vv );
xdata[i] = vv[0];
ydata[i] = vv[1];
zdata[i] = phi[idx] - phi[k]; //vv[2];
// zero reference phi at the vertex where we are forming tangent plane
}
/*if( abs(zdata).min() < 1e-6 )
continue;*/
// step 4. find first derivatives
double phi_x = 0.0;
double phi_y = 0.0;
{
std::valarray<double> RHS(2);
std::valarray<double> ATA(4);
ATA[0] = (xdata * xdata).sum();
ATA[1] = (xdata * ydata).sum();
ATA[2] = ATA[1];
ATA[3] = (ydata * ydata).sum();
RHS[0] = (xdata * zdata).sum();
RHS[1] = (ydata * zdata).sum();
int maxits = 1000;
std::valarray<double> ab = RHS; // initial guess
std::valarray<double> LHS(2);
pkmult2( ab, ATA, LHS );
double res = sqrt( ( (LHS - RHS)*(LHS - RHS) ).sum() );
double tol = 1e-8;
int iter = 0;
while( iter < maxits && res > tol )
{
iter++;
ab[0] = (RHS[0] - ( ab[1]*ATA[1] ) )/ ATA[0];
ab[1] = (RHS[1] - ( ab[0]*ATA[2] ) )/ ATA[3];
pkmult2( ab, ATA, LHS );
res = sqrt( ( (LHS - RHS)*(LHS - RHS) ).sum() );
}
phi_x = ab[0];
phi_y = ab[1];
}
// step 4. find least-squares fit for phi(x,y) = ax^2 + bxy + cy^2
// to get second derivatives
std::valarray<double> RHS(3); // A'z
RHS[0] = ( xdata * xdata * zdata ).sum();
RHS[1] = ( xdata * ydata * zdata ).sum();
RHS[2] = ( ydata * ydata * zdata ).sum();
double tik_delta = 1e-1 * abs(RHS).min();
std::vector<double> ATA(9); // A'A
ATA[0] = tik_delta + (xdata * xdata * xdata * xdata).sum();
ATA[1] = (xdata * xdata * xdata * ydata).sum();
ATA[2] = (xdata * xdata * ydata * ydata).sum();
ATA[3] = (xdata * ydata * xdata * xdata).sum();
ATA[4] = tik_delta + (xdata * ydata * xdata * ydata).sum();
ATA[5] = (xdata * ydata * ydata * ydata).sum();
ATA[6] = (ydata * ydata * xdata * xdata).sum();
ATA[7] = (ydata * ydata * xdata * ydata).sum();
ATA[8] = tik_delta + (ydata * ydata * ydata * ydata).sum();
int maxits = 1000;
std::valarray<double> abc = RHS; // initial guess
std::valarray<double> LHS(3);
pkmult( abc, ATA, LHS );
double res = sqrt( ( (LHS - RHS)*(LHS - RHS) ).sum() );
double tol = 1e-8;
int iter = 0;
while( iter < maxits && res > tol )
{
iter++;
abc[0] = (RHS[0] - ( abc[1]*ATA[1] + abc[2]*ATA[2] ) )/ ATA[0];
abc[1] = (RHS[1] - ( abc[0]*ATA[3] + abc[2]*ATA[5] ) )/ ATA[4];
abc[2] = (RHS[2] - ( abc[0]*ATA[6] + abc[1]*ATA[7] ) )/ ATA[8];
pkmult( abc, ATA, LHS );
res = sqrt( ( (LHS - RHS)*(LHS - RHS) ).sum() );
}
// step 5. get the derivatives from quadratic form
double phi_xx = 2*abc[0];
double phi_xy = abc[1];
double phi_yy = 2*abc[2];
kappa[k_] = phi_y * phi_y * phi_xx - 2 * phi_x * phi_y * phi_xy + phi_x * phi_x * phi_yy;
if( abs(phi_x) + abs(phi_y) > 1e-9 )
{
kappa[k_] /= pow( (phi_x*phi_x + phi_y*phi_y), 1.5 );
}
}
}
//METHOD
void draw(){
if(type == 0){//BOX
if(y<=10){
Matrix3x3 ms;//SCALE
Matrix3x3 mt;//TRANSLASI
//TITIK PENGGAMBARAN
int drawx1;//X1 SETELAH DITRANSFORMASI
int drawy1;//Y1 SETELAH DITRANSFORMASI
int drawx2;//X2 SETELAH DITRANSFORMASI
int drawy2;//Y2 SETELAH DITRANSFORMASI
int drawfx;//X UNTUK FILL
int drawfy;//Y UNTUK FILL
scale2(0.1*y,0.1*y,x*213+112,mobilY-10,ms);
translate2(-(x-1)*(213)*(10-y)/10,-(mobilY-40-(maxy+1)/8-1)*(10-y)/10,mt);
transformPoints2(ms,x*213+82,&drawx1,mobilY+20,&drawy1);
transformPoints2(ms,x*213+142,&drawx2,mobilY+20,&drawy2);
transformPoints2(mt,drawx1,&drawx1,drawy1,&drawy1);
transformPoints2(mt,drawx2,&drawx2,drawy2,&drawy2);
lineDDA(drawx1, drawy1, drawx2, drawy2); //BAWAH LUAR
transformPoints2(ms,x*213+82,&drawx1,mobilY-40,&drawy1);
transformPoints2(ms,x*213+142,&drawx2,mobilY-40,&drawy2);
transformPoints2(mt,drawx1,&drawx1,drawy1,&drawy1);
transformPoints2(mt,drawx2,&drawx2,drawy2,&drawy2);
lineDDA(drawx1, drawy1, drawx2, drawy2);//ATAS LUAR
transformPoints2(ms,x*213+82,&drawx1,mobilY+20,&drawy1);
transformPoints2(ms,x*213+82,&drawx2,mobilY-40,&drawy2);
transformPoints2(mt,drawx1,&drawx1,drawy1,&drawy1);
transformPoints2(mt,drawx2,&drawx2,drawy2,&drawy2);
lineDDA(drawx1, drawy1, drawx2, drawy2); //KIRI LUAR
transformPoints2(ms,x*213+142,&drawx1,mobilY+20,&drawy1);
transformPoints2(ms,x*213+142,&drawx2,mobilY-40,&drawy2);
transformPoints2(mt,drawx1,&drawx1,drawy1,&drawy1);
transformPoints2(mt,drawx2,&drawx2,drawy2,&drawy2);
lineDDA(drawx1, drawy1, drawx2, drawy2); //KANAN LUAR
//AREA FILLNYA
if(y>0){//KALO Y 0 JADI TITIK, GA AMAN BUAT FILL
transformPoints2(ms,x*213+82,&drawfx,mobilY-40,&drawfy);
transformPoints2(mt,drawfx,&drawfx,drawfy,&drawfy);
boundaryFill4(drawfx+2,drawfy+1,BLUE,WHITE);
}
// lineDDA(x*213+82, yFactor()*(mobilY+20), x*213+142, yFactor()*(mobilY+20)); //BAWAH LUAR
// lineDDA(x*213+88, yFactor()*(mobilY+15), x*213+137, yFactor()*(mobilY+15)); //BAWAH DALAM
// lineDDA(x*213+82, yFactor()*(mobilY-40),x*213+142, yFactor()*(mobilY-40));//ATAS LUAR
// lineDDA(x*213+87, yFactor()*(mobilY-35), x*213+137, yFactor()*(mobilY-35)); //ATAS DALAM
// lineDDA(x*213+82, yFactor()*(mobilY+20), x*213+82, yFactor()*(mobilY-40)); //KIRI LUAR
// lineDDA(x*213+87, yFactor()*(mobilY+15), x*213+87, yFactor()*(mobilY-35)); //KIRI DALAM
// lineDDA(x*213+142, yFactor()*(mobilY+20), x*213+142, yFactor()*(mobilY-40)); //KANAN LUAR
// lineDDA(x*213+137, yFactor()*(mobilY+15), x*213+137, yFactor()*(mobilY-35)); //KANAN DALAM
// lineDDA(x*213+92, yFactor()*(mobilY+15), x*213+137, yFactor()*(mobilY-30));
// lineDDA(x*213+87, yFactor()*(mobilY+10), x*213+132, yFactor()*(mobilY-35));
// lineDDA(x*213+132, yFactor()*(mobilY+15), x*213+87, yFactor()*(mobilY-30));
// lineDDA(x*213+137, yFactor()*(mobilY+10), x*213+92, yFactor()*(mobilY-35));
}else{
Matrix3x3 mr;//ROTASI
Matrix3x3 mt;//TRANSLASI
//TITIK PENGGAMBARAN
int drawx1;//X1 SETELAH DITRANSFORMASI
int drawy1;//Y1 SETELAH DITRANSFORMASI
int drawx2;//X2 SETELAH DITRANSFORMASI
int drawy2;//Y2 SETELAH DITRANSFORMASI
int drawfx;//X UNTUK FILL
int drawfy;//Y UNTUK FILL
// cout << "kiri " << x*213+82 << "\n";
// cout << "kanan " << x*213+142 << "\n";
// cout << "atas " << 70+(mobilY-40) << "\n";
// cout << "bawah " << 70+(mobilY+20) << "\n";
// cout << "Tengah " << x*213+112 << " " << 70+(mobilY-10) <<"\n";
rotate2(22.5*(y-11),x*213+112,70+(mobilY-10),mr);
translate2((y-11)*(maxx-(x*213))/8,0,mt);
transformPoints2(mr,x*213+82,&drawx1,70+(mobilY+20),&drawy1);
transformPoints2(mr,x*213+142,&drawx2,70+(mobilY+20),&drawy2);
transformPoints2(mt,drawx1,&drawx1,drawy1,&drawy1);
transformPoints2(mt,drawx2,&drawx2,drawy2,&drawy2);
lineDDA(drawx1, drawy1, drawx2, drawy2); //BAWAH LUAR
transformPoints2(mr,x*213+82,&drawx1,70+(mobilY-40),&drawy1);
transformPoints2(mr,x*213+142,&drawx2,70+(mobilY-40),&drawy2);
transformPoints2(mt,drawx1,&drawx1,drawy1,&drawy1);
transformPoints2(mt,drawx2,&drawx2,drawy2,&drawy2);
lineDDA(drawx1, drawy1, drawx2, drawy2);//ATAS LUAR
transformPoints2(mr,x*213+82,&drawx1,70+(mobilY+20),&drawy1);
transformPoints2(mr,x*213+82,&drawx2,70+(mobilY-40),&drawy2);
transformPoints2(mt,drawx1,&drawx1,drawy1,&drawy1);
transformPoints2(mt,drawx2,&drawx2,drawy2,&drawy2);
lineDDA(drawx1, drawy1, drawx2, drawy2); //KIRI LUAR
transformPoints2(mr,x*213+142,&drawx1,70+(mobilY+20),&drawy1);
transformPoints2(mr,x*213+142,&drawx2,70+(mobilY-40),&drawy2);
transformPoints2(mt,drawx1,&drawx1,drawy1,&drawy1);
transformPoints2(mt,drawx2,&drawx2,drawy2,&drawy2);
lineDDA(drawx1, drawy1, drawx2, drawy2); //KANAN LUAR
//AREA FILLNYA
transformPoints2(mr,x*213+82+5,&drawfx,70+(mobilY-37),&drawfy);
transformPoints2(mt,drawfx,&drawfx,drawfy,&drawfy);
boundaryFill4(drawfx,drawfy,BLUE,WHITE);
// lineDDA(x*213+82, 70+(mobilY+20), x*213+142, 70+(mobilY+20)); //BAWAH LUAR
// lineDDA(x*213+88, 70+(mobilY+15), x*213+137, 70+(mobilY+15)); //BAWAH DALAM
// lineDDA(x*213+82, 70+(mobilY-40),x*213+142, 70+(mobilY-40));//ATAS LUAR
// lineDDA(x*213+87, 70+(mobilY+-35), x*213+137, 70+(mobilY-35)); //ATAS DALAM
// lineDDA(x*213+82, 70+(mobilY+20), x*213+82, 70+(mobilY-40)); //KIRI LUAR
// lineDDA(x*213+87, 70+(mobilY+15), x*213+87, 70+(mobilY-35)); //KIRI DALAM
// lineDDA(x*213+142, 70+(mobilY+20), x*213+142, 70+(mobilY-40)); //KANAN LUAR
//lineDDA(x*213+137, 70+(mobilY+15), x*213+137, 70+(mobilY-35)); //KANAN DALAM
//lineDDA(x*213+92, 70+(mobilY+15), x*213+137, 70+(mobilY-30));
//lineDDA(x*213+87, 70+(mobilY+10), x*213+132, 70+(mobilY-35));
//lineDDA(x*213+132, 70+(mobilY+15), x*213+87, 70+(mobilY-30));
//lineDDA(x*213+137, 70+(mobilY+10), x*213+92, 70+(mobilY-35));
}
}else if(type == 1){//STONE
lineDDA(40, 80, 90, 80);
lineDDA(40, 80, 50, 60);
lineDDA(50, 60, 55, 55);
lineDDA(55, 55, 75, 65);
lineDDA(75, 65, 90, 80);
}
}
