// File: /workspace/github_cpp_makefile_star2_O2/Chrom3D[P]Chrom3D/src/Util.cpp
// Function: util::rotatePointsAroundArbitraryAxis(double, boost::numeric::ublas::vector<double, boost::numeric::ublas::unbounded_array<double, std::allocator<double>>>, boost::numeric::ublas::vector<double, boost::numeric::ublas::unbounded_array<double, std::allocator<double>>>, boost::numeric::ublas::matrix<double, boost::numeric::ublas::basic_row_major<unsigned long, long>, boost::numeric::ublas::unbounded_array<double, std::allocator<double>>>)
CoordinateMatrix rotatePointsAroundArbitraryAxis(double theta, CoordinateVector startPoint, CoordinateVector endPoint, CoordinateMatrix mat) {
  CoordinateVector rU = normalizeVector(startPoint-endPoint); // Normalized rotation vector

  double C = cos(theta);
  double S = sin(theta);
  double t = 1 - C;
  double ux = rU(0);
  double uy = rU(1);
  double uz = rU(2); 

  CoordinateMatrix rotMat(3,3);
  
  rotMat(0,0) = t*ux*ux + C;
  rotMat(1,0) = t*ux*uy + S*uz;
  rotMat(2,0) = t*ux*uz - S*uy;
  rotMat(0,1) = t*ux*uy - S*uz;
  rotMat(1,1) = t*uy*uy + C;
  rotMat(2,1) =  t*uy*uz + S*ux;
  rotMat(0,2) = t*ux*uz + S*uy;
  rotMat(1,2) = t*uy*uz - S*ux;
  rotMat(2,2) =  t*uz*uz + C;


  CoordinateMatrix res(mat.size1(),3);
  
  res = ublas::trans(ublas::prod(rotMat, ublas::trans(mat-repeatVector(startPoint,mat.size1())))); // rotated matrix at origo

  return res + repeatVector(startPoint,mat.size1()); // rotated matrix transformed back to original coordinate system
}

// Assembly:
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x140, %rsp            # imm = 0x140
movq %rcx, %r14
movq %rsi, %r15
movsd %xmm0, (%rsp)
movq %rdi, %rbx
leaq 0x40(%rsp), %rsi
movq %r15, (%rsi)
movq %rdx, 0x8(%rsi)
leaq 0xc0(%rsp), %rdi
callq 0x7b7e
leaq 0x78(%rsp), %rdi
leaq 0xc0(%rsp), %rsi
callq 0x6a3c
leaq 0xc0(%rsp), %rdi
callq 0x8012
movsd (%rsp), %xmm0
callq 0x6630
movsd %xmm0, 0x20(%rsp)
movsd (%rsp), %xmm0
callq 0x60a0
movsd %xmm0, (%rsp)
leaq 0x78(%rsp), %rdi
xorl %esi, %esi
callq 0x8638
movsd (%rax), %xmm0
movsd %xmm0, 0x38(%rsp)
leaq 0x78(%rsp), %rdi
pushq $0x1
popq %rsi
callq 0x8638
movsd (%rax), %xmm0
movsd %xmm0, 0x28(%rsp)
leaq 0x78(%rsp), %rdi
pushq $0x2
popq %rsi
callq 0x8638
movsd (%rax), %xmm0
movsd %xmm0, 0x10(%rsp)
leaq 0x40(%rsp), %rdi
pushq $0x3
popq %rdx
movq %rdx, %rsi
callq 0x7af6
leaq 0x40(%rsp), %rdi
xorl %esi, %esi
xorl %edx, %edx
callq 0x8614
movsd 0x194b2(%rip), %xmm0    # 0x20008
movsd 0x20(%rsp), %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, 0x30(%rsp)
movsd 0x38(%rsp), %xmm2
mulsd %xmm2, %xmm0
movsd %xmm0, 0x18(%rsp)
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, (%rax)
leaq 0x40(%rsp), %rdi
pushq $0x1
popq %rsi
xorl %edx, %edx
callq 0x8614
movsd (%rsp), %xmm1
mulsd 0x10(%rsp), %xmm1
movsd 0x18(%rsp), %xmm0
mulsd 0x28(%rsp), %xmm0
movsd %xmm0, 0x70(%rsp)
movsd %xmm1, 0x8(%rsp)
addsd %xmm1, %xmm0
movsd %xmm0, (%rax)
leaq 0x40(%rsp), %rdi
pushq $0x2
popq %rsi
xorl %edx, %edx
callq 0x8614
movsd (%rsp), %xmm1
mulsd 0x28(%rsp), %xmm1
movsd 0x18(%rsp), %xmm0
mulsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x18(%rsp)
movsd %xmm1, 0x68(%rsp)
subsd %xmm1, %xmm0
movsd %xmm0, (%rax)
leaq 0x40(%rsp), %rdi
pushq $0x1
popq %rdx
xorl %esi, %esi
callq 0x8614
movsd 0x70(%rsp), %xmm0
subsd 0x8(%rsp), %xmm0
movsd %xmm0, (%rax)
leaq 0x40(%rsp), %rdi
pushq $0x1
popq %rdx
movq %rdx, %rsi
callq 0x8614
movsd 0x30(%rsp), %xmm1
movsd 0x28(%rsp), %xmm0
mulsd %xmm0, %xmm1
movsd %xmm1, 0x8(%rsp)
mulsd %xmm1, %xmm0
addsd 0x20(%rsp), %xmm0
movsd %xmm0, (%rax)
leaq 0x40(%rsp), %rdi
pushq $0x2
popq %rsi
pushq $0x1
popq %rdx
callq 0x8614
movsd (%rsp), %xmm1
mulsd 0x38(%rsp), %xmm1
movsd 0x8(%rsp), %xmm0
mulsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x8(%rsp)
movsd %xmm1, (%rsp)
addsd %xmm1, %xmm0
movsd %xmm0, (%rax)
leaq 0x40(%rsp), %rdi
pushq $0x2
popq %rdx
xorl %esi, %esi
callq 0x8614
movsd 0x18(%rsp), %xmm0
addsd 0x68(%rsp), %xmm0
movsd %xmm0, (%rax)
leaq 0x40(%rsp), %rdi
pushq $0x1
popq %rsi
pushq $0x2
popq %rdx
callq 0x8614
movsd 0x8(%rsp), %xmm0
subsd (%rsp), %xmm0
movsd %xmm0, (%rax)
leaq 0x40(%rsp), %r12
pushq $0x2
popq %rdx
movq %r12, %rdi
movq %rdx, %rsi
callq 0x8614
movsd 0x10(%rsp), %xmm0
movsd 0x30(%rsp), %xmm1
mulsd %xmm0, %xmm1
mulsd %xmm0, %xmm1
addsd 0x20(%rsp), %xmm1
movsd %xmm1, (%rax)
movq (%r14), %rsi
leaq 0x100(%rsp), %rdi
pushq $0x3
popq %rdx
callq 0x7af6
leaq 0xa8(%rsp), %rdi
movq %r15, %rsi
callq 0x8932
movl (%r14), %edx
leaq 0xd8(%rsp), %r13
leaq 0xa8(%rsp), %rsi
movq %r13, %rdi
callq 0x6959
leaq 0x128(%rsp), %rsi
movq %r12, (%rsi)
movq %r14, 0x8(%rsi)
movq %r13, 0x10(%rsi)
leaq 0x100(%rsp), %r12
movq %r12, %rdi
callq 0x7bc6
leaq 0xe8(%rsp), %rdi
callq 0x8012
leaq 0xa8(%rsp), %rdi
callq 0x8012
leaq 0x90(%rsp), %rdi
movq %r15, %rsi
callq 0x8932
movl (%r14), %edx
leaq 0xd8(%rsp), %r14
leaq 0x90(%rsp), %rsi
movq %r14, %rdi
callq 0x6959
leaq 0x128(%rsp), %rsi
movq %r12, (%rsi)
movq %r14, 0x8(%rsi)
movq %rbx, %rdi
callq 0x7c10
leaq 0xe8(%rsp), %rdi
callq 0x8012
leaq 0x90(%rsp), %rdi
callq 0x8012
leaq 0x110(%rsp), %rdi
callq 0x8012
leaq 0x50(%rsp), %rdi
callq 0x8012
leaq 0x78(%rsp), %rdi
callq 0x8012
movq %rbx, %rax
addq $0x140, %rsp            # imm = 0x140
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
retq
movq %rax, %rbx
leaq 0xe8(%rsp), %rdi
callq 0x8012
jmp 0x6e13
movq %rax, %rbx
leaq 0x90(%rsp), %rdi
jmp 0x6e3c
jmp 0x6e43
movq %rax, %rbx
leaq 0xe8(%rsp), %rdi
callq 0x8012
jmp 0x6e34
movq %rax, %rbx
leaq 0xa8(%rsp), %rdi
callq 0x8012
jmp 0x6e46
movq %rax, %rbx
leaq 0x110(%rsp), %rdi
callq 0x8012
jmp 0x6e77
jmp 0x6e74
jmp 0x6e5d
jmp 0x6e5d
jmp 0x6e5d
movq %rax, %rbx
jmp 0x6e81
movq %rax, %rbx
leaq 0xc0(%rsp), %rdi
jmp 0x6e86
movq %rax, %rbx
jmp 0x6e8b
movq %rax, %rbx
leaq 0x50(%rsp), %rdi
callq 0x8012
leaq 0x78(%rsp), %rdi
callq 0x8012
movq %rbx, %rdi
callq 0x65d0