// File: /workspace/github_c_makefile_star2_O3/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 $0x218, %rsp            # imm = 0x218
movq %rsi, %r15
movq %rdi, %r14
xorl %esi, %esi
movb $0x1, %al
xorl %r8d, %r8d
leaq (%rsi,%rsi,2), %rsi
leaq (%rcx,%rsi,8), %rsi
xorl %edi, %edi
cmpl $0x1, (%rsi,%rdi,4)
sbbl $-0x1, %r8d
incq %rdi
cmpq $0x6, %rdi
jne 0x28c4
movl $0x1, %esi
testb $0x1, %al
movl $0x0, %eax
jne 0x28ba
cmpl $0x6, %r8d
jne 0x2f0e
xorps %xmm0, %xmm0
movaps %xmm0, 0x40(%rsp)
movaps %xmm0, 0x30(%rsp)
movaps %xmm0, 0x20(%rsp)
movq 0x76f9(%rip), %xmm0     # 0xa000
leaq 0xd0(%rsp), %rax
xorl %edi, %edi
movdqa 0x76f7(%rip), %xmm1     # 0xa010
xorl %esi, %esi
movq %rdi, (%rax)
movq %rdi, 0x38(%rax)
movq %xmm0, 0x1f0(%rsp,%rsi,4)
addq $0x2, %rsi
paddd %xmm1, %xmm0
addq $0x70, %rax
cmpq $0x6, %rsi
jne 0x291b
cmpl $0x0, (%rcx)
jne 0x294e
movq 0x48(%r15), %xmm0
movq %xmm0, 0x20(%rsp)
cmpl $0x0, 0x4(%rcx)
jne 0x2960
movq 0x10(%r15), %xmm0
movq %xmm0, 0x28(%rsp)
cmpl $0x0, 0x8(%rcx)
jne 0x2972
movq 0x20(%r15), %xmm0
movq %xmm0, 0x30(%rsp)
cmpl $0x0, 0xc(%rcx)
jne 0x2984
movq 0x18(%r15), %xmm0
movq %xmm0, 0x38(%rsp)
cmpl $0x0, 0x10(%rcx)
jne 0x2996
movq 0x38(%r15), %xmm0
movq %xmm0, 0x40(%rsp)
cmpl $0x0, 0x14(%rcx)
jne 0x29a8
movq 0x58(%r15), %xmm0
movq %xmm0, 0x48(%rsp)
cmpl $0x0, 0x18(%rcx)
jne 0x29bc
movq 0x48(%rdx), %xmm0
movq %xmm0, 0xd0(%rsp)
cmpl $0x0, 0x1c(%rcx)
jne 0x29d0
movq 0x10(%rdx), %xmm0
movq %xmm0, 0x108(%rsp)
cmpl $0x0, 0x20(%rcx)
jne 0x29e4
movq 0x20(%rdx), %xmm0
movq %xmm0, 0x140(%rsp)
cmpl $0x0, 0x24(%rcx)
jne 0x29f8
movq 0x18(%rdx), %xmm0
movq %xmm0, 0x178(%rsp)
cmpl $0x0, 0x28(%rcx)
jne 0x2a0c
movq 0x38(%rdx), %xmm0
movq %xmm0, 0x1b0(%rsp)
cmpl $0x0, 0x2c(%rcx)
jne 0x2a20
movq 0x58(%rdx), %xmm0
movq %xmm0, 0x1e8(%rsp)
movq %rdx, 0x70(%rsp)
movsd 0x28(%r14), %xmm0
movsd 0x50(%r14), %xmm1
movapd %xmm0, %xmm2
addsd %xmm1, %xmm2
movsd %xmm2, 0xa0(%rsp)
movsd (%r14), %xmm2
addsd %xmm2, %xmm1
movsd %xmm1, 0xe0(%rsp)
addsd %xmm2, %xmm0
movsd %xmm0, 0x120(%rsp)
movdqu 0x8(%r14), %xmm1
movdqa 0x75b1(%rip), %xmm0     # 0xa020
pxor %xmm0, %xmm1
movq %xmm1, 0xd8(%rsp)
pshufd $0xee, %xmm1, %xmm2     # xmm2 = xmm1[2,3,2,3]
movq %xmm2, 0x110(%rsp)
movdqu %xmm1, 0xa8(%rsp)
movq 0x30(%r14), %xmm1
pxor %xmm0, %xmm1
movq %xmm1, 0x118(%rsp)
movq %xmm1, 0xe8(%rsp)
xorl %eax, %eax
movq %rax, 0xb8(%rsp)
movq %rax, 0xf8(%rsp)
movq %rax, 0x138(%rsp)
movq %rax, 0x148(%rsp)
movq %rax, 0x188(%rsp)
movq %rax, 0x1c8(%rsp)
movq 0x18(%r14), %xmm1
movq %xmm1, 0x190(%rsp)
movq %xmm1, 0x130(%rsp)
pxor %xmm0, %xmm1
movq %xmm1, 0x1c0(%rsp)
movq %xmm1, 0x100(%rsp)
movq 0x38(%r14), %xmm1
movq %xmm1, 0x1b8(%rsp)
movq %xmm1, 0xc8(%rsp)
pxor %xmm0, %xmm1
movq %xmm1, 0x158(%rsp)
movq %xmm1, 0x128(%rsp)
movq 0x58(%r14), %xmm1
movq %xmm1, 0x150(%rsp)
movq %xmm1, 0xf0(%rsp)
pxor %xmm0, %xmm1
movq %xmm1, 0x180(%rsp)
movq %xmm1, 0xc0(%rsp)
movsd 0x78(%r14), %xmm0
movsd %xmm0, 0x160(%rsp)
movq %rax, 0x198(%rsp)
pxor %xmm1, %xmm1
movdqu %xmm1, 0x168(%rsp)
movsd %xmm0, 0x1a0(%rsp)
movq %rax, 0x1a8(%rsp)
movdqa %xmm1, 0x1d0(%rsp)
movsd %xmm0, 0x1e0(%rsp)
movl %eax, 0x50(%rsp,%rax,4)
movl %eax, 0x80(%rsp,%rax,4)
incq %rax
cmpq $0x6, %rax
jne 0x2bb2
movl (%rcx), %eax
movl %eax, 0x4(%rsp)
movl 0x4(%rcx), %eax
movl %eax, 0x8(%rsp)
movl 0x8(%rcx), %eax
movl %eax, 0xc(%rsp)
movl 0xc(%rcx), %eax
movl %eax, 0x10(%rsp)
movl 0x10(%rcx), %eax
movl %eax, 0x14(%rsp)
movl 0x14(%rcx), %eax
movl %eax, 0x18(%rsp)
leaq 0xa8(%rsp), %rax
xorl %edx, %edx
leaq 0xa0(%rsp), %rsi
leaq 0xd8(%rsp), %rdi
movq %rsi, %r8
xorl %ebp, %ebp
cmpl $0x0, (%rcx,%rdx,4)
movq %rdi, %rbx
movq %rax, %r13
movq %rdx, %r9
je 0x2c24
incl %ebp
jmp 0x2d0b
cmpq $0x5, %r9
je 0x2d0b
movq %r13, %r12
movq %rbx, %r10
movl 0x4(%rcx,%r9,4), %r11d
incq %r9
addq $0x8, %r13
addq $0x38, %rbx
testl %r11d, %r11d
je 0x2c24
incl %ebp
movl %r9d, %r13d
xorl %ebx, %ebx
movsd (%r12), %xmm0
movq (%r8,%rbx), %xmm1
movq %xmm1, (%r12)
movsd %xmm0, (%r8,%rbx)
addq $0x38, %rbx
addq $0x38, %r12
cmpq $0x150, %rbx            # imm = 0x150
jne 0x2c50
movl 0x50(%rsp,%r13,4), %ebx
movl 0x50(%rsp,%rdx,4), %r12d
movl %r12d, 0x50(%rsp,%r13,4)
movl %ebx, 0x50(%rsp,%rdx,4)
movsd 0x20(%rsp,%rdx,8), %xmm0
movsd 0x20(%rsp,%r9,8), %xmm1
movsd %xmm1, 0x20(%rsp,%rdx,8)
movsd %xmm0, 0x20(%rsp,%r9,8)
movl %r11d, (%rcx,%rdx,4)
movl $0x0, (%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
movq (%rsi,%r11,8), %xmm1
movq %xmm1, (%r10)
movsd %xmm0, (%rsi,%r11,8)
incq %r11
addq $0x8, %r10
cmpq $0x7, %r11
jne 0x2cc8
movl 0x80(%rsp,%rdx,4), %r10d
movl 0x80(%rsp,%r9,4), %r11d
movl %r11d, 0x80(%rsp,%rdx,4)
movl %r10d, 0x80(%rsp,%r9,4)
incq %rdx
addq $0x8, %r8
addq $0x8, %rax
addq $0x38, %rsi
addq $0x38, %rdi
cmpq $0x6, %rdx
jne 0x2c0e
movl $0xffffffff, 0x1c(%rsp) # imm = 0xFFFFFFFF
movslq %ebp, %rax
leaq 0x1(%rax), %rcx
leaq 0xa0(%rsp), %rdx
xorl %esi, %esi
xorpd %xmm0, %xmm0
cmpl $0x5, %ebp
jg 0x2d6b
movq %rcx, %rdi
movq %rcx, %r8
movsd -0x8(%rdx,%rdi,8), %xmm1
mulsd 0x18(%rsp,%rdi,8), %xmm1
addsd %xmm1, %xmm0
incq %r8
cmpl $0x6, %edi
movq %r8, %rdi
jne 0x2d50
imulq $0x38, %rsi, %rdi
addq %rsp, %rdi
addq $0xa0, %rdi
movsd 0x30(%rdi), %xmm1
subsd %xmm0, %xmm1
movsd %xmm1, (%rdi,%rax,8)
incq %rsi
addq $0x38, %rdx
cmpq $0x6, %rsi
jne 0x2d41
testl %ebp, %ebp
je 0x2e24
leaq 0x1c(%rsp), %rax
leaq 0x210(%rsp), %r10
leaq 0x7c(%rsp), %rbx
leaq 0x1f0(%rsp), %r11
leaq 0xa0(%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 0x2f18
testl %ebp, %ebp
jle 0x2e24
movl %ebp, %eax
leaq (%rsp,%rax,8), %rcx
addq $0xa0, %rcx
xorl %edx, %edx
movsd (%rcx), %xmm0
movslq 0x1f0(%rsp,%rdx,4), %rsi
movslq 0x50(%rsp,%rsi,4), %rsi
movsd %xmm0, 0x20(%rsp,%rsi,8)
incq %rdx
addq $0x38, %rcx
cmpq %rdx, %rax
jne 0x2e01
movl $0xc, %eax
xorl %ecx, %ecx
movq %r15, %rdx
movq %rcx, (%r15,%rax,8)
movq %rcx, (%rdx)
addq $0x28, %rdx
incq %rax
cmpq $0x10, %rax
jne 0x2e2e
cmpl $0x0, 0x4(%rsp)
movq 0x70(%rsp), %rdx
je 0x2e68
movsd 0x20(%rsp), %xmm0
movsd %xmm0, 0x48(%r15)
xorpd 0x71be(%rip), %xmm0     # 0xa020
movlpd %xmm0, 0x30(%r15)
cmpl $0x0, 0x8(%rsp)
je 0x2e89
movsd 0x28(%rsp), %xmm0
movsd %xmm0, 0x10(%r15)
xorpd 0x719d(%rip), %xmm0     # 0xa020
movlpd %xmm0, 0x40(%r15)
cmpl $0x0, 0xc(%rsp)
je 0x2eaa
movsd 0x30(%rsp), %xmm0
movsd %xmm0, 0x20(%r15)
xorpd 0x717c(%rip), %xmm0     # 0xa020
movlpd %xmm0, 0x8(%r15)
cmpl $0x0, 0x10(%rsp)
je 0x2ebd
movsd 0x38(%rsp), %xmm0
movsd %xmm0, 0x18(%r15)
cmpl $0x0, 0x14(%rsp)
je 0x2ed0
movsd 0x40(%rsp), %xmm0
movsd %xmm0, 0x38(%r15)
cmpl $0x0, 0x18(%rsp)
je 0x2ee3
movsd 0x48(%rsp), %xmm0
movsd %xmm0, 0x58(%r15)
movq %r15, %rdi
movq %r14, %rsi
movl $0x4, %ecx
callq 0x2440
movl $0x1, %r12d
movl %r12d, %eax
addq $0x218, %rsp            # imm = 0x218
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
callq 0x2460
xorl %r12d, %r12d
jmp 0x2ef9
movq 0xb0d9(%rip), %rax      # 0xdff8
movq (%rax), %rcx
leaq 0x7175(%rip), %rdi      # 0xa09e
movl $0x24, %esi
movl $0x1, %edx
callq 0x23c0
negl %r12d
jmp 0x2ef9