// File: /workspace/github_c_makefile_star2_O1/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 $0x238, %rsp            # imm = 0x238
movq %rsi, %r15
xorl %r8d, %r8d
movq %rcx, %rax
xorl %esi, %esi
xorl %r9d, %r9d
cmpl $0x1, (%rax,%r9,4)
sbbl $-0x1, %esi
incq %r9
cmpq $0x6, %r9
jne 0x28bb
leaq 0x1(%r8), %r9
addq $0x18, %rax
testq %r8, %r8
movq %r9, %r8
je 0x28b8
cmpl $0x6, %esi
jne 0x2ef6
xorps %xmm0, %xmm0
movaps %xmm0, 0x30(%rsp)
movaps %xmm0, 0x20(%rsp)
movaps %xmm0, 0x10(%rsp)
leaq 0xf0(%rsp), %rax
xorl %esi, %esi
movq $0x0, (%rax)
movl %esi, 0x210(%rsp,%rsi,4)
incq %rsi
addq $0x38, %rax
cmpq $0x6, %rsi
jne 0x2901
cmpl $0x0, (%rcx)
jne 0x292d
movsd 0x48(%r15), %xmm0
movsd %xmm0, 0x10(%rsp)
cmpl $0x0, 0x4(%rcx)
jne 0x293f
movsd 0x10(%r15), %xmm0
movsd %xmm0, 0x18(%rsp)
cmpl $0x0, 0x8(%rcx)
jne 0x2951
movsd 0x20(%r15), %xmm0
movsd %xmm0, 0x20(%rsp)
cmpl $0x0, 0xc(%rcx)
jne 0x2963
movsd 0x18(%r15), %xmm0
movsd %xmm0, 0x28(%rsp)
cmpl $0x0, 0x10(%rcx)
jne 0x2975
movsd 0x38(%r15), %xmm0
movsd %xmm0, 0x30(%rsp)
cmpl $0x0, 0x14(%rcx)
jne 0x2987
movsd 0x58(%r15), %xmm0
movsd %xmm0, 0x38(%rsp)
cmpl $0x0, 0x18(%rcx)
jne 0x299b
movsd 0x48(%rdx), %xmm0
movsd %xmm0, 0xf0(%rsp)
cmpl $0x0, 0x1c(%rcx)
jne 0x29af
movsd 0x10(%rdx), %xmm0
movsd %xmm0, 0x128(%rsp)
cmpl $0x0, 0x20(%rcx)
jne 0x29c3
movsd 0x20(%rdx), %xmm0
movsd %xmm0, 0x160(%rsp)
cmpl $0x0, 0x24(%rcx)
jne 0x29d7
movsd 0x18(%rdx), %xmm0
movsd %xmm0, 0x198(%rsp)
cmpl $0x0, 0x28(%rcx)
jne 0x29eb
movsd 0x38(%rdx), %xmm0
movsd %xmm0, 0x1d0(%rsp)
cmpl $0x0, 0x2c(%rcx)
jne 0x29ff
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)
movsd 0x8(%rdi), %xmm1
movapd 0x75bb(%rip), %xmm0     # 0xa000
xorpd %xmm0, %xmm1
movlpd %xmm1, 0xf8(%rsp)
movlpd %xmm1, 0xc8(%rsp)
movsd 0x10(%rdi), %xmm1
xorpd %xmm0, %xmm1
movlpd %xmm1, 0x130(%rsp)
movlpd %xmm1, 0xd0(%rsp)
movsd 0x30(%rdi), %xmm1
xorpd %xmm0, %xmm1
movlpd %xmm1, 0x138(%rsp)
movlpd %xmm1, 0x108(%rsp)
xorl %eax, %eax
movq %rax, 0xd8(%rsp)
movq %rax, 0x118(%rsp)
movq %rax, 0x158(%rsp)
movq %rax, 0x168(%rsp)
movq %rax, 0x1a8(%rsp)
movq %rax, 0x1e8(%rsp)
movsd 0x18(%rdi), %xmm1
movsd %xmm1, 0x1b0(%rsp)
movsd %xmm1, 0x150(%rsp)
xorpd %xmm0, %xmm1
movlpd %xmm1, 0x1e0(%rsp)
movlpd %xmm1, 0x120(%rsp)
movsd 0x38(%rdi), %xmm1
movsd %xmm1, 0x1d8(%rsp)
movsd %xmm1, 0xe8(%rsp)
xorpd %xmm0, %xmm1
movlpd %xmm1, 0x178(%rsp)
movlpd %xmm1, 0x148(%rsp)
movsd 0x58(%rdi), %xmm1
movsd %xmm1, 0x170(%rsp)
movsd %xmm1, 0x110(%rsp)
xorpd %xmm0, %xmm1
movlpd %xmm1, 0x1a0(%rsp)
movlpd %xmm1, 0xe0(%rsp)
movsd 0x78(%rdi), %xmm0
movsd %xmm0, 0x180(%rsp)
movq %rax, 0x1b8(%rsp)
xorpd %xmm1, %xmm1
movupd %xmm1, 0x188(%rsp)
movsd %xmm0, 0x1c0(%rsp)
movq %rax, 0x1c8(%rsp)
movapd %xmm1, 0x1f0(%rsp)
movsd %xmm0, 0x200(%rsp)
movl %eax, 0x40(%rsp,%rax,4)
movl %eax, 0x70(%rsp,%rax,4)
incq %rax
cmpq $0x6, %rax
jne 0x2b90
movq %rdi, 0x58(%rsp)
movq %rdx, 0x60(%rsp)
xorl %eax, %eax
movq 0x10(%rcx,%rax), %rdx
movq %rdx, 0xa0(%rsp,%rax)
movupd (%rcx,%rax), %xmm0
movupd %xmm0, 0x90(%rsp,%rax)
addq $0x18, %rax
cmpq $0x30, %rax
jne 0x2bad
leaq 0xc8(%rsp), %rax
xorl %edx, %edx
leaq 0xc0(%rsp), %rsi
leaq 0xf8(%rsp), %rdi
movq %rsi, %r8
xorl %ebp, %ebp
movl (%rcx,%rdx,4), %r11d
movq %rdi, %rbx
movq %rax, %r14
movq %rdx, %r9
testl %r11d, %r11d
je 0x2c0a
incl %ebp
jmp 0x2ce3
cmpq $0x5, %r9
je 0x2ce3
movq %r14, %r13
movq %rbx, %r10
movl 0x4(%rcx,%r9,4), %r12d
incq %r9
addq $0x8, %r14
addq $0x38, %rbx
testl %r12d, %r12d
je 0x2c0a
incl %ebp
movl %r9d, %ebx
xorl %r14d, %r14d
movsd (%r13), %xmm0
movsd (%r8,%r14), %xmm1
movsd %xmm1, (%r13)
movsd %xmm0, (%r8,%r14)
addq $0x38, %r14
addq $0x38, %r13
cmpq $0x150, %r14            # imm = 0x150
jne 0x2c37
movl 0x40(%rsp,%rbx,4), %r14d
movl 0x40(%rsp,%rdx,4), %r13d
movl %r13d, 0x40(%rsp,%rbx,4)
movl %r14d, 0x40(%rsp,%rdx,4)
movsd 0x10(%rsp,%rdx,8), %xmm0
movsd 0x10(%rsp,%r9,8), %xmm1
movsd %xmm1, 0x10(%rsp,%rdx,8)
movsd %xmm0, 0x10(%rsp,%r9,8)
movl %r12d, (%rcx,%rdx,4)
movl %r11d, (%rcx,%r9,4)
movl 0x18(%rcx,%rdx,4), %r11d
movl 0x18(%rcx,%r9,4), %ebx
movl %ebx, 0x18(%rcx,%rdx,4)
movl %r11d, 0x18(%rcx,%r9,4)
xorl %r11d, %r11d
movsd (%r10), %xmm0
movsd (%rsi,%r11,8), %xmm1
movsd %xmm1, (%r10)
movsd %xmm0, (%rsi,%r11,8)
incq %r11
addq $0x8, %r10
cmpq $0x7, %r11
jne 0x2cac
movl 0x70(%rsp,%rdx,4), %r10d
movl 0x70(%rsp,%r9,4), %r11d
movl %r11d, 0x70(%rsp,%rdx,4)
movl %r10d, 0x70(%rsp,%r9,4)
incq %rdx
addq $0x8, %r8
addq $0x8, %rax
addq $0x38, %rsi
addq $0x38, %rdi
cmpq $0x6, %rdx
jne 0x2bf1
movl $0xffffffff, 0xc(%rsp)  # imm = 0xFFFFFFFF
movslq %ebp, %rax
leaq 0x1(%rax), %rcx
leaq 0xc0(%rsp), %rdx
xorl %esi, %esi
xorpd %xmm0, %xmm0
cmpl $0x5, %ebp
jg 0x2d43
movq %rcx, %rdi
movq %rcx, %r8
movsd -0x8(%rdx,%rdi,8), %xmm1
mulsd 0x8(%rsp,%rdi,8), %xmm1
addsd %xmm1, %xmm0
incq %r8
cmpl $0x6, %edi
movq %r8, %rdi
jne 0x2d28
imulq $0x38, %rsi, %rdi
addq %rsp, %rdi
addq $0xc0, %rdi
movsd 0x30(%rdi), %xmm1
subsd %xmm0, %xmm1
movsd %xmm1, (%rdi,%rax,8)
incq %rsi
addq $0x38, %rdx
cmpq $0x6, %rsi
jne 0x2d19
testl %ebp, %ebp
je 0x2dc2
leaq 0xc(%rsp), %rax
leaq 0x230(%rsp), %r10
leaq 0x6c(%rsp), %rbx
leaq 0x210(%rsp), %r11
leaq 0xc0(%rsp), %rdi
movl $0x6, %esi
movl $0x7, %edx
movl %ebp, %ecx
movl %ebp, %r8d
movl $0x1, %r9d
pushq %rax
pushq %r10
pushq %rbx
pushq %r11
callq 0x21f0
addq $0x20, %rsp
movl (%rbx), %r12d
cmpl %ebp, %r12d
jne 0x2f00
testl %ebp, %ebp
jle 0x2df8
movl %ebp, %eax
leaq (%rsp,%rax,8), %rcx
addq $0xc0, %rcx
xorl %edx, %edx
movsd (%rcx), %xmm0
movslq 0x210(%rsp,%rdx,4), %rsi
movslq 0x40(%rsp,%rsi,4), %rsi
movsd %xmm0, 0x10(%rsp,%rsi,8)
incq %rdx
addq $0x38, %rcx
cmpq %rdx, %rax
jne 0x2dd5
movl $0xc, %eax
xorl %ecx, %ecx
movq %r15, %rdi
movq 0x60(%rsp), %rdx
movq 0x58(%rsp), %rsi
movq %rcx, (%r15,%rax,8)
movq %rcx, (%rdi)
addq $0x28, %rdi
incq %rax
cmpq $0x10, %rax
jne 0x2e0c
cmpl $0x0, 0x90(%rsp)
je 0x2e44
movsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x48(%r15)
xorpd 0x71c2(%rip), %xmm0     # 0xa000
movlpd %xmm0, 0x30(%r15)
cmpl $0x0, 0x94(%rsp)
je 0x2e68
movsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x10(%r15)
xorpd 0x719e(%rip), %xmm0     # 0xa000
movlpd %xmm0, 0x40(%r15)
cmpl $0x0, 0x98(%rsp)
je 0x2e8c
movsd 0x20(%rsp), %xmm0
movsd %xmm0, 0x20(%r15)
xorpd 0x717a(%rip), %xmm0     # 0xa000
movlpd %xmm0, 0x8(%r15)
cmpl $0x0, 0x9c(%rsp)
je 0x2ea2
movsd 0x28(%rsp), %xmm0
movsd %xmm0, 0x18(%r15)
cmpl $0x0, 0xa0(%rsp)
je 0x2eb8
movsd 0x30(%rsp), %xmm0
movsd %xmm0, 0x38(%r15)
cmpl $0x0, 0xa4(%rsp)
je 0x2ece
movsd 0x38(%rsp), %xmm0
movsd %xmm0, 0x58(%r15)
movq %r15, %rdi
movl $0x4, %ecx
callq 0x2440
movl $0x1, %r12d
movl %r12d, %eax
addq $0x238, %rsp            # imm = 0x238
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
callq 0x2460
xorl %r12d, %r12d
jmp 0x2ee1
movq 0xb0f1(%rip), %rax      # 0xdff8
movq (%rax), %rcx
leaq 0x713d(%rip), %rdi      # 0xa04e
movl $0x24, %esi
movl $0x1, %edx
callq 0x23c0
negl %r12d
jmp 0x2ee1