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;
}
