user: 请从给定的汇编指令推导出C源代码:

```asm
<MatrixExp6>:
  endbr64
  push   %r14
  push   %r13
  push   %r12
  push   %rbp
  mov    %rdi,%rbp
  push   %rbx
  mov    %rsi,%rbx
  sub    $0x200,%rsp
  mov    %fs:0x28,%rax
  mov    %rax,0x1f8(%rsp)
  xor    %eax,%eax
  lea    0xc0(%rsp),%r14
  lea    0x40(%rsp),%r13
  mov    %r13,%rdx
  mov    %r14,%rsi
  lea    0x80(%rsp),%r12
  call   <TransToRp@plt>
  mov    %r12,%rsi
  mov    %r14,%rdi
  call   <so3ToVec@plt>
  mov    %r12,%rdi
  call   <Vec3Norm2@plt>
  movsd  D35(%rip),%xmm1
  comisd %xmm0,%xmm1
  jbe    L6
  movq   D12(%rip),%xmm0
  movsd  D12(%rip),%xmm1
  movq   $0x0,0x10(%rbx)
  movq   $0x0,0x20(%rbx)
  movq   $0x0,0x30(%rbx)
  movq   $0x0,0x40(%rbx)
  movq   $0x0,0x48(%rbx)
  movups %xmm0,(%rbx)
  movsd  %xmm1,0x28(%rbx)
  movsd  %xmm1,0x50(%rbx)
  movsd  0x18(%rbp),%xmm0
  movsd  %xmm0,0x18(%rbx)
  movsd  0x38(%rbp),%xmm0
  movsd  %xmm0,0x38(%rbx)
  movsd  0x58(%rbp),%xmm0
L19:
  movq   $0x0,0x60(%rbx)
  movq   $0x0,0x68(%rbx)
  movq   $0x0,0x70(%rbx)
  movsd  %xmm0,0x58(%rbx)
  movsd  %xmm1,0x78(%rbx)
  mov    0x1f8(%rsp),%rax
  sub    %fs:0x28,%rax
  jne    L8
  add    $0x200,%rsp
  pop    %rbx
  pop    %rbp
  pop    %r12
  pop    %r13
  pop    %r14
  ret
  nopl   0x0(%rax,%rax,1)
L6:
  lea    0x38(%rsp),%rdx
  mov    %r12,%rdi
  lea    0xa0(%rsp),%rsi
  lea    0x160(%rsp),%r12
  lea    0x110(%rsp),%rbp
  call   <AxisAng3@plt>
  mov    %r12,%rsi
  mov    %r14,%rdi
  call   <MatrixExp3@plt>
  movsd  D12(%rip),%xmm1
  mov    %rbp,%rsi
  mov    %r14,%rdi
  movapd 0x170(%rsp),%xmm0
  movapd 0x180(%rsp),%xmm7
  movapd 0x160(%rsp),%xmm3
  movsd  %xmm1,(%rsp)
  movapd 0x190(%rsp),%xmm6
  movlpd %xmm0,0x10(%rbx)
  shufpd $0x1,0x180(%rsp),%xmm0
  movups %xmm0,0x20(%rbx)
  movsd  0x1a0(%rsp),%xmm0
  movhpd %xmm7,0x30(%rbx)
  movups %xmm3,(%rbx)
  movups %xmm6,0x40(%rbx)
  movsd  %xmm0,0x50(%rbx)
  movapd %xmm1,%xmm0
  divsd  0x38(%rsp),%xmm0
  call   <Matrix3MultValue@plt>
  mov    %r12,%rdx
  mov    %rbp,%rsi
  mov    %rbp,%rdi
  call   <Matrix3Mult@plt>
  movsd  0x38(%rsp),%xmm3
  lea    0x28(%rsp),%rdi
  lea    0x20(%rsp),%rsi
  movapd %xmm3,%xmm6
  movapd %xmm3,%xmm0
  movsd  %xmm3,0x8(%rsp)
  unpcklpd %xmm6,%xmm6
  movaps %xmm6,0x10(%rsp)
  call   <sincos@plt>
  movsd  (%rsp),%xmm1
  pxor   %xmm10,%xmm10
  movsd  0x8(%rsp),%xmm3
  movapd 0x10(%rsp),%xmm6
  lea    0x60(%rsp),%rsi
  mov    %r13,%rdx
  movapd 0x120(%rsp),%xmm7
  mulsd  %xmm3,%xmm10
  movapd %xmm1,%xmm2
  subsd  0x20(%rsp),%xmm2
  movapd 0x170(%rsp),%xmm5
  movapd %xmm3,%xmm9
  subsd  0x28(%rsp),%xmm9
  movq   D12(%rip),%xmm11
  lea    0x1b0(%rsp),%rdi
  movapd 0x180(%rsp),%xmm8
  mulpd  %xmm6,%xmm11
  movapd %xmm2,%xmm0
  unpcklpd %xmm0,%xmm0
  movapd %xmm9,%xmm4
  mulpd  %xmm0,%xmm7
  unpcklpd %xmm4,%xmm4
  unpcklpd %xmm10,%xmm10
  mulpd  %xmm4,%xmm5
  mulpd  %xmm4,%xmm8
  mulsd  0x150(%rsp),%xmm2
  mulsd  0x1a0(%rsp),%xmm9
  addpd  %xmm10,%xmm7
  addpd  %xmm5,%xmm7
  addsd  %xmm3,%xmm2
  movapd 0x130(%rsp),%xmm5
  mulpd  %xmm0,%xmm5
  addsd  %xmm9,%xmm2
  movaps %xmm7,0x1c0(%rsp)
  movsd  %xmm2,0x1f0(%rsp)
  addpd  %xmm11,%xmm5
  addpd  %xmm8,%xmm5
  movapd 0x110(%rsp),%xmm8
  mulpd  %xmm0,%xmm8
  mulpd  0x140(%rsp),%xmm0
  movaps %xmm5,0x1d0(%rsp)
  addpd  %xmm11,%xmm8
  movapd 0x160(%rsp),%xmm11
  addpd  %xmm10,%xmm0
  mulpd  %xmm4,%xmm11
  mulpd  0x190(%rsp),%xmm4
  addpd  %xmm11,%xmm8
  addpd  %xmm4,%xmm0
  movaps %xmm8,0x1b0(%rsp)
  movaps %xmm0,0x1e0(%rsp)
  movapd 0x40(%rsp),%xmm0
  divpd  %xmm6,%xmm0
  movaps %xmm0,0x60(%rsp)
  movsd  0x50(%rsp),%xmm0
  divsd  %xmm3,%xmm0
  movsd  %xmm0,0x70(%rsp)
  call   <Matrix3MultVec@plt>
  movsd  0x40(%rsp),%xmm0
  movsd  (%rsp),%xmm1
  movsd  %xmm0,0x18(%rbx)
  movsd  0x48(%rsp),%xmm0
  movsd  %xmm0,0x38(%rbx)
  movsd  0x50(%rsp),%xmm0
  jmp    L19
L8:
  call   <__stack_chk_fail@plt>
```

tool_calls: name: disassemble_data
arguments: label: D35
datatype: byte[8]
name: disassemble_data
arguments: label: D12
datatype: f64

tool: {"D35": [0x8d,0xed,0xb5,0xa0,0xf7,0xc6,0xb0,0x3e]}

tool: D12: 1

assistant: ```c
void MatrixExp6(double se3Mat[4][4], double T[4][4]) {
  int i;
  int j;
  double so3mat[3][3];
  double omgmat[3][3];
  double temp[3][3];
  double Gtheta[3][3];
  double p[3];
  double v[3];
  double omgtheta[3];
  double omghat[3];
  double theta;
  double MatI3[3][3] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
  TransToRp(se3Mat, so3mat, p);
  so3ToVec(so3mat, omgtheta);
  if (Vec3Norm2(omgtheta) < 1.0E-6) {
    for (i = 0; i < 3; i++) {
      for (j = 0; j < 3; j++) {
        T[i][j] = MatI3[i][j];
      }
    }
    T[0][3] = se3Mat[0][3];
    T[1][3] = se3Mat[1][3];
    T[2][3] = se3Mat[2][3];
    T[3][0] = 0.0;
    T[3][1] = 0.0;
    T[3][2] = 0.0;
    T[3][3] = 1.0;
  } else {
    AxisAng3(omgtheta, omghat, &theta);
    MatrixExp3(so3mat, temp);
    for (i = 0; i < 3; i++) {
      for (j = 0; j < 3; j++) {
        T[i][j] = temp[i][j];
      }
    }
    Matrix3MultValue(so3mat, 1.0 / theta, omgmat);
    Matrix3Mult(omgmat, omgmat, temp);
    for (i = 0; i < 3; i++) {
      for (j = 0; j < 3; j++) {
        Gtheta[i][j] = MatI3[i][j] * theta + (1.0 - cos(theta)) * omgmat[i][j] + (theta - sin(theta)) * temp[i][j];
      }
    }
    v[0] = p[0] / theta;
    v[1] = p[1] / theta;
    v[2] = p[2] / theta;
    Matrix3MultVec(Gtheta, v, p);
    T[0][3] = p[0];
    T[1][3] = p[1];
    T[2][3] = p[2];
    T[3][0] = 0.0;
    T[3][1] = 0.0;
    T[3][2] = 0.0;
    T[3][3] = 1.0;
  }
  return;
}
```