// File: 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:
subq $0x2c8, %rsp            # imm = 0x2C8
movq %rcx, 0xc8(%rsp)
movq %rsi, 0xd0(%rsp)
movq %rdi, 0xd8(%rsp)
movq %rdi, %rax
movq %rax, 0xe0(%rsp)
movq %rdi, 0x2c0(%rsp)
movsd %xmm0, 0x2b8(%rsp)
movq %rsi, 0x2b0(%rsp)
movq %rdx, 0x2a8(%rsp)
movq %rcx, 0x2a0(%rsp)
leaq 0x260(%rsp), %rdi
movq %rdi, 0xe8(%rsp)
callq 0xb0f0
movq 0xe8(%rsp), %rsi
leaq 0x270(%rsp), %rdi
callq 0xb4d0
jmp 0x901f
leaq 0x288(%rsp), %rdi
leaq 0x270(%rsp), %rsi
callq 0x8f00
jmp 0x9036
leaq 0x270(%rsp), %rdi
callq 0xb5b0
leaq 0x260(%rsp), %rdi
callq 0xb150
movsd 0x2b8(%rsp), %xmm0
callq 0x88f0
movsd %xmm0, 0x248(%rsp)
movsd 0x2b8(%rsp), %xmm0
callq 0x8100
movsd %xmm0, 0x240(%rsp)
movsd 0x248(%rsp), %xmm1
movsd 0x45f79(%rip), %xmm0    # 0x4f008
subsd %xmm1, %xmm0
movsd %xmm0, 0x238(%rsp)
xorl %eax, %eax
movl %eax, %esi
leaq 0x288(%rsp), %rdi
callq 0xb270
movq %rax, 0xc0(%rsp)
jmp 0x90b7
movq 0xc0(%rsp), %rax
movsd (%rax), %xmm0
movsd %xmm0, 0x230(%rsp)
leaq 0x288(%rsp), %rdi
movl $0x1, %esi
callq 0xb270
movq %rax, 0xb8(%rsp)
jmp 0x90e8
movq 0xb8(%rsp), %rax
movsd (%rax), %xmm0
movsd %xmm0, 0x228(%rsp)
leaq 0x288(%rsp), %rdi
movl $0x2, %esi
callq 0xb270
movq %rax, 0xb0(%rsp)
jmp 0x9119
movq 0xb0(%rsp), %rax
movsd (%rax), %xmm0
movsd %xmm0, 0x220(%rsp)
leaq 0x1f8(%rsp), %rdi
movl $0x3, %edx
movq %rdx, %rsi
callq 0xb1a0
jmp 0x9145
movsd 0x238(%rsp), %xmm0
movsd 0x230(%rsp), %xmm2
mulsd %xmm2, %xmm0
movsd 0x248(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xa0(%rsp)
xorl %eax, %eax
movl %eax, %edx
leaq 0x1f8(%rsp), %rdi
movq %rdx, %rsi
callq 0xb2a0
movq %rax, 0xa8(%rsp)
jmp 0x9193
movq 0xa8(%rsp), %rax
movsd 0xa0(%rsp), %xmm0
movsd %xmm0, (%rax)
movsd 0x238(%rsp), %xmm0
movsd 0x230(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x228(%rsp), %xmm2
movsd 0x240(%rsp), %xmm1
movsd 0x220(%rsp), %xmm3
mulsd %xmm3, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x90(%rsp)
xorl %eax, %eax
movl %eax, %edx
leaq 0x1f8(%rsp), %rdi
movl $0x1, %esi
callq 0xb2a0
movq %rax, 0x98(%rsp)
jmp 0x920e
movq 0x98(%rsp), %rax
movsd 0x90(%rsp), %xmm0
movsd %xmm0, (%rax)
movsd 0x238(%rsp), %xmm0
movsd 0x230(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x220(%rsp), %xmm2
movsd 0x240(%rsp), %xmm1
movsd 0x228(%rsp), %xmm3
mulsd %xmm3, %xmm1
mulsd %xmm2, %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x80(%rsp)
xorl %eax, %eax
movl %eax, %edx
leaq 0x1f8(%rsp), %rdi
movl $0x2, %esi
callq 0xb2a0
movq %rax, 0x88(%rsp)
jmp 0x9289
movq 0x88(%rsp), %rax
movsd 0x80(%rsp), %xmm0
movsd %xmm0, (%rax)
movsd 0x238(%rsp), %xmm0
movsd 0x230(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x228(%rsp), %xmm2
movsd 0x240(%rsp), %xmm1
movsd 0x220(%rsp), %xmm3
mulsd %xmm3, %xmm1
mulsd %xmm2, %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x70(%rsp)
xorl %eax, %eax
movl %eax, %esi
leaq 0x1f8(%rsp), %rdi
movl $0x1, %edx
callq 0xb2a0
movq %rax, 0x78(%rsp)
jmp 0x92fe
movq 0x78(%rsp), %rax
movsd 0x70(%rsp), %xmm0
movsd %xmm0, (%rax)
movsd 0x238(%rsp), %xmm0
movsd 0x228(%rsp), %xmm2
mulsd %xmm2, %xmm0
movsd 0x248(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x60(%rsp)
leaq 0x1f8(%rsp), %rdi
movl $0x1, %edx
movq %rdx, %rsi
callq 0xb2a0
movq %rax, 0x68(%rsp)
jmp 0x9356
movq 0x68(%rsp), %rax
movsd 0x60(%rsp), %xmm0
movsd %xmm0, (%rax)
movsd 0x238(%rsp), %xmm0
movsd 0x228(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x220(%rsp), %xmm2
movsd 0x240(%rsp), %xmm1
movsd 0x230(%rsp), %xmm3
mulsd %xmm3, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x50(%rsp)
leaq 0x1f8(%rsp), %rdi
movl $0x2, %esi
movl $0x1, %edx
callq 0xb2a0
movq %rax, 0x58(%rsp)
jmp 0x93c6
movq 0x58(%rsp), %rax
movsd 0x50(%rsp), %xmm0
movsd %xmm0, (%rax)
movsd 0x238(%rsp), %xmm0
movsd 0x230(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x220(%rsp), %xmm2
movsd 0x240(%rsp), %xmm1
movsd 0x228(%rsp), %xmm3
mulsd %xmm3, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x40(%rsp)
xorl %eax, %eax
movl %eax, %esi
leaq 0x1f8(%rsp), %rdi
movl $0x2, %edx
callq 0xb2a0
movq %rax, 0x48(%rsp)
jmp 0x9435
movq 0x48(%rsp), %rax
movsd 0x40(%rsp), %xmm0
movsd %xmm0, (%rax)
movsd 0x238(%rsp), %xmm0
movsd 0x228(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x220(%rsp), %xmm2
movsd 0x240(%rsp), %xmm1
movsd 0x230(%rsp), %xmm3
mulsd %xmm3, %xmm1
mulsd %xmm2, %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x30(%rsp)
leaq 0x1f8(%rsp), %rdi
movl $0x1, %esi
movl $0x2, %edx
callq 0xb2a0
movq %rax, 0x38(%rsp)
jmp 0x94a5
movq 0x38(%rsp), %rax
movsd 0x30(%rsp), %xmm0
movsd %xmm0, (%rax)
movsd 0x238(%rsp), %xmm0
movsd 0x220(%rsp), %xmm2
mulsd %xmm2, %xmm0
movsd 0x248(%rsp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x20(%rsp)
leaq 0x1f8(%rsp), %rdi
movl $0x2, %edx
movq %rdx, %rsi
callq 0xb2a0
movq %rax, 0x28(%rsp)
jmp 0x94fd
movq 0xc8(%rsp), %rdi
movq 0x28(%rsp), %rax
movsd 0x20(%rsp), %xmm0
movsd %xmm0, (%rax)
callq 0xb5e0
movq %rax, 0x18(%rsp)
jmp 0x9520
movq 0x18(%rsp), %rsi
leaq 0x1d0(%rsp), %rdi
movl $0x3, %edx
callq 0xb1a0
jmp 0x9539
movq 0xd0(%rsp), %rsi
leaq 0x140(%rsp), %rdi
callq 0xb710
jmp 0x9550
movq 0xc8(%rsp), %rdi
callq 0xb5e0
movq %rax, 0x10(%rsp)
jmp 0x9564
movq 0x10(%rsp), %rax
movl %eax, %edx
leaq 0x158(%rsp), %rdi
leaq 0x140(%rsp), %rsi
callq 0x8d30
jmp 0x9582
movq 0xc8(%rsp), %rsi
leaq 0x180(%rsp), %rdi
leaq 0x158(%rsp), %rdx
callq 0xb6b0
jmp 0x95a1
leaq 0x190(%rsp), %rdi
leaq 0x180(%rsp), %rsi
callq 0xb670
jmp 0x95b8
leaq 0x1a0(%rsp), %rdi
leaq 0x1f8(%rsp), %rsi
leaq 0x190(%rsp), %rdx
callq 0xb630
jmp 0x95d7
leaq 0x1b8(%rsp), %rdi
leaq 0x1a0(%rsp), %rsi
callq 0xb5f0
jmp 0x95ee
leaq 0x1d0(%rsp), %rdi
leaq 0x1b8(%rsp), %rsi
callq 0xb770
jmp 0x9605
leaq 0x1b8(%rsp), %rdi
callq 0xb7f0
leaq 0x1a0(%rsp), %rdi
callq 0xb820
leaq 0x190(%rsp), %rdi
callq 0xb860
leaq 0x180(%rsp), %rdi
callq 0xb890
leaq 0x158(%rsp), %rdi
callq 0xb2d0
leaq 0x140(%rsp), %rdi
callq 0xb5b0
movq 0xd0(%rsp), %rsi
leaq 0xf0(%rsp), %rdi
callq 0xb710
jmp 0x966a
movq 0xc8(%rsp), %rdi
callq 0xb5e0
movq %rax, 0x8(%rsp)
jmp 0x967e
movq 0x8(%rsp), %rax
movl %eax, %edx
leaq 0x108(%rsp), %rdi
leaq 0xf0(%rsp), %rsi
callq 0x8d30
jmp 0x969c
leaq 0x130(%rsp), %rdi
leaq 0x1d0(%rsp), %rsi
leaq 0x108(%rsp), %rdx
callq 0xb8d0
jmp 0x96bb
movq 0xd8(%rsp), %rdi
leaq 0x130(%rsp), %rsi
callq 0xb930
jmp 0x96d2
leaq 0x130(%rsp), %rdi
callq 0xba80
leaq 0x108(%rsp), %rdi
callq 0xb2d0
leaq 0xf0(%rsp), %rdi
callq 0xb5b0
leaq 0x1d0(%rsp), %rdi
callq 0xb2d0
leaq 0x1f8(%rsp), %rdi
callq 0xb2d0
leaq 0x288(%rsp), %rdi
callq 0xb5b0
movq 0xe0(%rsp), %rax
addq $0x2c8, %rsp            # imm = 0x2C8
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
jmp 0x9767
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
leaq 0x270(%rsp), %rdi
callq 0xb5b0
leaq 0x260(%rsp), %rdi
callq 0xb150
jmp 0x992a
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
jmp 0x991d
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
jmp 0x9910
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
jmp 0x9903
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
jmp 0x988d
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
jmp 0x9880
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
jmp 0x9873
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
jmp 0x9866
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
jmp 0x9859
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
leaq 0x1b8(%rsp), %rdi
callq 0xb7f0
leaq 0x1a0(%rsp), %rdi
callq 0xb820
leaq 0x190(%rsp), %rdi
callq 0xb860
leaq 0x180(%rsp), %rdi
callq 0xb890
leaq 0x158(%rsp), %rdi
callq 0xb2d0
leaq 0x140(%rsp), %rdi
callq 0xb5b0
jmp 0x9903
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
jmp 0x98f6
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
jmp 0x98e9
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x258(%rsp)
movl %eax, 0x254(%rsp)
leaq 0x130(%rsp), %rdi
callq 0xba80
leaq 0x108(%rsp), %rdi
callq 0xb2d0
leaq 0xf0(%rsp), %rdi
callq 0xb5b0
leaq 0x1d0(%rsp), %rdi
callq 0xb2d0
leaq 0x1f8(%rsp), %rdi
callq 0xb2d0
leaq 0x288(%rsp), %rdi
callq 0xb5b0
movq 0x258(%rsp), %rdi
callq 0x8870
nopw (%rax,%rax)