      subroutine rotation_matrix(q, U)
!-----------------------------------------------------------------------
! This subroutine constructs rotation matrix U from quaternion q.
!-----------------------------------------------------------------------
! This subroutine calculates RMSD using quaternions.
! It is based on the F90 routine bu E. Coutsias
! http://www.math.unm.edu/~vageli/homepage.html
! I (PFL) have just translated it, and I have changed the diagonalization
! subroutine.
! I also made some changes to make it suitable for Cart package.
!----------------------------------------------------------------------
!----------------------------------------------------------------------
! Copyright (C) 2004, 2005 Chaok Seok, Evangelos Coutsias and Ken Dill
!      UCSF, Univeristy of New Mexico, Seoul National University
! Witten by Chaok Seok and Evangelos Coutsias 2004.

! This library is free software; you can redistribute it and/or
! modify it under the terms of the GNU Lesser General Public
! License as published by the Free Software Foundation; either
! version 2.1 of the License, or (at your option) any later version.
!
! This library is distributed in the hope that it will be useful,
! but WITHOUT ANY WARRANTY; without even the implied warranty of
! MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
! Lesser General Public License for more details.
!
! You should have received a copy of the GNU Lesser General Public
! License along with this library; if not, write to the Free Software
! Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
!----------------------------------------------------------------------------
!----------------------------------------------------------------------
!  Copyright 2003-2014 Ecole Normale Supérieure de Lyon, 
!  Centre National de la Recherche Scientifique,
!  Université Claude Bernard Lyon 1. All rights reserved.
!
!  This work is registered with the Agency for the Protection of Programs 
!  as IDDN.FR.001.100009.000.S.P.2014.000.30625
!
!  Authors: P. Fleurat-Lessard, P. Dayal
!  Contact: optnpath@gmail.com
!
! This file is part of "Opt'n Path".
!
!  "Opt'n Path" is free software: you can redistribute it and/or modify
!  it under the terms of the GNU Affero General Public License as
!  published by the Free Software Foundation, either version 3 of the License,
!  or (at your option) any later version.
!
!  "Opt'n Path" is distributed in the hope that it will be useful,
!  but WITHOUT ANY WARRANTY; without even the implied warranty of
!
!  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
!  GNU Affero General Public License for more details.
!
!  You should have received a copy of the GNU Affero General Public License
!  along with "Opt'n Path". If not, see <http://www.gnu.org/licenses/>.
!
! Contact The Office of Technology Licensing, valorisation@ens-lyon.fr,
! for commercial licensing opportunities.
!----------------------------------------------------------------------

        Use VarTypes

      real(KREAL) :: q(4)
      real(KREAL) :: U(3,3)
      real(KREAL) :: q0,q1,q2,q3,b0,b1,b2,b3,q00,q01,q02,q03
      REAL(KREAL) :: q11,q12,q13,q22,q23,q33

      q0 = q(1)
      q1 = q(2)
      q2 = q(3)
      q3 = q(4)

      b0 = 2.0d0*q0
      b1 = 2.0d0*q1
      b2 = 2.0d0*q2
      b3 = 2.0d0*q3

      q00 = b0*q0-1.0d0
      q01 = b0*q1
      q02 = b0*q2
      q03 = b0*q3

      q11 = b1*q1
      q12 = b1*q2
      q13 = b1*q3

      q22 = b2*q2
      q23 = b2*q3

      q33 = b3*q3

      U(1,1) = q00+q11
      U(1,2) = q12-q03
      U(1,3) = q13+q02

      U(2,1) = q12+q03
      U(2,2) = q00+q22
      U(2,3) = q23-q01

      U(3,1) = q13-q02
      U(3,2) = q23+q01
      U(3,3) = q00+q33

      end
