void DynamicChain2D::GetKineticEnergyMatrixDeriv(int z,Matrix& dB) const { dB.resize(q.n,q.n); Array2D<Vector2> dJP(q.n,q.n); //no angular rotation needed //Array2D<Real> dJO(q.n,q.n); Real dJOTemp; for(int i=0;i<q.n;i++) { for(int j=0;j<q.n;j++) { if(!GetJacobianDeriv(links[i].com,i,j,z,dJOTemp,dJP(i,j))) { dJP(i,j)=Zero; } } } for(int i=0;i<q.n;i++) { for(int j=0;j<q.n;j++) { Real val=Zero; Real inertiaWorld; for(int k=0;k<q.n;k++) { //if(!IsAncestor(k,z)) continue; const Vector2& JPi=JP(k,i); const Vector2& JPj=JP(k,j); const Vector2& dJPi=dJP(k,i); const Vector2& dJPj=dJP(k,j); links[k].GetWorldInertia(inertiaWorld); val+=links[k].mass*(dot(JPi,dJPj)+dot(dJPi,JPj)); //no derivative of angular rotation //val+=(inertiaWorld*dJOi*JOj+inertiaWorld*JOi*dJOj); //no coriolis forces //val+=([w]*inertiaWorld-inertiaWorld*[w])*JOi*JOj } dB(i,j)=val; } } }
void DynamicChain2D::GetKineticEnergyMatrix(Matrix& B) const { B.resize(q.n,q.n); for(int i=0;i<q.n;i++) { for(int j=0;j<q.n;j++) { Real inertiaWorld; B(i,j) = Zero; for(int k=0;k<q.n;k++) { links[k].GetWorldInertia(inertiaWorld); B(i,j) += links[k].mass*dot(JP(k,i),JP(k,j))+inertiaWorld*JO(k,i)*JO(k,j); } } } }
void Stwart(vector<long>&IA,vector<long>&MA, vector<long>&JROW,vector<long>&JCOL) { static long M = JROW.size(); vector<long> IR(M),IC(M),R(M),C(M),IP(M),JP(M),IW(M); static long KERNS, MEND, LG, IER, L = MA[M]; // printf("M = %ld\n",M); // forVector(MA,i) printf("MA[%ld] = %ld\n",i,MA[i]); // forVector(IA,i) printf("IA[%ld] = %ld\n",i,IA[i]); // printf("L=%ld\n",L); // forVector(R,i) printf("R[%ld] = %ld\n",i,R[i]); // forVector(C,i) printf("C[%ld] = %ld\n",i,C[i]); // forVector(IR,i) printf("IR[%ld] = %ld\n",i,IR[i]); // forVector(IC,i) printf("IC[%ld] = %ld\n",i,IC[i]); // forVector(JROW,i) printf("JROW[%ld] = %ld\n",i,JROW[i]); // forVector(JCOL,i) printf("JCOL[%ld] = %ld\n",i,JCOL[i]); // forVector(IP,i) printf("IP[%ld] = %ld\n",i,IP[i]); // forVector(JP,i) printf("JP[%ld] = %ld\n",i,JP[i]); // forVector(IW,i) printf("IW[%ld] = %ld\n",i,IW[i]); printf("KERNS = %ld, MEND = %ld, LG = %ld, IER = %ld\n", KERNS, MEND, LG, IER); stwart_(&IA[0],&L,&MA[0],&M,&R[0],&C[0],&IR[0],&IC[0], &JROW[0],&JCOL[0],&IP[0],&JP[0],&KERNS,&MEND,&IW[0],&LG,&IER); }
double ChowLiuTree::calcMutInfo(int word1, int word2) { double accumulation = 0; double P00 = JP(word1, false, word2, false); if(P00) accumulation += P00 * log(P00 / (P(word1, false)*P(word2, false))); double P01 = JP(word1, false, word2, true); if(P01) accumulation += P01 * log(P01 / (P(word1, false)*P(word2, true))); double P10 = JP(word1, true, word2, false); if(P10) accumulation += P10 * log(P10 / (P(word1, true)*P(word2, false))); double P11 = JP(word1, true, word2, true); if(P11) accumulation += P11 * log(P11 / (P(word1, true)*P(word2, true))); return accumulation; }
/*JP @*/ static void op_0xc3(Z80EX_CONTEXT *cpu) { temp_word.b.l=READ_OP(); temp_word.b.h=READ_OP(); JP(temp_word.w); T_WAIT_UNTIL(10); return; }
void free(void *ptr) { __asm POP HL EX (SP), IX PCALL(FREE) EX (SP), IX JP (HL) __endasm; ptr; }
void DynamicChain2D::Update_J() { JP.resize(q.n,q.n); JO.resize(q.n,q.n); for(int i=0;i<q.n;i++) { for(int j=0;j<q.n;j++) { if(!GetJacobian(links[i].com,i,j,JO(i,j),JP(i,j))) { } } } }
/*JP M,@*/ static void op_0xfa(Z80EX_CONTEXT *cpu) { temp_word.b.l=READ_OP(); temp_word.b.h=READ_OP(); if(F & FLAG_S) { JP(temp_word.w); T_WAIT_UNTIL(10); } else { T_WAIT_UNTIL(10);MEMPTR=temp_word.w;} return; }
//dBij/dqz Real DynamicChain2D::GetKineticEnergyDeriv(int i,int j,int z) const { Real val=Zero; Real JOi,JOj,dJOi,dJOj; Vector2 JPi,JPj,dJPi,dJPj; Real inertiaWorld; for(int k=0;k<q.n;k++) { //if(!IsAncestor(k,z)) continue; //JOi=JO(k,i); //JOj=JO(k,j); JPi=JP(k,i); JPj=JP(k,j); if(!GetJacobianDeriv(links[k].com,k,i,z,dJOi,dJPi)) { dJOi=Zero; dJPi=Zero; } if(!GetJacobianDeriv(links[k].com,k,j,z,dJOj,dJPj)) { dJOj=Zero; dJPj=Zero; } links[k].GetWorldInertia(inertiaWorld); val+=links[k].mass*(dot(JPi,dJPj)+dot(dJPi,JPj)); //no derivative of angular rotation //val+=(inertiaWorld*dJOi*JOj+inertiaWorld*JOi*dJOj); //no coriolis forces //val+=([w]*inertiaWorld-inertiaWorld*[w])*JOi*JOj } return val; }
void DynamicChain2D::AddForceVector(Real torque, const Vector2& force, int i, Vector& F) const { //multiply by full jacobian of link i transpose int j=i; while(j!=-1) { F(j)+=JO(i,j)*torque + dot(JP(i,j),force); j=parents[j]; } /* non-sparse version Matrix Ji; GetFullJacobian(links[i].com,i,Ji); for(int i=0;i<q.n;i++) { F(i) += Ji(0,i)*torque + Ji(1,i)*force.x + Ji(2,i)*force.y; }*/ }
/* This is the messy part. This is what happens when you copy-paste one thing that works make the rest of it work */ int execute(int **mem, int *pc, int flow, int *areg, int *breg, int *creg){ int i; int *tryte; tryte = mem[*pc]; /* output instructions */ if (tryte[0]==-1&&tryte[1]==-1&&tryte[2]==0){ if(flow) puts("OUT A "); printf("PRINT %d\n",tern2dec(areg)); *pc+=1; }else if (tryte[0]==0&&tryte[1]==-1&&tryte[2]==0){ if(flow) puts("OUT B "); printf("PRINT %d\n",tern2dec(breg)); *pc+=1; }else if (tryte[0]==1&&tryte[1]==-1&&tryte[2]==0){ if(flow) puts("OUT C "); printf("PRINT %d\n",tern2dec(creg)); *pc+=1; /* input instructions */ }else if (tryte[0]==-1&&tryte[1]==0&&tryte[2]==1){ if(flow) puts("IN A "); printf("IN A? :"); scanf("%d",&i); dec2tern(i); for(i=0;i<WIDTH;i++){ areg[i] = itryte[i]; } *pc+=1; }else if (tryte[0]==0&&tryte[1]==0&&tryte[2]==1){ if(flow) puts("IN B "); printf("IN B? :"); scanf("%d",&i); dec2tern(i); for(i=0;i<WIDTH;i++){ breg[i] = itryte[i]; } *pc+=1; }else if (tryte[0]==1&&tryte[1]==0&&tryte[2]==1){ if(flow) puts("IN C "); scanf("%d",&i); dec2tern(i); for(i=0;i<WIDTH;i++){ creg[i] = itryte[i]; } *pc+=1; *pc+=1; /* load instructions */ }else if (tryte[0]==-1&&tryte[1]==-1&&tryte[2]==-1){ if(flow) puts("LD A "); for(i=0;i<WIDTH;i++){ areg[i]=mem[364+tern2dec(mem[*pc+1])][i]; } *pc+=2; }else if (tryte[0]==0&&tryte[1]==-1&&tryte[2]==-1){ if(flow) puts("LD B "); for(i=0;i<WIDTH;i++){ breg[i]=mem[364+tern2dec(mem[*pc+1])][i]; } *pc+=2; }else if (tryte[0]==1&&tryte[1]==-1&&tryte[2]==-1){ if(flow) puts("LD C "); for(i=0;i<WIDTH;i++){ creg[i]=mem[364+tern2dec(mem[*pc+1])][i]; } *pc+=2; /* increment instructions */ }else if (tryte[0]==-1 &&tryte[1]==0 &&tryte[2]==-1){ if(flow) puts("INC A"); incrTryte(areg); *pc+=1; }else if (tryte[0]==0 &&tryte[1]==0 &&tryte[2]==-1){ if(flow) puts("INC B"); incrTryte(breg); *pc+=1; }else if (tryte[0]==1 &&tryte[1]==0 &&tryte[2]==-1){ if(flow) puts("INC C"); incrTryte(creg); *pc+=1; /* conditional skip instructions */ }else if (tryte[0]==-1&&tryte[1]==1&&tryte[2]==-1){ if(flow) puts("SKP A "); if(condSkip(areg)) *pc+=3; else *pc+=1; }else if (tryte[0]==0&&tryte[1]==1&&tryte[2]==-1){ if(flow) puts("SKP B "); if(condSkip(breg)) *pc+=3; else *pc+=1; }else if (tryte[0]==1&&tryte[1]==1&&tryte[2]==-1){ if(flow) puts("SKP C "); if(condSkip(creg)) *pc+=3; else *pc+=1; /* 3 ADD instructions */ }else if(tryte[0]==-1 &&tryte[1]==1 &&tryte[2]==1){ if(flow) puts("ADD A"); addTryte(areg,mem[364+tern2dec(mem[*pc+1])]); *pc+=2; }else if(tryte[0]==0 &&tryte[1]==1 &&tryte[2]==1){ if(flow) puts("ADD B"); addTryte(breg,mem[364+tern2dec(mem[*pc+1])]); *pc+=2; }else if(tryte[0]==1 &&tryte[1]==1 &&tryte[2]==1){ if(flow) puts("ADD C"); addTryte(creg,mem[364+tern2dec(mem[*pc+1])]); *pc+=2; /* 3 STO instructions */ }else if(tryte[0]==-1 &&tryte[1]==-1 &&tryte[2]==1){ if(flow) puts("STO A"); STO(areg,mem[364+tern2dec(mem[*pc+1])]); *pc+=2; }else if(tryte[0]==0 &&tryte[1]==-1 &&tryte[2]==1){ if(flow) puts("STO B"); STO(breg,mem[364+tern2dec(mem[*pc+1])]); *pc+=2; }else if(tryte[0]==1 &&tryte[1]==-1 &&tryte[2]==1){ if(flow) puts("STO C"); STO(creg,mem[364+tern2dec(mem[*pc+1])]); *pc+=2; /* HALT Instruction */ }else if(tryte[0]==-1 &&tryte[1]==0 &&tryte[2]==0){ if(flow) puts("HALT"); return 1; /* Bitwise invert instructions */ }else if(tryte[0]==-1 &&tryte[1]==1 &&tryte[2]==0){ if(flow) puts("INV A"); invTryte(areg); *pc+=1; }else if(tryte[0]==0 &&tryte[1]==1 &&tryte[2]==0){ if(flow) puts("INV B"); invTryte(breg); *pc+=1; }else if(tryte[0]==1 &&tryte[1]==1 &&tryte[2]==0){ if(flow) puts("INV C"); invTryte(creg); *pc+=1; /* Unconditional JP instruction */ }else if (tryte[0]==1&&tryte[1]==0&&tryte[2]==0){ if(flow) puts("JP"); JP(mem[*pc+1],pc); /* NOOP instruction */ }else if (tryte[0]==0&&tryte[1]==0&&tryte[2]==0){ if(flow) puts("NOOP"); *pc+=1; }else{ if(flow){ dispTryte(tryte); puts("?"); } *pc+=1; } return 0; }