// File: /workspace/github_c_makefile_star2_O2/Alessandro-Barbieri[P]SpaceLib/src/LINEAR.c
// Function: dyn_eq
int   dyn_eq(MAT4 J, MAT4 Wp, MAT4 F, int var[2][6])
{
	real mat[6][7];
	real acc[6],t,arm;
	int i,j,k,it,nvar,irank,neq;
	int var_a[6],var_f[6],ivar[6],tappo;
	int var2[2][6];

	nvar=0;
	for(i=0;i<2;i++)
		for(j=0;j<6;j++)
			if (var[i][j])
				nvar++;
	if (nvar!=6)
	{
		fprintf(stderr,"Error (dyn_eq) : \a wrong number of unknown variables\n");
		return(NOTOK);
	}

	for(i=0;i<6;i++)
	{
		acc[i]=0.; mat[i][6]=0.; ivar[i]=i;
	}
	if (!var[0][X])		acc[X]=Wp[Z][Y];          /* angular acceleration */
	if (!var[0][Y])		acc[Y]=Wp[X][Z];
	if (!var[0][Z])		acc[Z]=Wp[Y][X];
	if (!var[0][3+X])		acc[3+X]=Wp[X][U];        /* accelerations */
	if (!var[0][3+Y])		acc[3+Y]=Wp[Y][U];
	if (!var[0][3+Z])		acc[3+Z]=Wp[Z][U];
	if (!var[1][X])		mat[X][6]=F[Z][Y];        /* torques */
	if (!var[1][Y])		mat[Y][6]=F[X][Z];
	if (!var[1][Z])		mat[Z][6]=F[Y][X];
	if (!var[1][3+X])		mat[3+X][6]=F[X][U];      /* forces */
	if (!var[1][3+Y])		mat[3+Y][6]=F[Y][U];
	if (!var[1][3+Z])		mat[3+Z][6]=F[Z][U];

	mat[X][X]=J[Y][Y]+J[Z][Z]; mat[Y][Y]=J[X][X]+J[Z][Z];
	mat[Z][Z]=J[X][X]+J[Y][Y];

	mat[X][Y]=mat[Y][X]= -J[X][Y];
	mat[X][Z]=mat[Z][X]= -J[X][Z];
	mat[Y][Z]=mat[Z][Y]= -J[Y][Z];

	mat[X][3+X]=0.;mat[Y][3+Y]=0.;mat[Z][3+Z]=0.;
	mat[3+X][X]=0.;mat[3+Y][Y]=0.;mat[3+Z][Z]=0.;

	mat[Y][3+Z]=mat[3+Z][Y]= -(mat[Z][3+Y]=mat[3+Y][Z]=J[X][U]);
	mat[Z][3+X]=mat[3+X][Z]= -(mat[X][3+Z]=mat[3+Z][X]=J[Y][U]);
	mat[X][3+Y]=mat[3+Y][X]= -(mat[Y][3+X]=mat[3+X][Y]=J[Z][U]);

	mat[3+X][3+X]=J[U][U]; mat[3+X][3+Y]=0.;      mat[3+X][3+Z]=0.;
	mat[3+Y][3+X]=0.     ; mat[3+Y][3+Y]=J[U][U]; mat[3+Y][3+Z]=0.;
	mat[3+Z][3+X]=0.     ; mat[3+Z][3+Y]=0.;      mat[3+Z][3+Z]=J[U][U];


	//printm67("mat",mat);

	for(j=0;j<6;j++)
	{
		var_a[j]=j; var_f[j]=j;
	}

	for(i=0;i<2;i++)
		for(j=0;j<6;j++)
			var2[i][j]=var[i][j];

	neq=0;
	for (j=0;j<6;j++)
	{
		if(var[0][j]) {
			neq++;
			//printf("j: %d neq: %d\n",j,neq);
		} else {
			for(i=j+1;i<6;i++)
			{
				if(var[0][i])
				{
					neq++;
					//printf(" swabc i:%d j:%d neq:%d\n",i,j,neq); /* debug only */
					swabc(M mat,6,7,6,i,j,var_a);
					t=acc[j]; acc[j]=acc[i]; acc[i]=t;
					it=var[0][j]; var[0][j]=var[0][i]; var[0][i]=it;
					it=var[1][j]; var[1][j]=var[1][i]; var[1][i]=it;
					//printf(" swabr \n"); /* debug only */
					swabr(M mat,6,7,7,i,j);
					k=var_f[j]; var_f[j]=var_f[i]; var_f[i]=k;
					break;
				}
			}
		}
	}

	//printf("**neq: %d\n",neq);
	tappo= -1;
	for(i=0;i<6;i++)
	{
		for(j=neq,t=0;j<6;j++)
		{
			t += mat[i][j]*acc[j];
		}
		mat[i][neq]=mat[i][6]-t;
	}
	if(neq!=0)
	{
		linear(M mat,6,7,neq,neq,1,ivar,&irank,&arm,&tappo);
		//printm67("mat--",mat);
		//printf("irank: %d",irank);
		if (irank!=neq){
			fprintf(stderr,"\a DYN_EQ: error matrix is singular!\n");
			return(-irank);
			}
	}

	for (i=0;i<neq;i++)
		acc[var_a[ivar[i]]]=mat[i][neq];

	for (i=0;i<4;i++)
	{
		Wp[i][i]=Wp[U][i]=0.;
	}
	if(var2[0][X]) Wp[Y][Z]= -(Wp[Z][Y]=acc[X]);
	if(var2[0][Y]) Wp[Z][X]= -(Wp[X][Z]=acc[Y]);
	if(var2[0][Z]) Wp[X][Y]= -(Wp[Y][X]=acc[Z]);
	if(var2[0][3+X]) Wp[X][U]=acc[3+X];
	if(var2[0][3+Y]) Wp[Y][U]=acc[3+Y];
	if(var2[0][3+Z]) Wp[Z][U]=acc[3+Z];

	skew4(Wp,J,F);

	return(OK);
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x258, %rsp            # imm = 0x258
movq %rcx, %r12
xorl %eax, %eax
xorl %r8d, %r8d
cmpq $0x2, %rax
je 0x25ab
xorl %r9d, %r9d
cmpq $0x6, %r9
je 0x25a2
cmpl $0x1, (%rcx,%r9,4)
sbbl $-0x1, %r8d
incq %r9
jmp 0x258e
incq %rax
addq $0x18, %rcx
jmp 0x2585
cmpl $0x6, %r8d
jne 0x2b87
pushq $-0x6
popq %rax
leaq 0xf0(%rsp), %rcx
testq %rax, %rax
je 0x25e4
andq $0x0, 0x40(%rsp,%rax,8)
andq $0x0, (%rcx)
leal 0x6(%rax), %r8d
movl %r8d, 0x228(%rsp,%rax,4)
incq %rax
addq $0x38, %rcx
jmp 0x25c0
cmpl $0x0, (%r12)
jne 0x25f6
movsd 0x48(%rsi), %xmm0
movsd %xmm0, 0x10(%rsp)
cmpl $0x0, 0x4(%r12)
jne 0x2609
movsd 0x10(%rsi), %xmm0
movsd %xmm0, 0x18(%rsp)
cmpl $0x0, 0x8(%r12)
jne 0x261c
movsd 0x20(%rsi), %xmm0
movsd %xmm0, 0x20(%rsp)
cmpl $0x0, 0xc(%r12)
jne 0x262f
movsd 0x18(%rsi), %xmm0
movsd %xmm0, 0x28(%rsp)
cmpl $0x0, 0x10(%r12)
jne 0x2642
movsd 0x38(%rsi), %xmm0
movsd %xmm0, 0x30(%rsp)
cmpl $0x0, 0x14(%r12)
jne 0x2655
movsd 0x58(%rsi), %xmm0
movsd %xmm0, 0x38(%rsp)
cmpl $0x0, 0x18(%r12)
jne 0x266b
movsd 0x48(%rdx), %xmm0
movsd %xmm0, 0xf0(%rsp)
cmpl $0x0, 0x1c(%r12)
jne 0x2681
movsd 0x10(%rdx), %xmm0
movsd %xmm0, 0x128(%rsp)
cmpl $0x0, 0x20(%r12)
jne 0x2697
movsd 0x20(%rdx), %xmm0
movsd %xmm0, 0x160(%rsp)
cmpl $0x0, 0x24(%r12)
jne 0x26ad
movsd 0x18(%rdx), %xmm0
movsd %xmm0, 0x198(%rsp)
cmpl $0x0, 0x28(%r12)
jne 0x26c3
movsd 0x38(%rdx), %xmm0
movsd %xmm0, 0x1d0(%rsp)
cmpl $0x0, 0x2c(%r12)
jne 0x26d9
movsd 0x58(%rdx), %xmm0
movsd %xmm0, 0x208(%rsp)
movsd 0x28(%rdi), %xmm0
movsd 0x50(%rdi), %xmm1
movapd %xmm0, %xmm2
addsd %xmm1, %xmm2
movsd %xmm2, 0xc0(%rsp)
movsd (%rdi), %xmm2
addsd %xmm2, %xmm1
movsd %xmm1, 0x100(%rsp)
addsd %xmm2, %xmm0
movsd %xmm0, 0x140(%rsp)
movdqu 0x8(%rdi), %xmm1
movdqa 0x78e1(%rip), %xmm0     # 0xa000
pxor %xmm0, %xmm1
movq %xmm1, 0xf8(%rsp)
pshufd $0xee, %xmm1, %xmm2     # xmm2 = xmm1[2,3,2,3]
movq %xmm2, 0x130(%rsp)
movdqu %xmm1, 0xc8(%rsp)
movq 0x30(%rdi), %xmm1
pxor %xmm0, %xmm1
movq %xmm1, 0x138(%rsp)
movq %xmm1, 0x108(%rsp)
andq $0x0, 0xd8(%rsp)
andq $0x0, 0x118(%rsp)
andq $0x0, 0x158(%rsp)
andq $0x0, 0x168(%rsp)
andq $0x0, 0x1a8(%rsp)
andq $0x0, 0x1e8(%rsp)
movq 0x18(%rdi), %xmm1
movq %xmm1, 0x1b0(%rsp)
movq %xmm1, 0x150(%rsp)
pxor %xmm0, %xmm1
movq %xmm1, 0x1e0(%rsp)
movq %xmm1, 0x120(%rsp)
movq 0x38(%rdi), %xmm1
movq %xmm1, 0x1d8(%rsp)
movq %xmm1, 0xe8(%rsp)
pxor %xmm0, %xmm1
movq %xmm1, 0x178(%rsp)
movq %xmm1, 0x148(%rsp)
movq 0x58(%rdi), %xmm1
movq %xmm1, 0x170(%rsp)
movq %xmm1, 0x110(%rsp)
pxor %xmm0, %xmm1
movq %xmm1, 0x1a0(%rsp)
movq %xmm1, 0xe0(%rsp)
movsd 0x78(%rdi), %xmm0
movsd %xmm0, 0x180(%rsp)
andq $0x0, 0x1b8(%rsp)
pxor %xmm1, %xmm1
movdqu %xmm1, 0x188(%rsp)
movsd %xmm0, 0x1c0(%rsp)
andq $0x0, 0x1c8(%rsp)
movdqa %xmm1, 0x1f0(%rsp)
movsd %xmm0, 0x200(%rsp)
xorl %eax, %eax
cmpq $0x6, %rax
je 0x287b
movl %eax, 0x230(%rsp,%rax,4)
movl %eax, 0x70(%rsp,%rax,4)
incq %rax
jmp 0x2865
leaq 0x90(%rsp), %rax
xorl %ecx, %ecx
movq %r12, %r8
cmpq $0x2, %rcx
je 0x28b1
xorl %r10d, %r10d
cmpq $0x6, %r10
je 0x28a4
movl (%r8,%r10,4), %r9d
movl %r9d, (%rax,%r10,4)
incq %r10
jmp 0x2891
incq %rcx
addq $0x18, %r8
addq $0x18, %rax
jmp 0x2888
movq %rsi, 0x48(%rsp)
movq %rdi, 0x50(%rsp)
movq %rdx, 0x58(%rsp)
xorl %ebx, %ebx
pushq $-0x5
popq %r15
leaq 0xc0(%rsp), %rbp
xorl %r9d, %r9d
cmpq $0x6, %rbx
je 0x29a9
cmpl $0x0, (%r12,%rbx,4)
movq %r15, %r14
je 0x28ed
incl %r9d
jmp 0x299e
testq %r14, %r14
je 0x299e
movl 0x18(%r12,%r14,4), %r13d
incq %r14
testl %r13d, %r13d
je 0x28ed
leal 0x5(%r14), %r8d
movl %r8d, 0x8(%rsp)
incl %r9d
movq %r9, 0x60(%rsp)
subq $0x8, %rsp
movq %rbp, %rdi
pushq $0x7
popq %rdx
pushq $0x6
popq %rcx
movl %ebx, %r9d
leaq 0x238(%rsp), %rax
pushq %rax
callq 0x23c0
popq %rax
popq %rcx
movsd 0x10(%rsp,%rbx,8), %xmm0
movq 0x38(%rsp,%r14,8), %xmm1
movq %xmm1, 0x10(%rsp,%rbx,8)
movsd %xmm0, 0x38(%rsp,%r14,8)
movl %r13d, (%r12,%rbx,4)
andl $0x0, 0x14(%r12,%r14,4)
movl 0x18(%r12,%rbx,4), %eax
movl 0x2c(%r12,%r14,4), %ecx
movl %ecx, 0x18(%r12,%rbx,4)
movl %eax, 0x2c(%r12,%r14,4)
movq %rbp, %rdi
pushq $0x7
popq %rcx
movl %ecx, %edx
movl 0x8(%rsp), %r8d
movl %ebx, %r9d
callq 0x21a0
movq 0x60(%rsp), %r9
movl 0x70(%rsp,%rbx,4), %eax
movl 0x84(%rsp,%r14,4), %ecx
movl %ecx, 0x70(%rsp,%rbx,4)
movl %eax, 0x84(%rsp,%r14,4)
incq %rbx
incq %r15
jmp 0x28d1
orl $-0x1, 0xc(%rsp)
movslq %r9d, %rbx
xorl %eax, %eax
cmpq $0x6, %rax
je 0x2a00
xorpd %xmm0, %xmm0
movq %rbx, %rcx
cmpq $0x5, %rcx
jg 0x29db
movsd (%rbp,%rcx,8), %xmm1
mulsd 0x10(%rsp,%rcx,8), %xmm1
addsd %xmm1, %xmm0
incq %rcx
jmp 0x29c0
imulq $0x38, %rax, %rcx
addq %rsp, %rcx
addq $0xc0, %rcx
movsd 0x30(%rcx), %xmm1
subsd %xmm0, %xmm1
movsd %xmm1, (%rcx,%rbx,8)
incq %rax
addq $0x38, %rbp
jmp 0x29b3
testl %r9d, %r9d
je 0x2a59
leaq 0xc(%rsp), %rax
leaq 0x250(%rsp), %r10
leaq 0x6c(%rsp), %r14
leaq 0x210(%rsp), %r11
leaq 0xc0(%rsp), %rdi
pushq $0x6
popq %rsi
pushq $0x7
popq %rdx
movq %r9, %r15
pushq $0x1
popq %r9
movl %r15d, %ecx
movl %r15d, %r8d
pushq %rax
pushq %r10
pushq %r14
pushq %r11
callq 0x2210
movq %r15, %r9
addq $0x20, %rsp
movl (%r14), %ebp
cmpl %r9d, %ebp
jne 0x2ba7
pushq $0x1
popq %rbp
xorl %eax, %eax
testl %r9d, %r9d
cmovlel %eax, %r9d
leaq (%rsp,%rbx,8), %rcx
addq $0xc0, %rcx
movq 0x58(%rsp), %rdx
movq 0x50(%rsp), %rsi
movq 0x48(%rsp), %rdi
cmpq %rax, %r9
je 0x2aa8
movsd (%rcx), %xmm0
movslq 0x210(%rsp,%rax,4), %r8
movslq 0x230(%rsp,%r8,4), %r8
movsd %xmm0, 0x10(%rsp,%r8,8)
incq %rax
addq $0x38, %rcx
jmp 0x2a7f
pushq $0xc
popq %rax
movq %rdi, %rcx
cmpq $0x10, %rax
je 0x2ac6
andq $0x0, (%rdi,%rax,8)
andq $0x0, (%rcx)
incq %rax
addq $0x28, %rcx
jmp 0x2aae
cmpl $0x0, 0x90(%rsp)
je 0x2ae8
movsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x48(%rdi)
xorpd 0x751d(%rip), %xmm0     # 0xa000
movlpd %xmm0, 0x30(%rdi)
cmpl $0x0, 0x94(%rsp)
je 0x2b0a
movsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x10(%rdi)
xorpd 0x74fb(%rip), %xmm0     # 0xa000
movlpd %xmm0, 0x40(%rdi)
cmpl $0x0, 0x98(%rsp)
je 0x2b2c
movsd 0x20(%rsp), %xmm0
movsd %xmm0, 0x20(%rdi)
xorpd 0x74d9(%rip), %xmm0     # 0xa000
movlpd %xmm0, 0x8(%rdi)
cmpl $0x0, 0x9c(%rsp)
je 0x2b41
movsd 0x28(%rsp), %xmm0
movsd %xmm0, 0x18(%rdi)
cmpl $0x0, 0xa0(%rsp)
je 0x2b56
movsd 0x30(%rsp), %xmm0
movsd %xmm0, 0x38(%rdi)
cmpl $0x0, 0xa4(%rsp)
je 0x2b6b
movsd 0x38(%rsp), %xmm0
movsd %xmm0, 0x58(%rdi)
pushq $0x4
popq %rcx
callq 0x2490
movl %ebp, %eax
addq $0x258, %rsp            # imm = 0x258
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0xb46a(%rip), %rax      # 0xdff8
movq (%rax), %rcx
leaq 0x7480(%rip), %rdi      # 0xa018
pushq $0x35
popq %rsi
pushq $0x1
popq %rdx
callq 0x2410
xorl %ebp, %ebp
jmp 0x2b73
movq 0xb44a(%rip), %rax      # 0xdff8
movq (%rax), %rcx
leaq 0x7496(%rip), %rdi      # 0xa04e
pushq $0x24
popq %rsi
pushq $0x1
popq %rdx
callq 0x2410
negl %ebp
jmp 0x2b73