// File: /workspace/github_c_makefile_star2_O0/Alessandro-Barbieri[P]SpaceLib/src/SPACELIB.c
// Function: dhtom
int dhtom(int jtype, real theta, real d, real b, real a, real alpha, real q,
	  MAT4 m)
{
	real ca,sa,ct,st;

	if (jtype==Pri)                        /* Prismatic joint */
		d+=q;
	else                                   /* Revolute joint */
	{
		if (jtype!=Rev)
		{
			fprintf(stderr,"Error (dhtom) : illegal joint type\n");
			return (NOTOK);
		}
		theta += q;
	}

	ca=(real) cos(alpha);  sa=(real)sin(alpha);
	ct=(real) cos(theta); st=(real)sin(theta);

	m[X][X] = ct;      m[X][Y] = -st*ca;  m[X][Z] = st*sa;
	m[Y][X] = st;      m[Y][Y] = ct*ca;   m[Y][Z] = -ct*sa;
	m[Z][X] =(real)0.; m[Z][Y] = sa;      m[Z][Z] = ca;
	m[U][X] =(real)0.; m[U][Y] =(real)0.; m[U][Z] = (real)0.;

	m[X][U] = a*ct-b*st;
	m[Y][U] = a*st+b*ct;
	m[Z][U] = d;
	m[U][U] =(real)1.;

	return (OK);
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x60, %rsp
movl %edi, -0x8(%rbp)
movsd %xmm0, -0x10(%rbp)
movsd %xmm1, -0x18(%rbp)
movsd %xmm2, -0x20(%rbp)
movsd %xmm3, -0x28(%rbp)
movsd %xmm4, -0x30(%rbp)
movsd %xmm5, -0x38(%rbp)
movq %rsi, -0x40(%rbp)
cmpl $0x1, -0x8(%rbp)
jne 0x98b4
movsd -0x38(%rbp), %xmm0
addsd -0x18(%rbp), %xmm0
movsd %xmm0, -0x18(%rbp)
jmp 0x98ed
cmpl $0x0, -0x8(%rbp)
je 0x98de
movq 0x8737(%rip), %rax      # 0x11ff8
movq (%rax), %rdi
leaq 0x5b49(%rip), %rsi      # 0xf414
movb $0x0, %al
callq 0x21b0
movl $0x0, -0x4(%rbp)
jmp 0x9a91
movsd -0x38(%rbp), %xmm0
addsd -0x10(%rbp), %xmm0
movsd %xmm0, -0x10(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x2120
movsd %xmm0, -0x48(%rbp)
movsd -0x30(%rbp), %xmm0
callq 0x2380
movsd %xmm0, -0x50(%rbp)
movsd -0x10(%rbp), %xmm0
callq 0x2120
movsd %xmm0, -0x58(%rbp)
movsd -0x10(%rbp), %xmm0
callq 0x2380
movsd %xmm0, -0x60(%rbp)
movsd -0x58(%rbp), %xmm0
movq -0x40(%rbp), %rax
movsd %xmm0, (%rax)
movsd -0x60(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd -0x48(%rbp), %xmm0
movq -0x40(%rbp), %rax
movsd %xmm0, 0x8(%rax)
movsd -0x60(%rbp), %xmm0
mulsd -0x50(%rbp), %xmm0
movq -0x40(%rbp), %rax
movsd %xmm0, 0x10(%rax)
movsd -0x60(%rbp), %xmm0
movq -0x40(%rbp), %rax
movsd %xmm0, 0x20(%rax)
movsd -0x58(%rbp), %xmm0
mulsd -0x48(%rbp), %xmm0
movq -0x40(%rbp), %rax
movsd %xmm0, 0x28(%rax)
movsd -0x58(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd -0x50(%rbp), %xmm0
movq -0x40(%rbp), %rax
movsd %xmm0, 0x30(%rax)
movq -0x40(%rbp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, 0x40(%rax)
movsd -0x50(%rbp), %xmm0
movq -0x40(%rbp), %rax
movsd %xmm0, 0x48(%rax)
movsd -0x48(%rbp), %xmm0
movq -0x40(%rbp), %rax
movsd %xmm0, 0x50(%rax)
movq -0x40(%rbp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, 0x60(%rax)
movq -0x40(%rbp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, 0x68(%rax)
movq -0x40(%rbp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, 0x70(%rax)
movsd -0x28(%rbp), %xmm0
movsd -0x58(%rbp), %xmm2
movsd -0x20(%rbp), %xmm1
mulsd -0x60(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movq -0x40(%rbp), %rax
movsd %xmm0, 0x18(%rax)
movsd -0x28(%rbp), %xmm0
movsd -0x60(%rbp), %xmm2
movsd -0x20(%rbp), %xmm1
mulsd -0x58(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movq -0x40(%rbp), %rax
movsd %xmm0, 0x38(%rax)
movsd -0x18(%rbp), %xmm0
movq -0x40(%rbp), %rax
movsd %xmm0, 0x58(%rax)
movq -0x40(%rbp), %rax
movsd 0x55db(%rip), %xmm0     # 0xf060
movsd %xmm0, 0x78(%rax)
movl $0x1, -0x4(%rbp)
movl -0x4(%rbp), %eax
addq $0x60, %rsp
popq %rbp
retq
nopw (%rax,%rax)