//	DCAfuncts.cu
//
//This file contains all of the functions that are used during initialization, 
//assembly, and disassembly.

#include <iostream>
//Function Prototypes
//	All function called in this file are written in this file
void transpose(double S[6][6]);
void Mat66Mult(double A[6][6], double B[6][6], double C[6][6]);
void Mat61Mult(double A[6][6], double B[6][6], double C[6][6]);
void makeS(double S[6][6],double r[]);
void set_D(double D[6][6]);
void set_Dt(double D[6][6]);
void printm(double A[6][6]);

//get_X:
//	Function used to find the intermediate quantity X for a body
//	Considering the two bodies used to create X as b1 and b2:
//		z1 is a matrix containing z11 for b2
//		z2 is z22 for b1
//		D is the matrix in which X will be stored
void get_X(double z1[6][6], double z2 [6][6], double D[6][6])
{		
	for(int r = 0; r<6; r++)	//Loop through all 6 rows
	{
		for(int c=0; c<6; c++)	//Loop through all 6 columns
		{
    		z1[r][c]=z1[r][c]+z2[r][c];	//Add z1 and z2 together and store the solution in z1
		}
	}
	//Completing the above loop, z1 now holds b1.z22+b2.z11

    set_D(D);	//put the D matrix corresponding to a joint in this simulation in D
    Mat66Mult(z1,D,z1);	//Perform z1*D and store the solution in z1
	//z1 now holds (b1.z22+b2.z11)*D


    set_Dt(D);	//put the transpose of the D matrix in D
    Mat66Mult(D,z1,D); //Perform D*z1 and store the solution in D
	//D now holds Dt*(b1.z22+b2.z11)*D=X	
}

//set_D:
//	Function used to set the matrix that defines the subspace of prohibited motion.
//		D is the matrix in which the prohibited motion matrix will be stored.  
void set_D(double D[6][6])
{
	for(int r = 0; r<6; r++)	//Loop through all 6 rows
	{
		for(int c=0; c<6; c++)	//Loop through all 6 columns
		{
    		D[r][c]= 0;	//Set the D matrix to 0
		}
	}    
	
	//Set the D matrix according to the 2D joints used in this simulation
    D[0][0]=1;
    D[1][1]=1;
    D[3][2]=1;
    D[4][3]=1;
    D[5][4]=1;
}

//set_Dt:
//	Function used to set the transpose of the matrix that defines the subspace of prohibited motion.
//		D is the matrix in which the transposed prohibited motion matrix will be stored.  
void set_Dt(double D[6][6])
{
    for(int r = 0; r<6; r++)	//Loop through all 6 rows
	{
		for(int c=0; c<6; c++)	//Loop through all 6 columns
		{
    		D[r][c]= 0;	//Set the D matrix to 0;
		}
	}
   
	//Set the D matrix according to the 2D joints used in this simulation
    D[0][0]=1.0;
    D[1][1]=1.0;
    D[2][3]=1.0;
    D[3][4]=1.0;
    D[4][5]=1.0;
}

//invert_X:
//	Function used to invert the X matrix.  This function takes advantage of the nature of this 
//	2D simulation by only inverting the 2x2 matrix inside of X that will effect the solution.
//	This was done for efficiency of the simulation as well as ease of implementation in this case.
//		X is the intermediate quantity X; X inverse will be stored in X, replacing it
void invert_X(double X[6][6])
{
	//Variable Declarations
	double det;	//The determinant of the 2x2 matrix
	double Xinv[2][2];	//The inverse of the 2x2 matrix

	//Matrix inversion
	det=(X[2][2]*X[3][3])-(X[2][3]*X[3][2]);	//Find the determinant of the 2x2 matrix

	//Create the inverse matrix based on the trick for 2x2 matrices 
	Xinv[0][0]=X[3][3]/det;
	Xinv[0][1]=-1*X[2][3]/det;
	Xinv[1][0]=-1*X[3][2]/det;
	Xinv[1][1]=X[2][2]/det;

	for(int c=0; c<6; c++)
	{
		for(int r=0; r<6; r++)
		{
			X[r][c]=0;
		}
	}
	//Save the Solution
	for(int c = 0; c<2; c++)	//Loop through 2 columns
	{
		for(int r=0; r<2;r++)	//Loop through 2 rows
		{
			X[r+2][c+2]=Xinv[r][c];	//Save X inverse in X
		}
	}
}

//make_W:
//	Function used to create the intermediate quantity W
//		Xinv is Xinverse;  W will be stored here
//		D is an empty matrix used for matrix calculations
void make_W(double Xinv[6][6], double D[6][6])                    
{   
	for(int c =0; c< 5; c++)	//Loop through 5 columns
	{
		for(int r=0; r<5; r++)	//Loop through 5 rows
		{
			//Set the values in row and column 6 to 0
    		if(r==5 || c==5)	
			{
     			Xinv[r][c]=0;	
			}
		}
	}

	set_Dt(D);	//Put the D transpose matrix in D
    Mat66Mult(Xinv,D,Xinv); //Perform Xinv*D and store the solution in Xinv
	//Xinv now holds Xinv*Dt
	
    set_D(D);	//Put the D matrix in D
    Mat66Mult(D,Xinv,Xinv);	//Perform D*Xinv and store the solution in Xinv
	//Xinv now holds D*Xinv*Dt=W
}

//zaa:
//	Function used to calculate z11 and z22 during initialization
//		r is the necessary position vector (r01 for z11 and r02 for z22)
//		z is the matrix in which the solution will be stored
//		Minv is the inverse mass matrix
//		S is an empty matrix that is made into the shifter matrix corresponding to the r vector
void zaa(double r[],double z[6][6],double Minv[6][6],double S[6][6])
{
    makeS(S,r);	//Create the shifter matrix
   	Mat66Mult(Minv,S,z);	//Perform Minv*S and store the solution in z
	//z now holds Minv*S
	

    transpose(S);	//Transpose the shifter matrix
	
    Mat66Mult(S,z,z);	//Perform S*z and store the solution in z
	//z now holds St*Minv*S=zaa
	
    Mat66Mult(S,Minv,Minv);	//Perform S*Minv and store the solution in Minv
	//Minv now holds S*Minv, this is done becuase this values is needed later
}

//zab:
//	Function used to calculate z12 and z21 during initialization
//		r is the necessary position vector (r01 for z21 and r02 for z12)
//		z is an empty matrix in which the solution will be stored
//		Minv is the necessary shifter matrix multiplied by the inverse mass matrix
//		S is an empty matrix to be made into the shifter matrix corresponding to the r vector
void zab(double r[],double z[6][6],double Minv[6][6],double S[6][6])
{    
   makeS(S,r);	//Make the shifter matrix

	//The following loop ensures that values that were not set by makeS are set to 0
   for(int c =0; c< 3; c++)
	{
		for(int r=3; r<6; r++)
		{
     			S[r][c]=0;
		}
	}

    Mat66Mult(Minv,S,z);	//Perform Minv*S and store the solution in z
	//z now holds Minv*S=zab
}

//makeS:
//	Function used to create a shifter matrix with the given position vector r
//		S is the matrix in which the shifter matrix will be stored
//		r is the position vector correspoding to the desired shifter matrix
void makeS(double S[6][6],double r[])
{
	//Set the top right 3x3 matrix in S to the skew matrix of r
    S[0][4]=-1*r[2];
    S[0][5]=r[1];
    S[1][3]=r[2];
    S[1][5]=-1*r[0];
    S[2][3]=-1*r[1];
    S[2][4]=r[0];    
}

void makeSt(double S[6][6],double r[])
{
	//Set the top right 3x3 matrix in S to the skew matrix of r
    S[4][0]=-1*r[2];
    S[5][0]=r[1];
    S[3][1]=r[2];
    S[5][1]=-1*r[0];
    S[3][2]=-1*r[1];
    S[4][2]=r[0];    
}

//MSsetup:
//	Function used to setup the inverse mass matrix and set the shifter matrix to the identity matrix.
//	This function takes advantage of the 2D nature of this simulation by assuming that 
//	the mass matrix will always be diagonal.
//		Minv is the matrix in which the inverse mass matrix will be stored
//		S is the matrix that must be set to the identity matrix to allow for population later
//		m is the mass of the body
//		I is the inertia tensor of the body
void MSsetup(double Minv[6][6],double S[6][6],double m, double I[3][3])
{
   for(int c =0; c< 6; c++)	//Loop through 6 columns
	{
		for(int r=0; r<6; r++)	//Loop through 6 rows
		{
			S[r][c]=0;	//Set the S matrix to 0

			//If the row is less than 3, and the current iteration is on the diagonal of the matrix,
			//then insert the inverse of the inertia tensor as it is needed in M inverse
			if (r<3 && c==r)
			{
    			Minv[r][c]=(1/(I[r][c]));
			}
			//If the row is greater than 2, and the current iteration is on the diagonal of the 
			//of the matrix, then insert 1/m as it is needed in M inverse
			else if(r>2 && c==r)
			{
				Minv[r][c]=(1/m);
			}
			//If not on the diagonal, set that element in M inverse to 0
			else
			{
				Minv[r][c]=0;
			}
			//If on the diagonal, set that element in S to 1
			if(c==r)
			{
				S[r][c]=1;
			}
		}
	}

}

//Fa:
//	Function used to find the state dependant force vector Fa
//		S is a matrix whose first column will hold Fa
//		w is the angular velocity of the body
//		r is the corresponding position vector (r01 or r02)
//		m is the mass of the body
void Fa(double S[6][6],double w, double r[], double m)
{
 	double g = 9.81;	//Set the gravitational constant
	
	//Loop through the first column of S and set it to 0
    for(int rr=0; rr<6; rr++)
	{
         S[rr][0]=0;
    }
	
	//Create Fa based on the force of gravity and the centripedal force on the body
    S[3][0]=-m*w*w*r[0];
    S[4][0]=-g*m-m*w*w*r[1];
}

//r02:
//	Function used to find the r02 vector of the body.  This function takes advantage of the
//	2D nature of this simulation by removing the need for a direction cosine matrix
//		r is the vector in which r02 will be stored
//		angle is the ange of the body with respect to the vertical
//		L is the length of the body
void r02(double r[],double L)
{
    r[0]=0;
    r[1]=-L/2;
    r[2]=0;

}

//r01:
//	Funtion used to find the r01 position vector of the body. This function takes advantage of the
//	2D nature of this simulation by removing the need for a direction cosine matrix
//		r is the vector in which r02 will be stored
//		angle is the ange of the body with respect to the vertical
void r01(double r[], double L)
{
    r[0]=0;
    r[1]=L/2;
    r[2]=0;
}

//transpose:
//	Function used to find the transpose of a 6x6 shifter matrix
//		S is the shifter matrix to be transposed.  The transposed matrix will also be stored here.
void transpose(double S[6][6])
{
	double S2[6][6];	//Temporary matrix used to store values
	
	//Loop through every row and column and save S transpose into S2
	for(int r =0; r< 6; r++)
	{
		for(int c=0; c<6; c++)
		{
			S2[r][c]=S[c][r];
		}
	}

	//Loop through every row and column and save S transpose into S
	for(int r =0; r< 6; r++)
	{
		for(int c=0; c<6; c++)
		{
			S[r][c]=S2[r][c];
		}
	}
}

//Mat66Mult:
//	Function used to multipy two 6x6 matrices.  Note that a temporary 
//	matrix is used to first store the solution. This allows for this 
//	function to store the solution into one of the matrices being multiplied.
//	The form of the operation is A*B=C
//		A is the left matrix
//		B is the right matrix
//		C is the matrix in which the solution is stored
void Mat66Mult(double A[6][6], double B[6][6], double C[6][6])
{
	double j[6][6];	//Temporary matrix used to store the solution

   	for(int r =0; r< 6; r++)	//Loop through all 6 rows
	{
		for(int c=0; c<6; c++)	//Loop through all 6 columns
		{	
			j[r][c]=0;	//Initialize the current element in the temporary matrix to 0

			//The loop below cycles through the row of the first matrix, and the column of the
			//second corresponding to the current row and column of the temporary matrix.  It 
			//takes the dot product of the row and column and stores it in j.
			for(int l =0; l<6; l++)
			{	
				j[r][c]=j[r][c]+(A[r][l]*B[l][c]);
			}
		}
	}

	//Loop through the rows and columns of the temporary matrix, saving the solution into C
	for(int r=0; r<6; r++)
	{
		for(int c=0; c<6; c++)
		{
			C[r][c]=j[r][c];
		}
	}
}

//Mat61Mult:
//	Function used to multipy a 6x6 and a 6x1 matrix.  Note that a temporary 
//	matrix is used to first store the solution. This allows for this function 
//	to store the solution into one of the matrices being multiplied.  It should
//	also be noted that this function takes only 6x6 matrices, and the 6x1 matrix 
//	to be multiplied is assumed to be held in the first column of matrix B.
//	The form of the operation is A*B=C
//		A is the 6x6 matrix
//		B is a 6x6 matrix with the 6x1 matrix stored in its first column
//		C is the matrix in which the solution is stored.  It is stored only in the first column
void Mat61Mult(double A[6][6], double B[6][6], double C[6][6])
{
	double j[6];	//Temporary matrix
 
	for(int r =0; r< 6; r++)	//Loop through all 6 rows
	{	
		j[r]=0;	//Initialize this element of j to 0
		
		//The loop below cycles through the row of the first matrix, and the column of the
		//second corresponding to the current row of the temporary matrix.  It 
		//takes the dot product of the row and column and stores it in j.
		for(int c=0; c<6; c++)
		{
			j[r]+=A[r][c]*B[c][0];
		}
	}	
	
	//Loop through the rows of the temporary matrix, saving the solution into C		
	for(int r = 0; r< 6; r++)
	{
		C[r][0]=j[r];
	}
        
}

void update(double R[6][6], double Rt[6][6], double zeta1[], double zeta2[], int n, int bodynum, int zetanum)
{
	double temp[6][6];
	for(int r =0; r<6; r++)
	{
		for(int c =0; c<6; c++)
		{
			temp[r][c]=0;
			for(int i =0; i<6; i++)
			{
				temp[r][c]+=zeta1[i+r*26*n+26*bodynum+zetanum]*Rt[i][c];
			}
			//std::cout<<Rt[r][c]<< "  ";
		}
		//std::cout<<std::endl;
	}

	for(int r =0; r<6; r++)
	{
		for(int c =0; c<6; c++)
		{
			zeta2[c+r*26*n+26*bodynum+zetanum]=0;
			for(int i =0; i<6; i++)
			{
				zeta2[c+r*26*n+26*bodynum+zetanum]+=R[r][i]*temp[i][c];
			}
		}
	}
}

void savez3(double Minv[6][6],double S[6][6] ,double Fa[], double nZetas[], int n, int bodynum, int zetanum)
{
	double temp[6][6];
	Mat66Mult(S,Minv,temp);
	for(int r =0; r<6; r++)
	{
		nZetas[r*26*n+bodynum*26+zetanum]=0;
		for(int c =0; c<6; c++)
		{
			nZetas[r*26*n+bodynum*26+zetanum]+=temp[r][c]*Fa[c];
		}
	}
}

void makeR(double R[6][6], double Rt[6][6], double angle)
{
	//std::cout<<angle<<std::endl;
	for(int r =0; r<6; r++)
	{
		for(int c =0; c<6; c++)
		{
			R[r][c]=0;
			Rt[r][c]=0;
		}
		R[r][r]=1;
		Rt[r][r]=1;
	}
	//std::cout<<std::endl<<cos(angle)<<std::endl;
	R[3][3]=cos(angle);
	R[4][4]=cos(angle);
	Rt[3][3]=cos(angle);
	Rt[4][4]=cos(angle);
	R[3][4]=-sin(angle);
	Rt[3][4]=sin(angle);
	R[4][3]=sin(angle);
	Rt[4][3]=-sin(angle);

}


			


	.file	"tmpxft_0027028f_00000000-6_DCAfuncts.cudafe1.cpp"
	.text
#APP
	.globl _ZSt21ios_base_library_initv

#NO_APP
	.type	_ZL26__cudaUnregisterBinaryUtilv, @function
_ZL26__cudaUnregisterBinaryUtilv:
.LFB3656:
	.cfi_startproc
	endbr64
	movq	_ZL20__cudaFatCubinHandle(%rip), %rdi
	jmp	__cudaUnregisterFatBinary@PLT
	.cfi_endproc
.LFE3656:
	.size	_ZL26__cudaUnregisterBinaryUtilv, .-_ZL26__cudaUnregisterBinaryUtilv
	.globl	_Z5set_DPA6_d
	.type	_Z5set_DPA6_d, @function
_Z5set_DPA6_d:
.LFB3636:
	.cfi_startproc
	endbr64
	movsd	.LC0(%rip), %xmm0
	movq	%rdi, %rdx
	movl	$72, %ecx
	xorl	%eax, %eax
	rep stosl
	movsd	%xmm0, (%rdx)
	movsd	%xmm0, 56(%rdx)
	movsd	%xmm0, 160(%rdx)
	movsd	%xmm0, 216(%rdx)
	movsd	%xmm0, 272(%rdx)
	ret
	.cfi_endproc
.LFE3636:
	.size	_Z5set_DPA6_d, .-_Z5set_DPA6_d
	.globl	_Z6set_DtPA6_d
	.type	_Z6set_DtPA6_d, @function
_Z6set_DtPA6_d:
.LFB3637:
	.cfi_startproc
	endbr64
	movsd	.LC0(%rip), %xmm0
	movq	%rdi, %rdx
	movl	$72, %ecx
	xorl	%eax, %eax
	rep stosl
	movsd	%xmm0, (%rdx)
	movsd	%xmm0, 56(%rdx)
	movsd	%xmm0, 120(%rdx)
	movsd	%xmm0, 176(%rdx)
	movsd	%xmm0, 232(%rdx)
	ret
	.cfi_endproc
.LFE3637:
	.size	_Z6set_DtPA6_d, .-_Z6set_DtPA6_d
	.globl	_Z8invert_XPA6_d
	.type	_Z8invert_XPA6_d, @function
_Z8invert_XPA6_d:
.LFB3638:
	.cfi_startproc
	endbr64
	movsd	112(%rdi), %xmm3
	movsd	120(%rdi), %xmm0
	movq	%rdi, %rax
	leaq	48(%rdi), %rsi
	movsd	168(%rdi), %xmm4
	movsd	160(%rdi), %xmm1
	movaps	%xmm3, %xmm2
	movaps	%xmm0, %xmm5
	mulsd	%xmm1, %xmm5
	mulsd	%xmm4, %xmm2
	subsd	%xmm5, %xmm2
	movq	.LC1(%rip), %xmm5
	xorps	%xmm5, %xmm0
	xorps	%xmm5, %xmm1
	divsd	%xmm2, %xmm4
	divsd	%xmm2, %xmm0
	divsd	%xmm2, %xmm1
	divsd	%xmm2, %xmm3
.L5:
	movq	%rax, %rcx
	movl	$6, %edx
.L6:
	movq	$0x000000000, (%rcx)
	addq	$48, %rcx
	decl	%edx
	jne	.L6
	addq	$8, %rax
	cmpq	%rsi, %rax
	jne	.L5
	movsd	%xmm4, 112(%rdi)
	movsd	%xmm1, 160(%rdi)
	movsd	%xmm0, 120(%rdi)
	movsd	%xmm3, 168(%rdi)
	ret
	.cfi_endproc
.LFE3638:
	.size	_Z8invert_XPA6_d, .-_Z8invert_XPA6_d
	.globl	_Z5makeSPA6_dPd
	.type	_Z5makeSPA6_dPd, @function
_Z5makeSPA6_dPd:
.LFB3642:
	.cfi_startproc
	endbr64
	movsd	16(%rsi), %xmm0
	movq	.LC1(%rip), %xmm1
	xorps	%xmm1, %xmm0
	movsd	%xmm0, 32(%rdi)
	movsd	8(%rsi), %xmm0
	movsd	%xmm0, 40(%rdi)
	movsd	16(%rsi), %xmm0
	movsd	%xmm0, 72(%rdi)
	movsd	(%rsi), %xmm0
	xorps	%xmm1, %xmm0
	movsd	%xmm0, 88(%rdi)
	movsd	8(%rsi), %xmm0
	xorps	%xmm1, %xmm0
	movsd	%xmm0, 120(%rdi)
	movsd	(%rsi), %xmm0
	movsd	%xmm0, 128(%rdi)
	ret
	.cfi_endproc
.LFE3642:
	.size	_Z5makeSPA6_dPd, .-_Z5makeSPA6_dPd
	.globl	_Z6makeStPA6_dPd
	.type	_Z6makeStPA6_dPd, @function
_Z6makeStPA6_dPd:
.LFB3643:
	.cfi_startproc
	endbr64
	movsd	16(%rsi), %xmm0
	movq	.LC1(%rip), %xmm1
	xorps	%xmm1, %xmm0
	movsd	%xmm0, 192(%rdi)
	movsd	8(%rsi), %xmm0
	movsd	%xmm0, 240(%rdi)
	movsd	16(%rsi), %xmm0
	movsd	%xmm0, 152(%rdi)
	movsd	(%rsi), %xmm0
	xorps	%xmm1, %xmm0
	movsd	%xmm0, 248(%rdi)
	movsd	8(%rsi), %xmm0
	xorps	%xmm1, %xmm0
	movsd	%xmm0, 160(%rdi)
	movsd	(%rsi), %xmm0
	movsd	%xmm0, 208(%rdi)
	ret
	.cfi_endproc
.LFE3643:
	.size	_Z6makeStPA6_dPd, .-_Z6makeStPA6_dPd
	.globl	_Z7MSsetupPA6_dS0_dPA3_d
	.type	_Z7MSsetupPA6_dS0_dPA3_d, @function
_Z7MSsetupPA6_dS0_dPA3_d:
.LFB3644:
	.cfi_startproc
	endbr64
	movsd	.LC0(%rip), %xmm1
	movq	%rdi, %r8
	xorl	%r9d, %r9d
	xorl	%edi, %edi
.L13:
	leaq	(%rdx,%r9), %r10
	movq	%r9, %rcx
	xorl	%eax, %eax
.L18:
	cmpl	%eax, %edi
	movq	$0x000000000, (%rsi,%rcx)
	sete	%r11b
	cmpl	$2, %eax
	jg	.L34
	testb	%r11b, %r11b
	je	.L14
	movaps	%xmm1, %xmm2
	divsd	(%r10), %xmm2
	jmp	.L35
.L14:
	cmpl	$2, %eax
	jle	.L16
.L34:
	testb	%r11b, %r11b
	je	.L16
	movaps	%xmm1, %xmm2
	divsd	%xmm0, %xmm2
.L35:
	movsd	%xmm2, (%r8,%rcx)
	jmp	.L15
.L16:
	movq	$0x000000000, (%r8,%rcx)
	xorps	%xmm2, %xmm2
	cmpl	%eax, %edi
	jne	.L17
.L15:
	movaps	%xmm1, %xmm2
.L17:
	incl	%eax
	movsd	%xmm2, (%rsi,%rcx)
	addq	$24, %r10
	addq	$48, %rcx
	cmpl	$6, %eax
	jne	.L18
	incl	%edi
	addq	$8, %r9
	cmpl	$6, %edi
	jne	.L13
	ret
	.cfi_endproc
.LFE3644:
	.size	_Z7MSsetupPA6_dS0_dPA3_d, .-_Z7MSsetupPA6_dS0_dPA3_d
	.globl	_Z2FaPA6_ddPdd
	.type	_Z2FaPA6_ddPdd, @function
_Z2FaPA6_ddPdd:
.LFB3645:
	.cfi_startproc
	endbr64
	movaps	%xmm0, %xmm2
	movq	%rdi, %rax
	leaq	288(%rdi), %rdx
.L37:
	movq	$0x000000000, (%rax)
	addq	$48, %rax
	cmpq	%rdx, %rax
	jne	.L37
	movaps	%xmm1, %xmm0
	xorps	.LC1(%rip), %xmm0
	mulsd	.LC3(%rip), %xmm1
	mulsd	%xmm2, %xmm0
	mulsd	%xmm2, %xmm0
	movsd	(%rsi), %xmm2
	mulsd	%xmm0, %xmm2
	movsd	%xmm2, 144(%rdi)
	mulsd	8(%rsi), %xmm0
	addsd	%xmm1, %xmm0
	movsd	%xmm0, 192(%rdi)
	ret
	.cfi_endproc
.LFE3645:
	.size	_Z2FaPA6_ddPdd, .-_Z2FaPA6_ddPdd
	.globl	_Z3r02Pdd
	.type	_Z3r02Pdd, @function
_Z3r02Pdd:
.LFB3646:
	.cfi_startproc
	endbr64
	mulsd	.LC5(%rip), %xmm0
	movq	$0x000000000, (%rdi)
	movq	$0x000000000, 16(%rdi)
	movsd	%xmm0, 8(%rdi)
	ret
	.cfi_endproc
.LFE3646:
	.size	_Z3r02Pdd, .-_Z3r02Pdd
	.globl	_Z3r01Pdd
	.type	_Z3r01Pdd, @function
_Z3r01Pdd:
.LFB3647:
	.cfi_startproc
	endbr64
	mulsd	.LC4(%rip), %xmm0
	movq	$0x000000000, (%rdi)
	movq	$0x000000000, 16(%rdi)
	movsd	%xmm0, 8(%rdi)
	ret
	.cfi_endproc
.LFE3647:
	.size	_Z3r01Pdd, .-_Z3r01Pdd
	.globl	_Z9transposePA6_d
	.type	_Z9transposePA6_d, @function
_Z9transposePA6_d:
.LFB3648:
	.cfi_startproc
	endbr64
	subq	$312, %rsp
	.cfi_def_cfa_offset 320
	movq	%rdi, %rax
	movq	%rdi, %rcx
	movq	%fs:40, %rdx
	movq	%rdx, 296(%rsp)
	xorl	%edx, %edx
	leaq	8(%rsp), %rdi
	leaq	48(%rax), %r9
	movq	%rdi, %rsi
.L42:
	movq	%rcx, %r8
	xorl	%edx, %edx
.L43:
	movsd	(%r8), %xmm0
	addq	$48, %r8
	movsd	%xmm0, (%rdi,%rdx,8)
	incq	%rdx
	cmpq	$6, %rdx
	jne	.L43
	addq	$8, %rcx
	addq	$48, %rdi
	cmpq	%r9, %rcx
	jne	.L42
	movl	$72, %ecx
	movq	%rax, %rdi
	rep movsl
	movq	296(%rsp), %rax
	subq	%fs:40, %rax
	je	.L44
	call	__stack_chk_fail@PLT
.L44:
	addq	$312, %rsp
	.cfi_def_cfa_offset 8
	ret
	.cfi_endproc
.LFE3648:
	.size	_Z9transposePA6_d, .-_Z9transposePA6_d
	.globl	_Z9Mat66MultPA6_dS0_S0_
	.type	_Z9Mat66MultPA6_dS0_S0_, @function
_Z9Mat66MultPA6_dS0_S0_:
.LFB3649:
	.cfi_startproc
	endbr64
	pushq	%rbx
	.cfi_def_cfa_offset 16
	.cfi_offset 3, -16
	movq	%rsi, %r9
	movq	%rdx, %r8
	subq	$304, %rsp
	.cfi_def_cfa_offset 320
	movq	%fs:40, %rax
	movq	%rax, 296(%rsp)
	xorl	%eax, %eax
	leaq	8(%rsp), %rsi
.L50:
	leaq	(%rdi,%rax), %rbx
	xorl	%edx, %edx
	leaq	(%rsi,%rax), %r11
.L52:
	leaq	(%r9,%rdx), %r10
	xorl	%ecx, %ecx
	xorps	%xmm0, %xmm0
.L51:
	movsd	(%rbx,%rcx,8), %xmm1
	mulsd	(%r10), %xmm1
	incq	%rcx
	addq	$48, %r10
	addsd	%xmm1, %xmm0
	cmpq	$6, %rcx
	jne	.L51
	movsd	%xmm0, (%r11,%rdx)
	addq	$8, %rdx
	cmpq	$48, %rdx
	jne	.L52
	addq	$48, %rax
	cmpq	$288, %rax
	jne	.L50
	movl	$72, %ecx
	movq	%r8, %rdi
	rep movsl
	movq	296(%rsp), %rax
	subq	%fs:40, %rax
	je	.L53
	call	__stack_chk_fail@PLT
.L53:
	addq	$304, %rsp
	.cfi_def_cfa_offset 16
	popq	%rbx
	.cfi_def_cfa_offset 8
	ret
	.cfi_endproc
.LFE3649:
	.size	_Z9Mat66MultPA6_dS0_S0_, .-_Z9Mat66MultPA6_dS0_S0_
	.globl	_Z5get_XPA6_dS0_S0_
	.type	_Z5get_XPA6_dS0_S0_, @function
_Z5get_XPA6_dS0_S0_:
.LFB3635:
	.cfi_startproc
	endbr64
	pushq	%rbp
	.cfi_def_cfa_offset 16
	.cfi_offset 6, -16
	movq	%rdi, %rbp
	pushq	%rbx
	.cfi_def_cfa_offset 24
	.cfi_offset 3, -24
	movq	%rdx, %rbx
	xorl	%edx, %edx
	pushq	%rcx
	.cfi_def_cfa_offset 32
.L60:
	leaq	0(%rbp,%rdx), %rcx
	leaq	(%rsi,%rdx), %rdi
	xorl	%eax, %eax
.L61:
	movsd	(%rcx,%rax), %xmm0
	addsd	(%rdi,%rax), %xmm0
	movsd	%xmm0, (%rcx,%rax)
	addq	$8, %rax
	cmpq	$48, %rax
	jne	.L61
	addq	$48, %rdx
	cmpq	$288, %rdx
	jne	.L60
	movq	%rbx, %rdi
	movq	%rbx, %rsi
	call	_Z5set_DPA6_d
	movq	%rbp, %rdx
	movq	%rbp, %rdi
	call	_Z9Mat66MultPA6_dS0_S0_
	movq	%rbx, %rdi
	movq	%rbp, %rsi
	call	_Z6set_DtPA6_d
	popq	%rax
	.cfi_def_cfa_offset 24
	movq	%rbx, %rdx
	movq	%rbx, %rdi
	popq	%rbx
	.cfi_def_cfa_offset 16
	popq	%rbp
	.cfi_def_cfa_offset 8
	jmp	_Z9Mat66MultPA6_dS0_S0_
	.cfi_endproc
.LFE3635:
	.size	_Z5get_XPA6_dS0_S0_, .-_Z5get_XPA6_dS0_S0_
	.globl	_Z6make_WPA6_dS0_
	.type	_Z6make_WPA6_dS0_, @function
_Z6make_WPA6_dS0_:
.LFB3639:
	.cfi_startproc
	endbr64
	pushq	%rbp
	.cfi_def_cfa_offset 16
	.cfi_offset 6, -16
	movq	%rdi, %rbp
	movq	%rsi, %rdi
	pushq	%rbx
	.cfi_def_cfa_offset 24
	.cfi_offset 3, -24
	movq	%rsi, %rbx
	pushq	%rax
	.cfi_def_cfa_offset 32
	call	_Z6set_DtPA6_d
	movq	%rbp, %rdx
	movq	%rbp, %rdi
	call	_Z9Mat66MultPA6_dS0_S0_
	movq	%rbx, %rdi
	movq	%rbp, %rsi
	call	_Z5set_DPA6_d
	popq	%rcx
	.cfi_def_cfa_offset 24
	movq	%rbp, %rdx
	movq	%rbx, %rdi
	popq	%rbx
	.cfi_def_cfa_offset 16
	popq	%rbp
	.cfi_def_cfa_offset 8
	jmp	_Z9Mat66MultPA6_dS0_S0_
	.cfi_endproc
.LFE3639:
	.size	_Z6make_WPA6_dS0_, .-_Z6make_WPA6_dS0_
	.globl	_Z3zaaPdPA6_dS1_S1_
	.type	_Z3zaaPdPA6_dS1_S1_, @function
_Z3zaaPdPA6_dS1_S1_:
.LFB3640:
	.cfi_startproc
	endbr64
	pushq	%r12
	.cfi_def_cfa_offset 16
	.cfi_offset 12, -16
	movq	%rsi, %r12
	movq	%rdi, %rsi
	movq	%rcx, %rdi
	pushq	%rbp
	.cfi_def_cfa_offset 24
	.cfi_offset 6, -24
	movq	%rdx, %rbp
	movq	%r12, %rdx
	pushq	%rbx
	.cfi_def_cfa_offset 32
	.cfi_offset 3, -32
	movq	%rcx, %rbx
	call	_Z5makeSPA6_dPd
	movq	%rcx, %rsi
	movq	%rbp, %rdi
	call	_Z9Mat66MultPA6_dS0_S0_
	movq	%rbx, %rdi
	call	_Z9transposePA6_d
	movq	%r12, %rdx
	movq	%r12, %rsi
	movq	%rbx, %rdi
	call	_Z9Mat66MultPA6_dS0_S0_
	movq	%rbp, %rdx
	movq	%rbp, %rsi
	movq	%rbx, %rdi
	popq	%rbx
	.cfi_def_cfa_offset 24
	popq	%rbp
	.cfi_def_cfa_offset 16
	popq	%r12
	.cfi_def_cfa_offset 8
	jmp	_Z9Mat66MultPA6_dS0_S0_
	.cfi_endproc
.LFE3640:
	.size	_Z3zaaPdPA6_dS1_S1_, .-_Z3zaaPdPA6_dS1_S1_
	.globl	_Z3zabPdPA6_dS1_S1_
	.type	_Z3zabPdPA6_dS1_S1_, @function
_Z3zabPdPA6_dS1_S1_:
.LFB3641:
	.cfi_startproc
	endbr64
	pushq	%rax
	.cfi_def_cfa_offset 16
	movq	%rcx, %r8
	movq	%rsi, %r10
	movq	%rdx, %r9
	movq	%rdi, %rsi
	leaq	144(%rcx), %rdx
	movq	%rcx, %rdi
	xorl	%eax, %eax
	movl	$6, %ecx
	call	_Z5makeSPA6_dPd
	movq	%rdx, %rdi
	leaq	192(%r8), %rdx
	movq	%r8, %rsi
	rep stosl
	movq	%rdx, %rdi
	movl	$6, %ecx
	leaq	240(%r8), %rdx
	rep stosl
	movq	%rdx, %rdi
	movl	$6, %ecx
	movq	%r10, %rdx
	rep stosl
	movq	%r9, %rdi
	popq	%rcx
	.cfi_def_cfa_offset 8
	jmp	_Z9Mat66MultPA6_dS0_S0_
	.cfi_endproc
.LFE3641:
	.size	_Z3zabPdPA6_dS1_S1_, .-_Z3zabPdPA6_dS1_S1_
	.globl	_Z9Mat61MultPA6_dS0_S0_
	.type	_Z9Mat61MultPA6_dS0_S0_, @function
_Z9Mat61MultPA6_dS0_S0_:
.LFB3650:
	.cfi_startproc
	endbr64
	subq	$72, %rsp
	.cfi_def_cfa_offset 80
	leaq	288(%rdi), %r9
	movq	%fs:40, %rax
	movq	%rax, 56(%rsp)
	xorl	%eax, %eax
	leaq	8(%rsp), %rcx
	movq	%rcx, %r8
.L73:
	xorl	%eax, %eax
	xorps	%xmm0, %xmm0
.L72:
	imulq	$48, %rax, %r10
	movsd	(%rsi,%r10), %xmm1
	mulsd	(%rdi,%rax,8), %xmm1
	incq	%rax
	addsd	%xmm1, %xmm0
	cmpq	$6, %rax
	jne	.L72
	addq	$48, %rdi
	movsd	%xmm0, (%rcx)
	addq	$8, %rcx
	cmpq	%r9, %rdi
	jne	.L73
	xorl	%eax, %eax
.L74:
	imulq	$48, %rax, %rcx
	movsd	(%r8,%rax,8), %xmm0
	incq	%rax
	movsd	%xmm0, (%rdx,%rcx)
	cmpq	$6, %rax
	jne	.L74
	movq	56(%rsp), %rax
	subq	%fs:40, %rax
	je	.L75
	call	__stack_chk_fail@PLT
.L75:
	addq	$72, %rsp
	.cfi_def_cfa_offset 8
	ret
	.cfi_endproc
.LFE3650:
	.size	_Z9Mat61MultPA6_dS0_S0_, .-_Z9Mat61MultPA6_dS0_S0_
	.globl	_Z6updatePA6_dS0_PdS1_iii
	.type	_Z6updatePA6_dS0_PdS1_iii, @function
_Z6updatePA6_dS0_PdS1_iii:
.LFB3651:
	.cfi_startproc
	endbr64
	pushq	%r13
	.cfi_def_cfa_offset 16
	.cfi_offset 13, -16
	movq	%rsi, %r11
	imull	$26, %r9d, %esi
	movslq	%r9d, %r9
	pushq	%r12
	.cfi_def_cfa_offset 24
	.cfi_offset 12, -24
	imulq	$26, %r9, %r9
	pushq	%rbp
	.cfi_def_cfa_offset 32
	.cfi_offset 6, -32
	pushq	%rbx
	.cfi_def_cfa_offset 40
	.cfi_offset 3, -40
	movq	%rcx, %rbx
	subq	$312, %rsp
	.cfi_def_cfa_offset 352
	movq	%fs:40, %rax
	movq	%rax, 296(%rsp)
	xorl	%eax, %eax
	movslq	%r8d, %rax
	leaq	8(%rsp), %r10
	movslq	352(%rsp), %r8
	imulq	$208, %rax, %rax
	leaq	296(%rsp), %r13
	movq	%r10, %rcx
	addq	%r8, %r9
	leaq	(%rdx,%r9,8), %rbp
.L81:
	xorl	%edx, %edx
.L83:
	leaq	(%r11,%rdx), %r12
	xorl	%r9d, %r9d
	xorps	%xmm0, %xmm0
.L82:
	movsd	0(%rbp,%r9,8), %xmm1
	mulsd	(%r12), %xmm1
	incq	%r9
	addq	$48, %r12
	addsd	%xmm1, %xmm0
	cmpq	$6, %r9
	jne	.L82
	movsd	%xmm0, (%rcx,%rdx)
	addq	$8, %rdx
	cmpq	$48, %rdx
	jne	.L83
	addq	$48, %rcx
	addq	%rax, %rbp
	cmpq	%r13, %rcx
	jne	.L81
	movslq	%esi, %rsi
	movq	%rdi, %rcx
	leaq	288(%rdi), %r11
	addq	%r8, %rsi
	leaq	(%rbx,%rsi,8), %r8
	leaq	48(%r10), %rbx
.L84:
	movq	%r10, %rsi
	movq	%r8, %rdx
.L86:
	movq	$0x000000000, (%rdx)
	movq	%rsi, %r9
	xorl	%edi, %edi
.L85:
	movsd	(%rcx,%rdi,8), %xmm0
	mulsd	(%r9), %xmm0
	incq	%rdi
	addq	$48, %r9
	addsd	(%rdx), %xmm0
	movsd	%xmm0, (%rdx)
	cmpq	$6, %rdi
	jne	.L85
	addq	$8, %rsi
	addq	$8, %rdx
	cmpq	%rsi, %rbx
	jne	.L86
	addq	$48, %rcx
	addq	%rax, %r8
	cmpq	%rcx, %r11
	jne	.L84
	movq	296(%rsp), %rax
	subq	%fs:40, %rax
	je	.L88
	call	__stack_chk_fail@PLT
.L88:
	addq	$312, %rsp
	.cfi_def_cfa_offset 40
	popq	%rbx
	.cfi_def_cfa_offset 32
	popq	%rbp
	.cfi_def_cfa_offset 24
	popq	%r12
	.cfi_def_cfa_offset 16
	popq	%r13
	.cfi_def_cfa_offset 8
	ret
	.cfi_endproc
.LFE3651:
	.size	_Z6updatePA6_dS0_PdS1_iii, .-_Z6updatePA6_dS0_PdS1_iii
	.globl	_Z6savez3PA6_dS0_PdS1_iii
	.type	_Z6savez3PA6_dS0_PdS1_iii, @function
_Z6savez3PA6_dS0_PdS1_iii:
.LFB3652:
	.cfi_startproc
	endbr64
	pushq	%r14
	.cfi_def_cfa_offset 16
	.cfi_offset 14, -16
	movq	%rdi, %rax
	movq	%rsi, %rdi
	movq	%rcx, %r14
	pushq	%r13
	.cfi_def_cfa_offset 24
	.cfi_offset 13, -24
	movq	%rax, %rsi
	movq	%rdx, %r13
	pushq	%r12
	.cfi_def_cfa_offset 32
	.cfi_offset 12, -32
	pushq	%rbp
	.cfi_def_cfa_offset 40
	.cfi_offset 6, -40
	movl	%r9d, %ebp
	pushq	%rbx
	.cfi_def_cfa_offset 48
	.cfi_offset 3, -48
	imull	$26, %ebp, %ebp
	movl	%r8d, %ebx
	imull	$26, %ebx, %ebx
	subq	$304, %rsp
	.cfi_def_cfa_offset 352
	movslq	%ebp, %rbp
	movq	%fs:40, %rdx
	movq	%rdx, 296(%rsp)
	xorl	%edx, %edx
	leaq	8(%rsp), %r12
	movslq	%ebx, %rbx
	movq	%r12, %rdx
	salq	$3, %rbx
	call	_Z9Mat66MultPA6_dS0_S0_
	movslq	352(%rsp), %rax
	leaq	296(%rsp), %rsi
	addq	%rax, %rbp
	movq	%r12, %rax
	leaq	(%r14,%rbp,8), %rdx
.L97:
	movq	$0x000000000, (%rdx)
	xorl	%ecx, %ecx
.L96:
	movsd	(%rax,%rcx), %xmm0
	mulsd	0(%r13,%rcx), %xmm0
	addq	$8, %rcx
	addsd	(%rdx), %xmm0
	movsd	%xmm0, (%rdx)
	cmpq	$48, %rcx
	jne	.L96
	addq	$48, %rax
	addq	%rbx, %rdx
	cmpq	%rsi, %rax
	jne	.L97
	movq	296(%rsp), %rax
	subq	%fs:40, %rax
	je	.L98
	call	__stack_chk_fail@PLT
.L98:
	addq	$304, %rsp
	.cfi_def_cfa_offset 48
	popq	%rbx
	.cfi_def_cfa_offset 40
	popq	%rbp
	.cfi_def_cfa_offset 32
	popq	%r12
	.cfi_def_cfa_offset 24
	popq	%r13
	.cfi_def_cfa_offset 16
	popq	%r14
	.cfi_def_cfa_offset 8
	ret
	.cfi_endproc
.LFE3652:
	.size	_Z6savez3PA6_dS0_PdS1_iii, .-_Z6savez3PA6_dS0_PdS1_iii
	.globl	_Z5makeRPA6_dS0_d
	.type	_Z5makeRPA6_dS0_d, @function
_Z5makeRPA6_dS0_d:
.LFB3653:
	.cfi_startproc
	endbr64
	pushq	%rbp
	.cfi_def_cfa_offset 16
	.cfi_offset 6, -16
	movq	%rsi, %r10
	movq	%rdi, %rbp
	movq	%rdi, %r11
	pushq	%rbx
	.cfi_def_cfa_offset 24
	.cfi_offset 3, -24
	xorl	%eax, %eax
	movq	%rsi, %rbx
	xorl	%esi, %esi
	subq	$24, %rsp
	.cfi_def_cfa_offset 48
	movsd	.LC0(%rip), %xmm1
.L103:
	movq	%r11, %r8
	movq	%r10, %r9
	addq	$48, %r11
	addq	$48, %r10
	cmpq	%r11, %r9
	jb	.L110
.L107:
	movl	$12, %ecx
	movq	%r8, %rdi
	rep stosl
	movl	$12, %ecx
	movq	%r9, %rdi
	rep stosl
.L105:
	movsd	%xmm1, 0(%rbp,%rsi)
	movsd	%xmm1, (%rbx,%rsi)
	addq	$56, %rsi
	cmpq	$336, %rsi
	jne	.L103
	leaq	8(%rsp), %rdi
	movq	%rsp, %rsi
	call	sincos@PLT
	movsd	(%rsp), %xmm0
	movsd	8(%rsp), %xmm1
	movsd	%xmm0, 168(%rbp)
	movsd	%xmm0, 224(%rbp)
	movsd	%xmm0, 168(%rbx)
	movsd	%xmm0, 224(%rbx)
	movaps	%xmm1, %xmm0
	xorps	.LC1(%rip), %xmm0
	movsd	%xmm0, 176(%rbp)
	movsd	%xmm1, 176(%rbx)
	movsd	%xmm1, 216(%rbp)
	movsd	%xmm0, 216(%rbx)
	addq	$24, %rsp
	.cfi_remember_state
	.cfi_def_cfa_offset 24
	popq	%rbx
	.cfi_def_cfa_offset 16
	popq	%rbp
	.cfi_def_cfa_offset 8
	ret
.L110:
	.cfi_restore_state
	xorl	%edx, %edx
	cmpq	%r10, %r8
	jnb	.L107
.L104:
	movq	$0x000000000, (%r8,%rdx)
	movq	$0x000000000, (%r9,%rdx)
	addq	$8, %rdx
	cmpq	$48, %rdx
	jne	.L104
	jmp	.L105
	.cfi_endproc
.LFE3653:
	.size	_Z5makeRPA6_dS0_d, .-_Z5makeRPA6_dS0_d
	.section	.text.startup,"ax",@progbits
	.type	_ZL24__sti____cudaRegisterAllv, @function
_ZL24__sti____cudaRegisterAllv:
.LFB3679:
	.cfi_startproc
	endbr64
	pushq	%rax
	.cfi_def_cfa_offset 16
	leaq	_ZL15__fatDeviceText(%rip), %rdi
	call	__cudaRegisterFatBinary@PLT
	movq	%rax, %rdi
	movq	%rax, _ZL20__cudaFatCubinHandle(%rip)
	call	__cudaRegisterFatBinaryEnd@PLT
	leaq	_ZL26__cudaUnregisterBinaryUtilv(%rip), %rdi
	popq	%rdx
	.cfi_def_cfa_offset 8
	jmp	atexit@PLT
	.cfi_endproc
.LFE3679:
	.size	_ZL24__sti____cudaRegisterAllv, .-_ZL24__sti____cudaRegisterAllv
	.section	.init_array,"aw"
	.align 8
	.quad	_ZL24__sti____cudaRegisterAllv
	.section	.nvFatBinSegment,"aw"
	.align 8
	.type	_ZL15__fatDeviceText, @object
	.size	_ZL15__fatDeviceText, 24
_ZL15__fatDeviceText:
	.long	1180844977
	.long	1
	.quad	fatbinData
	.quad	0
	.local	_ZL20__cudaFatCubinHandle
	.comm	_ZL20__cudaFatCubinHandle,8,8
	.section	.rodata.cst8,"aM",@progbits,8
	.align 8
.LC0:
	.long	0
	.long	1072693248
	.section	.rodata.cst16,"aM",@progbits,16
	.align 16
.LC1:
	.long	0
	.long	-2147483648
	.long	0
	.long	0
	.section	.rodata.cst8
	.align 8
.LC3:
	.long	1374389535
	.long	-1071407432
	.align 8
.LC4:
	.long	0
	.long	1071644672
	.align 8
.LC5:
	.long	0
	.long	-1075838976
	.ident	"GCC: (Ubuntu 13.3.0-6ubuntu2~24.04) 13.3.0"
	.section	.note.GNU-stack,"",@progbits
	.section	.note.gnu.property,"a"
	.align 8
	.long	1f - 0f
	.long	4f - 1f
	.long	5
0:
	.string	"GNU"
1:
	.align 8
	.long	0xc0000002
	.long	3f - 2f
2:
	.long	0x3
3:
	.align 8
4:

//	DCAfuncts.cu
//
//This file contains all of the functions that are used during initialization, 
//assembly, and disassembly.


#include <hip/hip_runtime.h>
#include <iostream>
//Function Prototypes
//	All function called in this file are written in this file
void transpose(double S[6][6]);
void Mat66Mult(double A[6][6], double B[6][6], double C[6][6]);
void Mat61Mult(double A[6][6], double B[6][6], double C[6][6]);
void makeS(double S[6][6],double r[]);
void set_D(double D[6][6]);
void set_Dt(double D[6][6]);
void printm(double A[6][6]);

//get_X:
//	Function used to find the intermediate quantity X for a body
//	Considering the two bodies used to create X as b1 and b2:
//		z1 is a matrix containing z11 for b2
//		z2 is z22 for b1
//		D is the matrix in which X will be stored
void get_X(double z1[6][6], double z2 [6][6], double D[6][6])
{		
	for(int r = 0; r<6; r++)	//Loop through all 6 rows
	{
		for(int c=0; c<6; c++)	//Loop through all 6 columns
		{
    		z1[r][c]=z1[r][c]+z2[r][c];	//Add z1 and z2 together and store the solution in z1
		}
	}
	//Completing the above loop, z1 now holds b1.z22+b2.z11

    set_D(D);	//put the D matrix corresponding to a joint in this simulation in D
    Mat66Mult(z1,D,z1);	//Perform z1*D and store the solution in z1
	//z1 now holds (b1.z22+b2.z11)*D


    set_Dt(D);	//put the transpose of the D matrix in D
    Mat66Mult(D,z1,D); //Perform D*z1 and store the solution in D
	//D now holds Dt*(b1.z22+b2.z11)*D=X	
}

//set_D:
//	Function used to set the matrix that defines the subspace of prohibited motion.
//		D is the matrix in which the prohibited motion matrix will be stored.  
void set_D(double D[6][6])
{
	for(int r = 0; r<6; r++)	//Loop through all 6 rows
	{
		for(int c=0; c<6; c++)	//Loop through all 6 columns
		{
    		D[r][c]= 0;	//Set the D matrix to 0
		}
	}    
	
	//Set the D matrix according to the 2D joints used in this simulation
    D[0][0]=1;
    D[1][1]=1;
    D[3][2]=1;
    D[4][3]=1;
    D[5][4]=1;
}

//set_Dt:
//	Function used to set the transpose of the matrix that defines the subspace of prohibited motion.
//		D is the matrix in which the transposed prohibited motion matrix will be stored.  
void set_Dt(double D[6][6])
{
    for(int r = 0; r<6; r++)	//Loop through all 6 rows
	{
		for(int c=0; c<6; c++)	//Loop through all 6 columns
		{
    		D[r][c]= 0;	//Set the D matrix to 0;
		}
	}
   
	//Set the D matrix according to the 2D joints used in this simulation
    D[0][0]=1.0;
    D[1][1]=1.0;
    D[2][3]=1.0;
    D[3][4]=1.0;
    D[4][5]=1.0;
}

//invert_X:
//	Function used to invert the X matrix.  This function takes advantage of the nature of this 
//	2D simulation by only inverting the 2x2 matrix inside of X that will effect the solution.
//	This was done for efficiency of the simulation as well as ease of implementation in this case.
//		X is the intermediate quantity X; X inverse will be stored in X, replacing it
void invert_X(double X[6][6])
{
	//Variable Declarations
	double det;	//The determinant of the 2x2 matrix
	double Xinv[2][2];	//The inverse of the 2x2 matrix

	//Matrix inversion
	det=(X[2][2]*X[3][3])-(X[2][3]*X[3][2]);	//Find the determinant of the 2x2 matrix

	//Create the inverse matrix based on the trick for 2x2 matrices 
	Xinv[0][0]=X[3][3]/det;
	Xinv[0][1]=-1*X[2][3]/det;
	Xinv[1][0]=-1*X[3][2]/det;
	Xinv[1][1]=X[2][2]/det;

	for(int c=0; c<6; c++)
	{
		for(int r=0; r<6; r++)
		{
			X[r][c]=0;
		}
	}
	//Save the Solution
	for(int c = 0; c<2; c++)	//Loop through 2 columns
	{
		for(int r=0; r<2;r++)	//Loop through 2 rows
		{
			X[r+2][c+2]=Xinv[r][c];	//Save X inverse in X
		}
	}
}

//make_W:
//	Function used to create the intermediate quantity W
//		Xinv is Xinverse;  W will be stored here
//		D is an empty matrix used for matrix calculations
void make_W(double Xinv[6][6], double D[6][6])                    
{   
	for(int c =0; c< 5; c++)	//Loop through 5 columns
	{
		for(int r=0; r<5; r++)	//Loop through 5 rows
		{
			//Set the values in row and column 6 to 0
    		if(r==5 || c==5)	
			{
     			Xinv[r][c]=0;	
			}
		}
	}

	set_Dt(D);	//Put the D transpose matrix in D
    Mat66Mult(Xinv,D,Xinv); //Perform Xinv*D and store the solution in Xinv
	//Xinv now holds Xinv*Dt
	
    set_D(D);	//Put the D matrix in D
    Mat66Mult(D,Xinv,Xinv);	//Perform D*Xinv and store the solution in Xinv
	//Xinv now holds D*Xinv*Dt=W
}

//zaa:
//	Function used to calculate z11 and z22 during initialization
//		r is the necessary position vector (r01 for z11 and r02 for z22)
//		z is the matrix in which the solution will be stored
//		Minv is the inverse mass matrix
//		S is an empty matrix that is made into the shifter matrix corresponding to the r vector
void zaa(double r[],double z[6][6],double Minv[6][6],double S[6][6])
{
    makeS(S,r);	//Create the shifter matrix
   	Mat66Mult(Minv,S,z);	//Perform Minv*S and store the solution in z
	//z now holds Minv*S
	

    transpose(S);	//Transpose the shifter matrix
	
    Mat66Mult(S,z,z);	//Perform S*z and store the solution in z
	//z now holds St*Minv*S=zaa
	
    Mat66Mult(S,Minv,Minv);	//Perform S*Minv and store the solution in Minv
	//Minv now holds S*Minv, this is done becuase this values is needed later
}

//zab:
//	Function used to calculate z12 and z21 during initialization
//		r is the necessary position vector (r01 for z21 and r02 for z12)
//		z is an empty matrix in which the solution will be stored
//		Minv is the necessary shifter matrix multiplied by the inverse mass matrix
//		S is an empty matrix to be made into the shifter matrix corresponding to the r vector
void zab(double r[],double z[6][6],double Minv[6][6],double S[6][6])
{    
   makeS(S,r);	//Make the shifter matrix

	//The following loop ensures that values that were not set by makeS are set to 0
   for(int c =0; c< 3; c++)
	{
		for(int r=3; r<6; r++)
		{
     			S[r][c]=0;
		}
	}

    Mat66Mult(Minv,S,z);	//Perform Minv*S and store the solution in z
	//z now holds Minv*S=zab
}

//makeS:
//	Function used to create a shifter matrix with the given position vector r
//		S is the matrix in which the shifter matrix will be stored
//		r is the position vector correspoding to the desired shifter matrix
void makeS(double S[6][6],double r[])
{
	//Set the top right 3x3 matrix in S to the skew matrix of r
    S[0][4]=-1*r[2];
    S[0][5]=r[1];
    S[1][3]=r[2];
    S[1][5]=-1*r[0];
    S[2][3]=-1*r[1];
    S[2][4]=r[0];    
}

void makeSt(double S[6][6],double r[])
{
	//Set the top right 3x3 matrix in S to the skew matrix of r
    S[4][0]=-1*r[2];
    S[5][0]=r[1];
    S[3][1]=r[2];
    S[5][1]=-1*r[0];
    S[3][2]=-1*r[1];
    S[4][2]=r[0];    
}

//MSsetup:
//	Function used to setup the inverse mass matrix and set the shifter matrix to the identity matrix.
//	This function takes advantage of the 2D nature of this simulation by assuming that 
//	the mass matrix will always be diagonal.
//		Minv is the matrix in which the inverse mass matrix will be stored
//		S is the matrix that must be set to the identity matrix to allow for population later
//		m is the mass of the body
//		I is the inertia tensor of the body
void MSsetup(double Minv[6][6],double S[6][6],double m, double I[3][3])
{
   for(int c =0; c< 6; c++)	//Loop through 6 columns
	{
		for(int r=0; r<6; r++)	//Loop through 6 rows
		{
			S[r][c]=0;	//Set the S matrix to 0

			//If the row is less than 3, and the current iteration is on the diagonal of the matrix,
			//then insert the inverse of the inertia tensor as it is needed in M inverse
			if (r<3 && c==r)
			{
    			Minv[r][c]=(1/(I[r][c]));
			}
			//If the row is greater than 2, and the current iteration is on the diagonal of the 
			//of the matrix, then insert 1/m as it is needed in M inverse
			else if(r>2 && c==r)
			{
				Minv[r][c]=(1/m);
			}
			//If not on the diagonal, set that element in M inverse to 0
			else
			{
				Minv[r][c]=0;
			}
			//If on the diagonal, set that element in S to 1
			if(c==r)
			{
				S[r][c]=1;
			}
		}
	}

}

//Fa:
//	Function used to find the state dependant force vector Fa
//		S is a matrix whose first column will hold Fa
//		w is the angular velocity of the body
//		r is the corresponding position vector (r01 or r02)
//		m is the mass of the body
void Fa(double S[6][6],double w, double r[], double m)
{
 	double g = 9.81;	//Set the gravitational constant
	
	//Loop through the first column of S and set it to 0
    for(int rr=0; rr<6; rr++)
	{
         S[rr][0]=0;
    }
	
	//Create Fa based on the force of gravity and the centripedal force on the body
    S[3][0]=-m*w*w*r[0];
    S[4][0]=-g*m-m*w*w*r[1];
}

//r02:
//	Function used to find the r02 vector of the body.  This function takes advantage of the
//	2D nature of this simulation by removing the need for a direction cosine matrix
//		r is the vector in which r02 will be stored
//		angle is the ange of the body with respect to the vertical
//		L is the length of the body
void r02(double r[],double L)
{
    r[0]=0;
    r[1]=-L/2;
    r[2]=0;

}

//r01:
//	Funtion used to find the r01 position vector of the body. This function takes advantage of the
//	2D nature of this simulation by removing the need for a direction cosine matrix
//		r is the vector in which r02 will be stored
//		angle is the ange of the body with respect to the vertical
void r01(double r[], double L)
{
    r[0]=0;
    r[1]=L/2;
    r[2]=0;
}

//transpose:
//	Function used to find the transpose of a 6x6 shifter matrix
//		S is the shifter matrix to be transposed.  The transposed matrix will also be stored here.
void transpose(double S[6][6])
{
	double S2[6][6];	//Temporary matrix used to store values
	
	//Loop through every row and column and save S transpose into S2
	for(int r =0; r< 6; r++)
	{
		for(int c=0; c<6; c++)
		{
			S2[r][c]=S[c][r];
		}
	}

	//Loop through every row and column and save S transpose into S
	for(int r =0; r< 6; r++)
	{
		for(int c=0; c<6; c++)
		{
			S[r][c]=S2[r][c];
		}
	}
}

//Mat66Mult:
//	Function used to multipy two 6x6 matrices.  Note that a temporary 
//	matrix is used to first store the solution. This allows for this 
//	function to store the solution into one of the matrices being multiplied.
//	The form of the operation is A*B=C
//		A is the left matrix
//		B is the right matrix
//		C is the matrix in which the solution is stored
void Mat66Mult(double A[6][6], double B[6][6], double C[6][6])
{
	double j[6][6];	//Temporary matrix used to store the solution

   	for(int r =0; r< 6; r++)	//Loop through all 6 rows
	{
		for(int c=0; c<6; c++)	//Loop through all 6 columns
		{	
			j[r][c]=0;	//Initialize the current element in the temporary matrix to 0

			//The loop below cycles through the row of the first matrix, and the column of the
			//second corresponding to the current row and column of the temporary matrix.  It 
			//takes the dot product of the row and column and stores it in j.
			for(int l =0; l<6; l++)
			{	
				j[r][c]=j[r][c]+(A[r][l]*B[l][c]);
			}
		}
	}

	//Loop through the rows and columns of the temporary matrix, saving the solution into C
	for(int r=0; r<6; r++)
	{
		for(int c=0; c<6; c++)
		{
			C[r][c]=j[r][c];
		}
	}
}

//Mat61Mult:
//	Function used to multipy a 6x6 and a 6x1 matrix.  Note that a temporary 
//	matrix is used to first store the solution. This allows for this function 
//	to store the solution into one of the matrices being multiplied.  It should
//	also be noted that this function takes only 6x6 matrices, and the 6x1 matrix 
//	to be multiplied is assumed to be held in the first column of matrix B.
//	The form of the operation is A*B=C
//		A is the 6x6 matrix
//		B is a 6x6 matrix with the 6x1 matrix stored in its first column
//		C is the matrix in which the solution is stored.  It is stored only in the first column
void Mat61Mult(double A[6][6], double B[6][6], double C[6][6])
{
	double j[6];	//Temporary matrix
 
	for(int r =0; r< 6; r++)	//Loop through all 6 rows
	{	
		j[r]=0;	//Initialize this element of j to 0
		
		//The loop below cycles through the row of the first matrix, and the column of the
		//second corresponding to the current row of the temporary matrix.  It 
		//takes the dot product of the row and column and stores it in j.
		for(int c=0; c<6; c++)
		{
			j[r]+=A[r][c]*B[c][0];
		}
	}	
	
	//Loop through the rows of the temporary matrix, saving the solution into C		
	for(int r = 0; r< 6; r++)
	{
		C[r][0]=j[r];
	}
        
}

void update(double R[6][6], double Rt[6][6], double zeta1[], double zeta2[], int n, int bodynum, int zetanum)
{
	double temp[6][6];
	for(int r =0; r<6; r++)
	{
		for(int c =0; c<6; c++)
		{
			temp[r][c]=0;
			for(int i =0; i<6; i++)
			{
				temp[r][c]+=zeta1[i+r*26*n+26*bodynum+zetanum]*Rt[i][c];
			}
			//std::cout<<Rt[r][c]<< "  ";
		}
		//std::cout<<std::endl;
	}

	for(int r =0; r<6; r++)
	{
		for(int c =0; c<6; c++)
		{
			zeta2[c+r*26*n+26*bodynum+zetanum]=0;
			for(int i =0; i<6; i++)
			{
				zeta2[c+r*26*n+26*bodynum+zetanum]+=R[r][i]*temp[i][c];
			}
		}
	}
}

void savez3(double Minv[6][6],double S[6][6] ,double Fa[], double nZetas[], int n, int bodynum, int zetanum)
{
	double temp[6][6];
	Mat66Mult(S,Minv,temp);
	for(int r =0; r<6; r++)
	{
		nZetas[r*26*n+bodynum*26+zetanum]=0;
		for(int c =0; c<6; c++)
		{
			nZetas[r*26*n+bodynum*26+zetanum]+=temp[r][c]*Fa[c];
		}
	}
}

void makeR(double R[6][6], double Rt[6][6], double angle)
{
	//std::cout<<angle<<std::endl;
	for(int r =0; r<6; r++)
	{
		for(int c =0; c<6; c++)
		{
			R[r][c]=0;
			Rt[r][c]=0;
		}
		R[r][r]=1;
		Rt[r][r]=1;
	}
	//std::cout<<std::endl<<cos(angle)<<std::endl;
	R[3][3]=cos(angle);
	R[4][4]=cos(angle);
	Rt[3][3]=cos(angle);
	Rt[4][4]=cos(angle);
	R[3][4]=-sin(angle);
	Rt[3][4]=sin(angle);
	R[4][3]=sin(angle);
	Rt[4][3]=-sin(angle);

}


			


	.text
	.file	"DCAfuncts.hip"
                                        # Start of file scope inline assembly
	.globl	_ZSt21ios_base_library_initv

                                        # End of file scope inline assembly
	.globl	_Z5get_XPA6_dS0_S0_             # -- Begin function _Z5get_XPA6_dS0_S0_
	.type	_Z5get_XPA6_dS0_S0_,@function
_Z5get_XPA6_dS0_S0_:                    # @_Z5get_XPA6_dS0_S0_
	.cfi_startproc
# %bb.0:
	pushq	%r15
	.cfi_def_cfa_offset 16
	pushq	%r14
	.cfi_def_cfa_offset 24
	pushq	%r12
	.cfi_def_cfa_offset 32
	pushq	%rbx
	.cfi_def_cfa_offset 40
	pushq	%rax
	.cfi_def_cfa_offset 48
	.cfi_offset %rbx, -40
	.cfi_offset %r12, -32
	.cfi_offset %r14, -24
	.cfi_offset %r15, -16
	movq	%rdx, %rbx
	movq	%rdi, %r14
	xorl	%eax, %eax
	movq	%rdi, %rcx
.LBB0_1:                                # %.preheader
                                        # =>This Loop Header: Depth=1
                                        #     Child Loop BB0_2 Depth 2
	xorl	%edx, %edx
.LBB0_2:                                #   Parent Loop BB0_1 Depth=1
                                        # =>  This Inner Loop Header: Depth=2
	movsd	(%rcx,%rdx,8), %xmm0            # xmm0 = mem[0],zero
	addsd	(%rsi,%rdx,8), %xmm0
	movsd	%xmm0, (%rcx,%rdx,8)
	incq	%rdx
	cmpq	$6, %rdx
	jne	.LBB0_2
# %bb.3:                                #   in Loop: Header=BB0_1 Depth=1
	incq	%rax
	addq	$48, %rsi
	addq	$48, %rcx
	cmpq	$6, %rax
	jne	.LBB0_1
# %bb.4:
	leaq	8(%rbx), %r15
	movl	$280, %edx                      # imm = 0x118
	movq	%r15, %rdi
	xorl	%esi, %esi
	callq	memset@PLT
	movabsq	$4607182418800017408, %r12      # imm = 0x3FF0000000000000
	movq	%r12, (%rbx)
	movq	%r12, 56(%rbx)
	movq	%r12, 160(%rbx)
	movq	%r12, 216(%rbx)
	movq	%r12, 272(%rbx)
	movq	%r14, %rdi
	movq	%rbx, %rsi
	movq	%r14, %rdx
	callq	_Z9Mat66MultPA6_dS0_S0_
	movl	$280, %edx                      # imm = 0x118
	movq	%r15, %rdi
	xorl	%esi, %esi
	callq	memset@PLT
	movq	%r12, (%rbx)
	movq	%r12, 56(%rbx)
	movq	%r12, 120(%rbx)
	movq	%r12, 176(%rbx)
	movq	%r12, 232(%rbx)
	movq	%rbx, %rdi
	movq	%r14, %rsi
	movq	%rbx, %rdx
	addq	$8, %rsp
	.cfi_def_cfa_offset 40
	popq	%rbx
	.cfi_def_cfa_offset 32
	popq	%r12
	.cfi_def_cfa_offset 24
	popq	%r14
	.cfi_def_cfa_offset 16
	popq	%r15
	.cfi_def_cfa_offset 8
	jmp	_Z9Mat66MultPA6_dS0_S0_         # TAILCALL
.Lfunc_end0:
	.size	_Z5get_XPA6_dS0_S0_, .Lfunc_end0-_Z5get_XPA6_dS0_S0_
	.cfi_endproc
                                        # -- End function
	.globl	_Z5set_DPA6_d                   # -- Begin function _Z5set_DPA6_d
	.type	_Z5set_DPA6_d,@function
_Z5set_DPA6_d:                          # @_Z5set_DPA6_d
	.cfi_startproc
# %bb.0:
	pushq	%rbx
	.cfi_def_cfa_offset 16
	.cfi_offset %rbx, -16
	movq	%rdi, %rbx
	addq	$8, %rdi
	movl	$280, %edx                      # imm = 0x118
	xorl	%esi, %esi
	callq	memset@PLT
	movabsq	$4607182418800017408, %rax      # imm = 0x3FF0000000000000
	movq	%rax, (%rbx)
	movq	%rax, 56(%rbx)
	movq	%rax, 160(%rbx)
	movq	%rax, 216(%rbx)
	movq	%rax, 272(%rbx)
	popq	%rbx
	.cfi_def_cfa_offset 8
	retq
.Lfunc_end1:
	.size	_Z5set_DPA6_d, .Lfunc_end1-_Z5set_DPA6_d
	.cfi_endproc
                                        # -- End function
	.globl	_Z9Mat66MultPA6_dS0_S0_         # -- Begin function _Z9Mat66MultPA6_dS0_S0_
	.type	_Z9Mat66MultPA6_dS0_S0_,@function
_Z9Mat66MultPA6_dS0_S0_:                # @_Z9Mat66MultPA6_dS0_S0_
	.cfi_startproc
# %bb.0:
	pushq	%rbx
	.cfi_def_cfa_offset 16
	subq	$160, %rsp
	.cfi_def_cfa_offset 176
	.cfi_offset %rbx, -16
	xorl	%eax, %eax
.LBB2_1:                                # %.preheader31
                                        # =>This Loop Header: Depth=1
                                        #     Child Loop BB2_2 Depth 2
                                        #       Child Loop BB2_3 Depth 3
	leaq	(%rax,%rax,2), %rcx
	shlq	$4, %rcx
	addq	%rsp, %rcx
	addq	$-128, %rcx
	movq	%rsi, %r8
	xorl	%r9d, %r9d
.LBB2_2:                                #   Parent Loop BB2_1 Depth=1
                                        # =>  This Loop Header: Depth=2
                                        #       Child Loop BB2_3 Depth 3
	leaq	(%rcx,%r9,8), %r10
	xorpd	%xmm0, %xmm0
	movq	%r8, %r11
	xorl	%ebx, %ebx
.LBB2_3:                                #   Parent Loop BB2_1 Depth=1
                                        #     Parent Loop BB2_2 Depth=2
                                        # =>    This Inner Loop Header: Depth=3
	movsd	(%rdi,%rbx,8), %xmm1            # xmm1 = mem[0],zero
	mulsd	(%r11), %xmm1
	addsd	%xmm1, %xmm0
	incq	%rbx
	addq	$48, %r11
	cmpq	$6, %rbx
	jne	.LBB2_3
# %bb.4:                                #   in Loop: Header=BB2_2 Depth=2
	movsd	%xmm0, (%r10)
	incq	%r9
	addq	$8, %r8
	cmpq	$6, %r9
	jne	.LBB2_2
# %bb.5:                                #   in Loop: Header=BB2_1 Depth=1
	incq	%rax
	addq	$48, %rdi
	cmpq	$6, %rax
	jne	.LBB2_1
# %bb.6:                                # %.preheader.preheader
	xorl	%eax, %eax
.LBB2_7:                                # %.preheader
                                        # =>This Inner Loop Header: Depth=1
	movupd	-128(%rsp,%rax), %xmm0
	movupd	-112(%rsp,%rax), %xmm1
	movups	-96(%rsp,%rax), %xmm2
	movups	%xmm2, 32(%rdx,%rax)
	movupd	%xmm1, 16(%rdx,%rax)
	movupd	%xmm0, (%rdx,%rax)
	addq	$48, %rax
	cmpq	$288, %rax                      # imm = 0x120
	jne	.LBB2_7
# %bb.8:
	addq	$160, %rsp
	.cfi_def_cfa_offset 16
	popq	%rbx
	.cfi_def_cfa_offset 8
	retq
.Lfunc_end2:
	.size	_Z9Mat66MultPA6_dS0_S0_, .Lfunc_end2-_Z9Mat66MultPA6_dS0_S0_
	.cfi_endproc
                                        # -- End function
	.globl	_Z6set_DtPA6_d                  # -- Begin function _Z6set_DtPA6_d
	.type	_Z6set_DtPA6_d,@function
_Z6set_DtPA6_d:                         # @_Z6set_DtPA6_d
	.cfi_startproc
# %bb.0:
	pushq	%rbx
	.cfi_def_cfa_offset 16
	.cfi_offset %rbx, -16
	movq	%rdi, %rbx
	addq	$8, %rdi
	movl	$280, %edx                      # imm = 0x118
	xorl	%esi, %esi
	callq	memset@PLT
	movabsq	$4607182418800017408, %rax      # imm = 0x3FF0000000000000
	movq	%rax, (%rbx)
	movq	%rax, 56(%rbx)
	movq	%rax, 120(%rbx)
	movq	%rax, 176(%rbx)
	movq	%rax, 232(%rbx)
	popq	%rbx
	.cfi_def_cfa_offset 8
	retq
.Lfunc_end3:
	.size	_Z6set_DtPA6_d, .Lfunc_end3-_Z6set_DtPA6_d
	.cfi_endproc
                                        # -- End function
	.section	.rodata.cst16,"aM",@progbits,16
	.p2align	4, 0x0                          # -- Begin function _Z8invert_XPA6_d
.LCPI4_0:
	.quad	0x8000000000000000              # double -0
	.quad	0x8000000000000000              # double -0
	.text
	.globl	_Z8invert_XPA6_d
	.type	_Z8invert_XPA6_d,@function
_Z8invert_XPA6_d:                       # @_Z8invert_XPA6_d
	.cfi_startproc
# %bb.0:
	movsd	112(%rdi), %xmm0                # xmm0 = mem[0],zero
	movsd	120(%rdi), %xmm1                # xmm1 = mem[0],zero
	movsd	168(%rdi), %xmm3                # xmm3 = mem[0],zero
	movsd	160(%rdi), %xmm2                # xmm2 = mem[0],zero
	xorl	%eax, %eax
	movq	%rdi, %rcx
.LBB4_1:                                # %.preheader31
                                        # =>This Loop Header: Depth=1
                                        #     Child Loop BB4_2 Depth 2
	movq	$-288, %rdx                     # imm = 0xFEE0
.LBB4_2:                                #   Parent Loop BB4_1 Depth=1
                                        # =>  This Inner Loop Header: Depth=2
	movq	$0, 288(%rcx,%rdx)
	addq	$48, %rdx
	jne	.LBB4_2
# %bb.3:                                #   in Loop: Header=BB4_1 Depth=1
	incq	%rax
	addq	$8, %rcx
	cmpq	$6, %rax
	jne	.LBB4_1
# %bb.4:                                # %.preheader
	movapd	%xmm0, %xmm4
	mulsd	%xmm3, %xmm4
	movapd	%xmm1, %xmm5
	mulsd	%xmm2, %xmm5
	subsd	%xmm5, %xmm4
	divsd	%xmm4, %xmm0
	movapd	.LCPI4_0(%rip), %xmm5           # xmm5 = [-0.0E+0,-0.0E+0]
	xorpd	%xmm5, %xmm2
	divsd	%xmm4, %xmm2
	xorpd	%xmm5, %xmm1
	divsd	%xmm4, %xmm1
	divsd	%xmm4, %xmm3
	movsd	%xmm3, 112(%rdi)
	movsd	%xmm2, 160(%rdi)
	movsd	%xmm1, 120(%rdi)
	movsd	%xmm0, 168(%rdi)
	retq
.Lfunc_end4:
	.size	_Z8invert_XPA6_d, .Lfunc_end4-_Z8invert_XPA6_d
	.cfi_endproc
                                        # -- End function
	.globl	_Z6make_WPA6_dS0_               # -- Begin function _Z6make_WPA6_dS0_
	.type	_Z6make_WPA6_dS0_,@function
_Z6make_WPA6_dS0_:                      # @_Z6make_WPA6_dS0_
	.cfi_startproc
# %bb.0:
	pushq	%r15
	.cfi_def_cfa_offset 16
	pushq	%r14
	.cfi_def_cfa_offset 24
	pushq	%r12
	.cfi_def_cfa_offset 32
	pushq	%rbx
	.cfi_def_cfa_offset 40
	pushq	%rax
	.cfi_def_cfa_offset 48
	.cfi_offset %rbx, -40
	.cfi_offset %r12, -32
	.cfi_offset %r14, -24
	.cfi_offset %r15, -16
	movq	%rsi, %r14
	movq	%rdi, %rbx
	leaq	8(%rsi), %r15
	movl	$280, %edx                      # imm = 0x118
	movq	%r15, %rdi
	xorl	%esi, %esi
	callq	memset@PLT
	movabsq	$4607182418800017408, %r12      # imm = 0x3FF0000000000000
	movq	%r12, (%r14)
	movq	%r12, 56(%r14)
	movq	%r12, 120(%r14)
	movq	%r12, 176(%r14)
	movq	%r12, 232(%r14)
	movq	%rbx, %rdi
	movq	%r14, %rsi
	movq	%rbx, %rdx
	callq	_Z9Mat66MultPA6_dS0_S0_
	movl	$280, %edx                      # imm = 0x118
	movq	%r15, %rdi
	xorl	%esi, %esi
	callq	memset@PLT
	movq	%r12, (%r14)
	movq	%r12, 56(%r14)
	movq	%r12, 160(%r14)
	movq	%r12, 216(%r14)
	movq	%r12, 272(%r14)
	movq	%r14, %rdi
	movq	%rbx, %rsi
	movq	%rbx, %rdx
	addq	$8, %rsp
	.cfi_def_cfa_offset 40
	popq	%rbx
	.cfi_def_cfa_offset 32
	popq	%r12
	.cfi_def_cfa_offset 24
	popq	%r14
	.cfi_def_cfa_offset 16
	popq	%r15
	.cfi_def_cfa_offset 8
	jmp	_Z9Mat66MultPA6_dS0_S0_         # TAILCALL
.Lfunc_end5:
	.size	_Z6make_WPA6_dS0_, .Lfunc_end5-_Z6make_WPA6_dS0_
	.cfi_endproc
                                        # -- End function
	.section	.rodata.cst16,"aM",@progbits,16
	.p2align	4, 0x0                          # -- Begin function _Z3zaaPdPA6_dS1_S1_
.LCPI6_0:
	.quad	0x8000000000000000              # double -0
	.quad	0x8000000000000000              # double -0
	.text
	.globl	_Z3zaaPdPA6_dS1_S1_
	.type	_Z3zaaPdPA6_dS1_S1_,@function
_Z3zaaPdPA6_dS1_S1_:                    # @_Z3zaaPdPA6_dS1_S1_
	.cfi_startproc
# %bb.0:
	pushq	%r15
	.cfi_def_cfa_offset 16
	pushq	%r14
	.cfi_def_cfa_offset 24
	pushq	%rbx
	.cfi_def_cfa_offset 32
	subq	$288, %rsp                      # imm = 0x120
	.cfi_def_cfa_offset 320
	.cfi_offset %rbx, -32
	.cfi_offset %r14, -24
	.cfi_offset %r15, -16
	movq	%rcx, %r14
	movq	%rdx, %rbx
	movq	%rsi, %r15
	movsd	16(%rdi), %xmm0                 # xmm0 = mem[0],zero
	movaps	.LCPI6_0(%rip), %xmm1           # xmm1 = [-0.0E+0,-0.0E+0]
	xorps	%xmm1, %xmm0
	movlps	%xmm0, 32(%rcx)
	movsd	8(%rdi), %xmm0                  # xmm0 = mem[0],zero
	movsd	%xmm0, 40(%rcx)
	movsd	16(%rdi), %xmm0                 # xmm0 = mem[0],zero
	movsd	%xmm0, 72(%rcx)
	movsd	(%rdi), %xmm0                   # xmm0 = mem[0],zero
	xorps	%xmm1, %xmm0
	movlps	%xmm0, 88(%rcx)
	movsd	8(%rdi), %xmm0                  # xmm0 = mem[0],zero
	xorps	%xmm1, %xmm0
	movlps	%xmm0, 120(%rcx)
	movsd	(%rdi), %xmm0                   # xmm0 = mem[0],zero
	movsd	%xmm0, 128(%rcx)
	movq	%rdx, %rdi
	movq	%rcx, %rsi
	movq	%r15, %rdx
	callq	_Z9Mat66MultPA6_dS0_S0_
	movq	%rsp, %rax
	xorl	%ecx, %ecx
	movq	%r14, %rdx
.LBB6_1:                                # %.preheader21.i
                                        # =>This Loop Header: Depth=1
                                        #     Child Loop BB6_2 Depth 2
	movq	%rdx, %rsi
	xorl	%edi, %edi
.LBB6_2:                                #   Parent Loop BB6_1 Depth=1
                                        # =>  This Inner Loop Header: Depth=2
	movsd	(%rsi), %xmm0                   # xmm0 = mem[0],zero
	movsd	%xmm0, (%rax,%rdi,8)
	incq	%rdi
	addq	$48, %rsi
	cmpq	$6, %rdi
	jne	.LBB6_2
# %bb.3:                                #   in Loop: Header=BB6_1 Depth=1
	incq	%rcx
	addq	$48, %rax
	addq	$8, %rdx
	cmpq	$6, %rcx
	jne	.LBB6_1
# %bb.4:                                # %.preheader.i.preheader
	movq	%rsp, %rsi
	movl	$288, %edx                      # imm = 0x120
	movq	%r14, %rdi
	callq	memcpy@PLT
	movq	%r14, %rdi
	movq	%r15, %rsi
	movq	%r15, %rdx
	callq	_Z9Mat66MultPA6_dS0_S0_
	movq	%r14, %rdi
	movq	%rbx, %rsi
	movq	%rbx, %rdx
	addq	$288, %rsp                      # imm = 0x120
	.cfi_def_cfa_offset 32
	popq	%rbx
	.cfi_def_cfa_offset 24
	popq	%r14
	.cfi_def_cfa_offset 16
	popq	%r15
	.cfi_def_cfa_offset 8
	jmp	_Z9Mat66MultPA6_dS0_S0_         # TAILCALL
.Lfunc_end6:
	.size	_Z3zaaPdPA6_dS1_S1_, .Lfunc_end6-_Z3zaaPdPA6_dS1_S1_
	.cfi_endproc
                                        # -- End function
	.section	.rodata.cst16,"aM",@progbits,16
	.p2align	4, 0x0                          # -- Begin function _Z5makeSPA6_dPd
.LCPI7_0:
	.quad	0x8000000000000000              # double -0
	.quad	0x8000000000000000              # double -0
	.text
	.globl	_Z5makeSPA6_dPd
	.type	_Z5makeSPA6_dPd,@function
_Z5makeSPA6_dPd:                        # @_Z5makeSPA6_dPd
	.cfi_startproc
# %bb.0:
	movsd	16(%rsi), %xmm0                 # xmm0 = mem[0],zero
	movaps	.LCPI7_0(%rip), %xmm1           # xmm1 = [-0.0E+0,-0.0E+0]
	xorps	%xmm1, %xmm0
	movlps	%xmm0, 32(%rdi)
	movsd	8(%rsi), %xmm0                  # xmm0 = mem[0],zero
	movsd	%xmm0, 40(%rdi)
	movsd	16(%rsi), %xmm0                 # xmm0 = mem[0],zero
	movsd	%xmm0, 72(%rdi)
	movsd	(%rsi), %xmm0                   # xmm0 = mem[0],zero
	xorps	%xmm1, %xmm0
	movlps	%xmm0, 88(%rdi)
	movsd	8(%rsi), %xmm0                  # xmm0 = mem[0],zero
	xorps	%xmm1, %xmm0
	movlps	%xmm0, 120(%rdi)
	movsd	(%rsi), %xmm0                   # xmm0 = mem[0],zero
	movsd	%xmm0, 128(%rdi)
	retq
.Lfunc_end7:
	.size	_Z5makeSPA6_dPd, .Lfunc_end7-_Z5makeSPA6_dPd
	.cfi_endproc
                                        # -- End function
	.globl	_Z9transposePA6_d               # -- Begin function _Z9transposePA6_d
	.type	_Z9transposePA6_d,@function
_Z9transposePA6_d:                      # @_Z9transposePA6_d
	.cfi_startproc
# %bb.0:
	subq	$168, %rsp
	.cfi_def_cfa_offset 176
	leaq	-128(%rsp), %rax
	xorl	%ecx, %ecx
	movq	%rdi, %rdx
.LBB8_1:                                # %.preheader21
                                        # =>This Loop Header: Depth=1
                                        #     Child Loop BB8_2 Depth 2
	movq	%rdx, %rsi
	xorl	%r8d, %r8d
.LBB8_2:                                #   Parent Loop BB8_1 Depth=1
                                        # =>  This Inner Loop Header: Depth=2
	movsd	(%rsi), %xmm0                   # xmm0 = mem[0],zero
	movsd	%xmm0, (%rax,%r8,8)
	incq	%r8
	addq	$48, %rsi
	cmpq	$6, %r8
	jne	.LBB8_2
# %bb.3:                                #   in Loop: Header=BB8_1 Depth=1
	incq	%rcx
	addq	$8, %rdx
	addq	$48, %rax
	cmpq	$6, %rcx
	jne	.LBB8_1
# %bb.4:                                # %.preheader.preheader
	xorl	%eax, %eax
.LBB8_5:                                # %.preheader
                                        # =>This Inner Loop Header: Depth=1
	movups	-128(%rsp,%rax), %xmm0
	movups	-112(%rsp,%rax), %xmm1
	movups	-96(%rsp,%rax), %xmm2
	movups	%xmm2, 32(%rdi,%rax)
	movups	%xmm1, 16(%rdi,%rax)
	movups	%xmm0, (%rdi,%rax)
	addq	$48, %rax
	cmpq	$288, %rax                      # imm = 0x120
	jne	.LBB8_5
# %bb.6:
	addq	$168, %rsp
	.cfi_def_cfa_offset 8
	retq
.Lfunc_end8:
	.size	_Z9transposePA6_d, .Lfunc_end8-_Z9transposePA6_d
	.cfi_endproc
                                        # -- End function
	.section	.rodata.cst16,"aM",@progbits,16
	.p2align	4, 0x0                          # -- Begin function _Z3zabPdPA6_dS1_S1_
.LCPI9_0:
	.quad	0x8000000000000000              # double -0
	.quad	0x8000000000000000              # double -0
	.text
	.globl	_Z3zabPdPA6_dS1_S1_
	.type	_Z3zabPdPA6_dS1_S1_,@function
_Z3zabPdPA6_dS1_S1_:                    # @_Z3zabPdPA6_dS1_S1_
	.cfi_startproc
# %bb.0:
	movq	%rdx, %rax
	movq	%rsi, %rdx
	movsd	16(%rdi), %xmm0                 # xmm0 = mem[0],zero
	movaps	.LCPI9_0(%rip), %xmm1           # xmm1 = [-0.0E+0,-0.0E+0]
	xorps	%xmm1, %xmm0
	movlps	%xmm0, 32(%rcx)
	movsd	8(%rdi), %xmm0                  # xmm0 = mem[0],zero
	movsd	%xmm0, 40(%rcx)
	movsd	16(%rdi), %xmm0                 # xmm0 = mem[0],zero
	movsd	%xmm0, 72(%rcx)
	movsd	(%rdi), %xmm0                   # xmm0 = mem[0],zero
	xorps	%xmm1, %xmm0
	movlps	%xmm0, 88(%rcx)
	movsd	8(%rdi), %xmm0                  # xmm0 = mem[0],zero
	xorps	%xmm1, %xmm0
	movlps	%xmm0, 120(%rcx)
	movsd	(%rdi), %xmm0                   # xmm0 = mem[0],zero
	movsd	%xmm0, 128(%rcx)
	leaq	144(%rcx), %rsi
	xorl	%edi, %edi
.LBB9_1:                                # %.preheader
                                        # =>This Loop Header: Depth=1
                                        #     Child Loop BB9_2 Depth 2
	xorl	%r8d, %r8d
.LBB9_2:                                #   Parent Loop BB9_1 Depth=1
                                        # =>  This Inner Loop Header: Depth=2
	movq	$0, (%rsi,%r8)
	addq	$48, %r8
	cmpq	$144, %r8
	jne	.LBB9_2
# %bb.3:                                #   in Loop: Header=BB9_1 Depth=1
	incq	%rdi
	addq	$8, %rsi
	cmpq	$3, %rdi
	jne	.LBB9_1
# %bb.4:
	movq	%rax, %rdi
	movq	%rcx, %rsi
	jmp	_Z9Mat66MultPA6_dS0_S0_         # TAILCALL
.Lfunc_end9:
	.size	_Z3zabPdPA6_dS1_S1_, .Lfunc_end9-_Z3zabPdPA6_dS1_S1_
	.cfi_endproc
                                        # -- End function
	.section	.rodata.cst16,"aM",@progbits,16
	.p2align	4, 0x0                          # -- Begin function _Z6makeStPA6_dPd
.LCPI10_0:
	.quad	0x8000000000000000              # double -0
	.quad	0x8000000000000000              # double -0
	.text
	.globl	_Z6makeStPA6_dPd
	.type	_Z6makeStPA6_dPd,@function
_Z6makeStPA6_dPd:                       # @_Z6makeStPA6_dPd
	.cfi_startproc
# %bb.0:
	movsd	16(%rsi), %xmm0                 # xmm0 = mem[0],zero
	movaps	.LCPI10_0(%rip), %xmm1          # xmm1 = [-0.0E+0,-0.0E+0]
	xorps	%xmm1, %xmm0
	movlps	%xmm0, 192(%rdi)
	movsd	8(%rsi), %xmm0                  # xmm0 = mem[0],zero
	movsd	%xmm0, 240(%rdi)
	movsd	16(%rsi), %xmm0                 # xmm0 = mem[0],zero
	movsd	%xmm0, 152(%rdi)
	movsd	(%rsi), %xmm0                   # xmm0 = mem[0],zero
	xorps	%xmm1, %xmm0
	movlps	%xmm0, 248(%rdi)
	movsd	8(%rsi), %xmm0                  # xmm0 = mem[0],zero
	xorps	%xmm1, %xmm0
	movlps	%xmm0, 160(%rdi)
	movsd	(%rsi), %xmm0                   # xmm0 = mem[0],zero
	movsd	%xmm0, 208(%rdi)
	retq
.Lfunc_end10:
	.size	_Z6makeStPA6_dPd, .Lfunc_end10-_Z6makeStPA6_dPd
	.cfi_endproc
                                        # -- End function
	.section	.rodata.cst8,"aM",@progbits,8
	.p2align	3, 0x0                          # -- Begin function _Z7MSsetupPA6_dS0_dPA3_d
.LCPI11_0:
	.quad	0x3ff0000000000000              # double 1
	.text
	.globl	_Z7MSsetupPA6_dS0_dPA3_d
	.type	_Z7MSsetupPA6_dS0_dPA3_d,@function
_Z7MSsetupPA6_dS0_dPA3_d:               # @_Z7MSsetupPA6_dS0_dPA3_d
	.cfi_startproc
# %bb.0:
	pushq	%rbx
	.cfi_def_cfa_offset 16
	.cfi_offset %rbx, -16
	movsd	.LCPI11_0(%rip), %xmm1          # xmm1 = mem[0],zero
	movapd	%xmm1, %xmm2
	divsd	%xmm0, %xmm2
	xorl	%eax, %eax
	movabsq	$4607182418800017408, %rcx      # imm = 0x3FF0000000000000
	xorl	%r8d, %r8d
.LBB11_1:                               # %.preheader
                                        # =>This Loop Header: Depth=1
                                        #     Child Loop BB11_2 Depth 2
	leaq	(%r8,%r8,2), %r9
	leaq	(%rdx,%r9,8), %r10
	shlq	$4, %r9
	addq	%rdi, %r9
	leaq	(%r9,%r8,8), %r9
	leaq	(%r10,%r8,8), %r10
	movq	%rax, %r11
	xorl	%ebx, %ebx
.LBB11_2:                               #   Parent Loop BB11_1 Depth=1
                                        # =>  This Inner Loop Header: Depth=2
	movq	$0, (%rsi,%r11)
	cmpq	$2, %rbx
	ja	.LBB11_7
# %bb.3:                                #   in Loop: Header=BB11_2 Depth=2
	cmpq	%rbx, %r8
	jne	.LBB11_7
# %bb.4:                                #   in Loop: Header=BB11_2 Depth=2
	movapd	%xmm1, %xmm0
	divsd	(%r10), %xmm0
	movsd	%xmm0, (%r9)
	jmp	.LBB11_11
.LBB11_7:                               #   in Loop: Header=BB11_2 Depth=2
	cmpq	$3, %rbx
	jb	.LBB11_10
# %bb.8:                                #   in Loop: Header=BB11_2 Depth=2
	cmpq	%rbx, %r8
	jne	.LBB11_10
# %bb.9:                                #   in Loop: Header=BB11_2 Depth=2
	movsd	%xmm2, (%r9)
	jmp	.LBB11_11
.LBB11_10:                              #   in Loop: Header=BB11_2 Depth=2
	movq	$0, (%rdi,%r11)
.LBB11_11:                              #   in Loop: Header=BB11_2 Depth=2
	cmpq	%rbx, %r8
	jne	.LBB11_13
# %bb.12:                               #   in Loop: Header=BB11_2 Depth=2
	movq	%rcx, (%rsi,%r11)
.LBB11_13:                              #   in Loop: Header=BB11_2 Depth=2
	incq	%rbx
	addq	$48, %r11
	cmpq	$6, %rbx
	jne	.LBB11_2
# %bb.5:                                #   in Loop: Header=BB11_1 Depth=1
	incq	%r8
	addq	$8, %rax
	cmpq	$6, %r8
	jne	.LBB11_1
# %bb.6:
	popq	%rbx
	.cfi_def_cfa_offset 8
	retq
.Lfunc_end11:
	.size	_Z7MSsetupPA6_dS0_dPA3_d, .Lfunc_end11-_Z7MSsetupPA6_dS0_dPA3_d
	.cfi_endproc
                                        # -- End function
	.section	.rodata.cst16,"aM",@progbits,16
	.p2align	4, 0x0                          # -- Begin function _Z2FaPA6_ddPdd
.LCPI12_0:
	.quad	0x8000000000000000              # double -0
	.quad	0x8000000000000000              # double -0
	.section	.rodata.cst8,"aM",@progbits,8
	.p2align	3, 0x0
.LCPI12_1:
	.quad	0xc0239eb851eb851f              # double -9.8100000000000004
	.text
	.globl	_Z2FaPA6_ddPdd
	.type	_Z2FaPA6_ddPdd,@function
_Z2FaPA6_ddPdd:                         # @_Z2FaPA6_ddPdd
	.cfi_startproc
# %bb.0:
	xorl	%eax, %eax
.LBB12_1:                               # =>This Inner Loop Header: Depth=1
	movq	$0, (%rdi,%rax)
	addq	$48, %rax
	cmpq	$288, %rax                      # imm = 0x120
	jne	.LBB12_1
# %bb.2:
	movapd	.LCPI12_0(%rip), %xmm2          # xmm2 = [-0.0E+0,-0.0E+0]
	xorpd	%xmm1, %xmm2
	mulsd	%xmm0, %xmm2
	mulsd	%xmm0, %xmm2
	mulsd	(%rsi), %xmm2
	movsd	%xmm2, 144(%rdi)
	movapd	%xmm0, %xmm2
	mulsd	%xmm1, %xmm2
	mulsd	.LCPI12_1(%rip), %xmm1
	mulsd	%xmm0, %xmm2
	mulsd	8(%rsi), %xmm2
	subsd	%xmm2, %xmm1
	movsd	%xmm1, 192(%rdi)
	retq
.Lfunc_end12:
	.size	_Z2FaPA6_ddPdd, .Lfunc_end12-_Z2FaPA6_ddPdd
	.cfi_endproc
                                        # -- End function
	.section	.rodata.cst8,"aM",@progbits,8
	.p2align	3, 0x0                          # -- Begin function _Z3r02Pdd
.LCPI13_0:
	.quad	0xbfe0000000000000              # double -0.5
	.text
	.globl	_Z3r02Pdd
	.type	_Z3r02Pdd,@function
_Z3r02Pdd:                              # @_Z3r02Pdd
	.cfi_startproc
# %bb.0:
	xorl	%eax, %eax
	movq	%rax, (%rdi)
	mulsd	.LCPI13_0(%rip), %xmm0
	movsd	%xmm0, 8(%rdi)
	movq	%rax, 16(%rdi)
	retq
.Lfunc_end13:
	.size	_Z3r02Pdd, .Lfunc_end13-_Z3r02Pdd
	.cfi_endproc
                                        # -- End function
	.section	.rodata.cst8,"aM",@progbits,8
	.p2align	3, 0x0                          # -- Begin function _Z3r01Pdd
.LCPI14_0:
	.quad	0x3fe0000000000000              # double 0.5
	.text
	.globl	_Z3r01Pdd
	.type	_Z3r01Pdd,@function
_Z3r01Pdd:                              # @_Z3r01Pdd
	.cfi_startproc
# %bb.0:
	xorl	%eax, %eax
	movq	%rax, (%rdi)
	mulsd	.LCPI14_0(%rip), %xmm0
	movsd	%xmm0, 8(%rdi)
	movq	%rax, 16(%rdi)
	retq
.Lfunc_end14:
	.size	_Z3r01Pdd, .Lfunc_end14-_Z3r01Pdd
	.cfi_endproc
                                        # -- End function
	.globl	_Z9Mat61MultPA6_dS0_S0_         # -- Begin function _Z9Mat61MultPA6_dS0_S0_
	.type	_Z9Mat61MultPA6_dS0_S0_,@function
_Z9Mat61MultPA6_dS0_S0_:                # @_Z9Mat61MultPA6_dS0_S0_
	.cfi_startproc
# %bb.0:
	xorl	%eax, %eax
.LBB15_1:                               # =>This Loop Header: Depth=1
                                        #     Child Loop BB15_2 Depth 2
	xorpd	%xmm0, %xmm0
	movq	%rsi, %rcx
	xorl	%r8d, %r8d
.LBB15_2:                               #   Parent Loop BB15_1 Depth=1
                                        # =>  This Inner Loop Header: Depth=2
	movsd	(%rdi,%r8,8), %xmm1             # xmm1 = mem[0],zero
	mulsd	(%rcx), %xmm1
	addsd	%xmm1, %xmm0
	incq	%r8
	addq	$48, %rcx
	cmpq	$6, %r8
	jne	.LBB15_2
# %bb.3:                                #   in Loop: Header=BB15_1 Depth=1
	movsd	%xmm0, -56(%rsp,%rax,8)
	incq	%rax
	addq	$48, %rdi
	cmpq	$6, %rax
	jne	.LBB15_1
# %bb.4:                                # %.preheader.preheader
	xorl	%eax, %eax
.LBB15_5:                               # %.preheader
                                        # =>This Inner Loop Header: Depth=1
	movsd	-56(%rsp,%rax,8), %xmm0         # xmm0 = mem[0],zero
	movsd	%xmm0, (%rdx)
	incq	%rax
	addq	$48, %rdx
	cmpq	$6, %rax
	jne	.LBB15_5
# %bb.6:
	retq
.Lfunc_end15:
	.size	_Z9Mat61MultPA6_dS0_S0_, .Lfunc_end15-_Z9Mat61MultPA6_dS0_S0_
	.cfi_endproc
                                        # -- End function
	.globl	_Z6updatePA6_dS0_PdS1_iii       # -- Begin function _Z6updatePA6_dS0_PdS1_iii
	.type	_Z6updatePA6_dS0_PdS1_iii,@function
_Z6updatePA6_dS0_PdS1_iii:              # @_Z6updatePA6_dS0_PdS1_iii
	.cfi_startproc
# %bb.0:
	pushq	%rbp
	.cfi_def_cfa_offset 16
	pushq	%r15
	.cfi_def_cfa_offset 24
	pushq	%r14
	.cfi_def_cfa_offset 32
	pushq	%r13
	.cfi_def_cfa_offset 40
	pushq	%r12
	.cfi_def_cfa_offset 48
	pushq	%rbx
	.cfi_def_cfa_offset 56
	subq	$168, %rsp
	.cfi_def_cfa_offset 224
	.cfi_offset %rbx, -56
	.cfi_offset %r12, -48
	.cfi_offset %r13, -40
	.cfi_offset %r14, -32
	.cfi_offset %r15, -24
	.cfi_offset %rbp, -16
                                        # kill: def $r9d killed $r9d def $r9
                                        # kill: def $r8d killed $r8d def $r8
	leal	(%r9,%r9,4), %eax
	leal	(%rax,%rax,4), %r10d
	addl	%r9d, %r10d
	addl	224(%rsp), %r10d
	leal	(%r8,%r8,4), %eax
	leal	(%rax,%rax,4), %r11d
	addl	%r8d, %r11d
	xorl	%ebx, %ebx
.LBB16_1:                               # %.preheader49
                                        # =>This Loop Header: Depth=1
                                        #     Child Loop BB16_2 Depth 2
                                        #       Child Loop BB16_3 Depth 3
	leaq	(%rbx,%rbx,2), %rax
	shlq	$4, %rax
	leaq	(%rsp,%rax), %r14
	addq	$-128, %r14
	movq	%rsi, %r15
	xorl	%r12d, %r12d
.LBB16_2:                               #   Parent Loop BB16_1 Depth=1
                                        # =>  This Loop Header: Depth=2
                                        #       Child Loop BB16_3 Depth 3
	leaq	(%r14,%r12,8), %r13
	xorpd	%xmm0, %xmm0
	movq	$-288, %rbp                     # imm = 0xFEE0
	movl	%r10d, %eax
.LBB16_3:                               #   Parent Loop BB16_1 Depth=1
                                        #     Parent Loop BB16_2 Depth=2
                                        # =>    This Inner Loop Header: Depth=3
	cltq
	movsd	(%rdx,%rax,8), %xmm1            # xmm1 = mem[0],zero
	mulsd	288(%r15,%rbp), %xmm1
	addsd	%xmm1, %xmm0
	incl	%eax
	addq	$48, %rbp
	jne	.LBB16_3
# %bb.4:                                #   in Loop: Header=BB16_2 Depth=2
	movsd	%xmm0, (%r13)
	incq	%r12
	addq	$8, %r15
	cmpq	$6, %r12
	jne	.LBB16_2
# %bb.5:                                #   in Loop: Header=BB16_1 Depth=1
	incq	%rbx
	addl	%r11d, %r10d
	cmpq	$6, %rbx
	jne	.LBB16_1
# %bb.6:                                # %.preheader.preheader
	xorl	%edx, %edx
	leaq	-128(%rsp), %rsi
	movl	224(%rsp), %ebp
.LBB16_7:                               # %.preheader
                                        # =>This Loop Header: Depth=1
                                        #     Child Loop BB16_8 Depth 2
                                        #       Child Loop BB16_9 Depth 3
	movl	%edx, %r10d
	imull	%r8d, %r10d
	addl	%r9d, %r10d
	leal	(%r10,%r10,4), %eax
	leal	(%rax,%rax,4), %eax
	addl	%ebp, %r10d
	addl	%eax, %r10d
	movq	%rsi, %r11
	xorl	%ebx, %ebx
.LBB16_8:                               #   Parent Loop BB16_7 Depth=1
                                        # =>  This Loop Header: Depth=2
                                        #       Child Loop BB16_9 Depth 3
	leal	(%r10,%rbx), %eax
	movslq	%eax, %r14
	movq	$0, (%rcx,%r14,8)
	xorpd	%xmm0, %xmm0
	movq	%r11, %rax
	xorl	%r15d, %r15d
.LBB16_9:                               #   Parent Loop BB16_7 Depth=1
                                        #     Parent Loop BB16_8 Depth=2
                                        # =>    This Inner Loop Header: Depth=3
	movsd	(%rdi,%r15,8), %xmm1            # xmm1 = mem[0],zero
	mulsd	(%rax), %xmm1
	addsd	%xmm1, %xmm0
	movsd	%xmm0, (%rcx,%r14,8)
	incq	%r15
	addq	$48, %rax
	cmpq	$6, %r15
	jne	.LBB16_9
# %bb.10:                               #   in Loop: Header=BB16_8 Depth=2
	incq	%rbx
	addq	$8, %r11
	cmpq	$6, %rbx
	jne	.LBB16_8
# %bb.11:                               #   in Loop: Header=BB16_7 Depth=1
	incq	%rdx
	addq	$48, %rdi
	cmpq	$6, %rdx
	jne	.LBB16_7
# %bb.12:
	addq	$168, %rsp
	.cfi_def_cfa_offset 56
	popq	%rbx
	.cfi_def_cfa_offset 48
	popq	%r12
	.cfi_def_cfa_offset 40
	popq	%r13
	.cfi_def_cfa_offset 32
	popq	%r14
	.cfi_def_cfa_offset 24
	popq	%r15
	.cfi_def_cfa_offset 16
	popq	%rbp
	.cfi_def_cfa_offset 8
	retq
.Lfunc_end16:
	.size	_Z6updatePA6_dS0_PdS1_iii, .Lfunc_end16-_Z6updatePA6_dS0_PdS1_iii
	.cfi_endproc
                                        # -- End function
	.globl	_Z6savez3PA6_dS0_PdS1_iii       # -- Begin function _Z6savez3PA6_dS0_PdS1_iii
	.type	_Z6savez3PA6_dS0_PdS1_iii,@function
_Z6savez3PA6_dS0_PdS1_iii:              # @_Z6savez3PA6_dS0_PdS1_iii
	.cfi_startproc
# %bb.0:
	pushq	%rbp
	.cfi_def_cfa_offset 16
	pushq	%r15
	.cfi_def_cfa_offset 24
	pushq	%r14
	.cfi_def_cfa_offset 32
	pushq	%r13
	.cfi_def_cfa_offset 40
	pushq	%r12
	.cfi_def_cfa_offset 48
	pushq	%rbx
	.cfi_def_cfa_offset 56
	subq	$296, %rsp                      # imm = 0x128
	.cfi_def_cfa_offset 352
	.cfi_offset %rbx, -56
	.cfi_offset %r12, -48
	.cfi_offset %r13, -40
	.cfi_offset %r14, -32
	.cfi_offset %r15, -24
	.cfi_offset %rbp, -16
	movl	%r9d, %ebx
	movl	%r8d, %ebp
	movq	%rcx, %r14
	movq	%rdx, %r15
	movq	%rdi, %rax
	movl	352(%rsp), %r13d
	movq	%rsp, %r12
	movq	%rsi, %rdi
	movq	%rax, %rsi
	movq	%r12, %rdx
	callq	_Z9Mat66MultPA6_dS0_S0_
	xorl	%eax, %eax
.LBB17_1:                               # =>This Loop Header: Depth=1
                                        #     Child Loop BB17_2 Depth 2
	movl	%eax, %ecx
	imull	%ebp, %ecx
	addl	%ebx, %ecx
	leal	(%rcx,%rcx,4), %edx
	leal	(%rdx,%rdx,4), %edx
	addl	%r13d, %ecx
	addl	%edx, %ecx
	movslq	%ecx, %rcx
	movq	$0, (%r14,%rcx,8)
	xorpd	%xmm0, %xmm0
	xorl	%edx, %edx
.LBB17_2:                               #   Parent Loop BB17_1 Depth=1
                                        # =>  This Inner Loop Header: Depth=2
	movsd	(%r12,%rdx,8), %xmm1            # xmm1 = mem[0],zero
	mulsd	(%r15,%rdx,8), %xmm1
	addsd	%xmm1, %xmm0
	movsd	%xmm0, (%r14,%rcx,8)
	incq	%rdx
	cmpq	$6, %rdx
	jne	.LBB17_2
# %bb.3:                                #   in Loop: Header=BB17_1 Depth=1
	incq	%rax
	addq	$48, %r12
	cmpq	$6, %rax
	jne	.LBB17_1
# %bb.4:
	addq	$296, %rsp                      # imm = 0x128
	.cfi_def_cfa_offset 56
	popq	%rbx
	.cfi_def_cfa_offset 48
	popq	%r12
	.cfi_def_cfa_offset 40
	popq	%r13
	.cfi_def_cfa_offset 32
	popq	%r14
	.cfi_def_cfa_offset 24
	popq	%r15
	.cfi_def_cfa_offset 16
	popq	%rbp
	.cfi_def_cfa_offset 8
	retq
.Lfunc_end17:
	.size	_Z6savez3PA6_dS0_PdS1_iii, .Lfunc_end17-_Z6savez3PA6_dS0_PdS1_iii
	.cfi_endproc
                                        # -- End function
	.section	.rodata.cst16,"aM",@progbits,16
	.p2align	4, 0x0                          # -- Begin function _Z5makeRPA6_dS0_d
.LCPI18_0:
	.quad	0x8000000000000000              # double -0
	.quad	0x8000000000000000              # double -0
	.text
	.globl	_Z5makeRPA6_dS0_d
	.type	_Z5makeRPA6_dS0_d,@function
_Z5makeRPA6_dS0_d:                      # @_Z5makeRPA6_dS0_d
	.cfi_startproc
# %bb.0:
	pushq	%r14
	.cfi_def_cfa_offset 16
	pushq	%rbx
	.cfi_def_cfa_offset 24
	pushq	%rax
	.cfi_def_cfa_offset 32
	.cfi_offset %rbx, -24
	.cfi_offset %r14, -16
	movq	%rsi, %rbx
	movq	%rdi, %r14
	xorl	%eax, %eax
	movabsq	$4607182418800017408, %rcx      # imm = 0x3FF0000000000000
	movq	%rdi, %rdx
	xorl	%edi, %edi
.LBB18_1:                               # %.preheader
                                        # =>This Loop Header: Depth=1
                                        #     Child Loop BB18_2 Depth 2
	xorl	%r8d, %r8d
.LBB18_2:                               #   Parent Loop BB18_1 Depth=1
                                        # =>  This Inner Loop Header: Depth=2
	movq	%rax, (%rdx,%r8,8)
	movq	%rax, (%rsi,%r8,8)
	incq	%r8
	cmpq	$6, %r8
	jne	.LBB18_2
# %bb.3:                                #   in Loop: Header=BB18_1 Depth=1
	movq	%rdi, %r8
	shlq	$4, %r8
	leaq	(%r8,%r8,2), %r8
	leaq	(%r14,%r8), %r9
	movq	%rcx, (%r9,%rdi,8)
	addq	%rbx, %r8
	movq	%rcx, (%r8,%rdi,8)
	incq	%rdi
	addq	$48, %rsi
	addq	$48, %rdx
	cmpq	$6, %rdi
	jne	.LBB18_1
# %bb.4:
	movsd	%xmm0, (%rsp)                   # 8-byte Spill
	callq	cos
	movsd	%xmm0, 168(%r14)
	movsd	(%rsp), %xmm0                   # 8-byte Reload
                                        # xmm0 = mem[0],zero
	callq	cos
	movsd	%xmm0, 224(%r14)
	movsd	(%rsp), %xmm0                   # 8-byte Reload
                                        # xmm0 = mem[0],zero
	callq	cos
	movsd	%xmm0, 168(%rbx)
	movsd	(%rsp), %xmm0                   # 8-byte Reload
                                        # xmm0 = mem[0],zero
	callq	cos
	movsd	%xmm0, 224(%rbx)
	movsd	(%rsp), %xmm0                   # 8-byte Reload
                                        # xmm0 = mem[0],zero
	callq	sin
	xorps	.LCPI18_0(%rip), %xmm0
	movlps	%xmm0, 176(%r14)
	movsd	(%rsp), %xmm0                   # 8-byte Reload
                                        # xmm0 = mem[0],zero
	callq	sin
	movsd	%xmm0, 176(%rbx)
	movsd	(%rsp), %xmm0                   # 8-byte Reload
                                        # xmm0 = mem[0],zero
	callq	sin
	movsd	%xmm0, 216(%r14)
	movsd	(%rsp), %xmm0                   # 8-byte Reload
                                        # xmm0 = mem[0],zero
	callq	sin
	xorps	.LCPI18_0(%rip), %xmm0
	movlps	%xmm0, 216(%rbx)
	addq	$8, %rsp
	.cfi_def_cfa_offset 24
	popq	%rbx
	.cfi_def_cfa_offset 16
	popq	%r14
	.cfi_def_cfa_offset 8
	retq
.Lfunc_end18:
	.size	_Z5makeRPA6_dS0_d, .Lfunc_end18-_Z5makeRPA6_dS0_d
	.cfi_endproc
                                        # -- End function
	.type	__hip_cuid_,@object             # @__hip_cuid_
	.bss
	.globl	__hip_cuid_
__hip_cuid_:
	.byte	0                               # 0x0
	.size	__hip_cuid_, 1

	.ident	"AMD clang version 18.0.0git (https://github.com/RadeonOpenCompute/llvm-project roc-6.3.2 25012 e5bf7e55c91490b07c49d8960fa7983d864936c4)"
	.section	".note.GNU-stack","",@progbits
	.addrsig
	.addrsig_sym __hip_cuid_

	.text
	.p2alignl 7, 3214868480
	.fill 96, 4, 3214868480
	.type	__hip_cuid_,@object             ; @__hip_cuid_
	.section	.bss,"aw",@nobits
	.globl	__hip_cuid_
__hip_cuid_:
	.byte	0                               ; 0x0
	.size	__hip_cuid_, 1

	.ident	"AMD clang version 18.0.0git (https://github.com/RadeonOpenCompute/llvm-project roc-6.3.2 25012 e5bf7e55c91490b07c49d8960fa7983d864936c4)"
	.section	".note.GNU-stack","",@progbits
	.addrsig
	.addrsig_sym __hip_cuid_
	.amdgcn_target "amdgcn-amd-amdhsa--gfx1100"
	.amdgpu_metadata
---
amdhsa.kernels:  []
amdhsa.target:   amdgcn-amd-amdhsa--gfx1100
amdhsa.version:
  - 1
  - 2
...

	.end_amdgpu_metadata
