user: 请显示与下面列出的汇编代码相匹配的C语言实现：

```asm
<MatrixExp6>:
  endbr64
  push   %r14
  push   %r13
  push   %r12
  push   %rbp
  mov    %rdi,%rbp
  push   %rbx
  mov    %rsi,%rbx
  sub    $0x240,%rsp
  mov    %fs:0x28,%rax
  mov    %rax,0x238(%rsp)
  xor    %eax,%eax
  lea    0x30(%rsp),%r14
  lea    0x70(%rsp),%r12
  mov    D26(%rip),%rax
  lea    0xb0(%rsp),%r13
  mov    %r14,%rdx
  movq   $0x0,0x1f8(%rsp)
  mov    %r13,%rsi
  mov    %rax,0x210(%rsp)
  movq   $0x0,0x200(%rsp)
  movq   $0x0,0x208(%rsp)
  movq   $0x0,0x218(%rsp)
  movq   $0x0,0x220(%rsp)
  movq   $0x0,0x228(%rsp)
  mov    %rax,0x230(%rsp)
  call   <TransToRp@plt>
  mov    %r12,%rsi
  mov    %r13,%rdi
  call   <so3ToVec@plt>
  mov    %r12,%rdi
  call   <Vec3Norm2@plt>
  movsd  D38(%rip),%xmm1
  comisd %xmm0,%xmm1
  jbe    L44
  mov    D26(%rip),%rsi
  mov    %rbx,%rdx
  lea    0x1f0(%rsp),%rax
  lea    0x238(%rsp),%rcx
  movq   %rsi,%xmm0
L37:
  movsd  %xmm0,(%rdx)
  movsd  0x8(%rax),%xmm0
  add    $0x18,%rax
  add    $0x20,%rdx
  movsd  %xmm0,-0x18(%rdx)
  movsd  -0x8(%rax),%xmm0
  movsd  %xmm0,-0x10(%rdx)
  cmp    %rcx,%rax
  je     L9
  movsd  (%rax),%xmm0
  jmp    L37
  nopw   0x0(%rax,%rax,1)
L9:
  movsd  0x18(%rbp),%xmm0
  movsd  %xmm0,0x18(%rbx)
  movsd  0x38(%rbp),%xmm0
  movsd  %xmm0,0x38(%rbx)
  movsd  0x58(%rbp),%xmm0
L11:
  mov    D26(%rip),%rax
  movq   $0x0,0x60(%rbx)
  movq   $0x0,0x68(%rbx)
  movq   $0x0,0x70(%rbx)
  mov    %rax,0x78(%rbx)
  movsd  %xmm0,0x58(%rbx)
  mov    0x238(%rsp),%rax
  sub    %fs:0x28,%rax
  jne    L38
  add    $0x240,%rsp
  pop    %rbx
  pop    %rbp
  pop    %r12
  pop    %r13
  pop    %r14
  ret
  nopl   0x0(%rax,%rax,1)
L44:
  lea    0x28(%rsp),%rdx
  mov    %r12,%rdi
  lea    0x90(%rsp),%rsi
  lea    0x150(%rsp),%r12
  lea    0x100(%rsp),%rbp
  call   <AxisAng3@plt>
  mov    %r12,%rsi
  mov    %r13,%rdi
  call   <MatrixExp3@plt>
  mov    0x160(%rsp),%rax
  mov    %rbp,%rsi
  movdqa 0x150(%rsp),%xmm5
  movdqu 0x168(%rsp),%xmm6
  mov    %r13,%rdi
  movdqa 0x180(%rsp),%xmm7
  mov    %rax,0x10(%rbx)
  mov    0x178(%rsp),%rax
  movups %xmm5,(%rbx)
  movsd  D26(%rip),%xmm0
  divsd  0x28(%rsp),%xmm0
  mov    %rax,0x30(%rbx)
  mov    0x190(%rsp),%rax
  movups %xmm6,0x20(%rbx)
  mov    %rax,0x50(%rbx)
  movups %xmm7,0x40(%rbx)
  call   <Matrix3MultValue@plt>
  mov    %r12,%rdx
  mov    %rbp,%rsi
  mov    %rbp,%rdi
  call   <Matrix3Mult@plt>
  movsd  0x28(%rsp),%xmm2
  lea    0x18(%rsp),%rdi
  lea    0x10(%rsp),%rsi
  movapd %xmm2,%xmm0
  movsd  %xmm2,0x8(%rsp)
  call   <sincos@plt>
  movsd  0x8(%rsp),%xmm2
  mov    $0x18,%edx
  movsd  D26(%rip),%xmm4
  mov    D26(%rip),%rax
  subsd  0x10(%rsp),%xmm4
  lea    0x1a0(%rsp),%rdi
  movapd %xmm2,%xmm3
  subsd  0x18(%rsp),%xmm3
  movq   %rax,%xmm1
L68:
  lea    -0x18(%rdx),%rax
L71:
  movsd  0x0(%rbp,%rax,1),%xmm0
  mulsd  %xmm2,%xmm1
  mulsd  %xmm4,%xmm0
  addsd  %xmm1,%xmm0
  movsd  (%r12,%rax,1),%xmm1
  mulsd  %xmm3,%xmm1
  addsd  %xmm1,%xmm0
  movsd  %xmm0,(%rdi,%rax,1)
  add    $0x8,%rax
  cmp    %rdx,%rax
  je     L88
  movsd  0x1f0(%rsp,%rax,1),%xmm1
  jmp    L71
  nopl   0x0(%rax)
L88:
  cmp    $0x48,%rdx
  je     L63
  movsd  0x1f0(%rsp,%rdx,1),%xmm1
  add    $0x18,%rdx
  jmp    L68
  nopl   (%rax)
L63:
  movsd  0x30(%rsp),%xmm0
  lea    0x50(%rsp),%rsi
  mov    %r14,%rdx
  divsd  %xmm2,%xmm0
  movsd  %xmm0,0x50(%rsp)
  movsd  0x38(%rsp),%xmm0
  divsd  %xmm2,%xmm0
  movsd  %xmm0,0x58(%rsp)
  movsd  0x40(%rsp),%xmm0
  divsd  %xmm2,%xmm0
  movsd  %xmm0,0x60(%rsp)
  call   <Matrix3MultVec@plt>
  movsd  0x30(%rsp),%xmm0
  movsd  %xmm0,0x18(%rbx)
  movsd  0x38(%rsp),%xmm0
  movsd  %xmm0,0x38(%rbx)
  movsd  0x40(%rsp),%xmm0
  jmp    L11
L38:
  call   <__stack_chk_fail@plt>
```

tool_calls: name: read_data
arguments: data_label: D26
data_type: f64
name: read_data
arguments: data_label: D38
data_type: byte[8]

tool: D26: 1

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

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;
}
```