/* -*- c++ -*- ----------------------------------------------------------
   LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
   https://www.lammps.org/, Sandia National Laboratories
   LAMMPS development team: developers@lammps.org

   Copyright (2003) Sandia Corporation.  Under the terms of Contract
   DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
   certain rights in this software.  This software is distributed under
   the GNU General Public License.

   See the README file in the top-level LAMMPS directory.
------------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
   Contributing author: Mike Brown (SNL)
------------------------------------------------------------------------- */

#ifndef LMP_MATH_EXTRA_H
#define LMP_MATH_EXTRA_H

#include <cmath>

namespace MathExtra {

// 3 vector operations

inline void copy3(const double *v, double *ans);
inline void zero3(double *v);
inline void norm3(double *v);
inline void normalize3(const double *v, double *ans);
inline void snormalize3(const double, const double *v, double *ans);
inline void negate3(double *v);
inline void scale3(const double s, double *v);
inline void scale3(const double s, const double *v, double *ans);
inline void add3(const double *v1, const double *v2, double *ans);
inline void scaleadd3(const double s, const double *v1, const double *v2, double *ans);
inline void scaleadd3(const double s1, const double *v1, const double s2, const double *v2,
                      double *ans);
inline void sub3(const double *v1, const double *v2, double *ans);
inline double len3(const double *v);
inline double lensq3(const double *v);
inline double distsq3(const double *v1, const double *v2);
inline double dot3(const double *v1, const double *v2);
inline void cross3(const double *v1, const double *v2, double *ans);

// 3x3 matrix operations

inline void zeromat3(double m[3][3]);
inline void zeromat3(double **m);

inline void col2mat(const double *ex, const double *ey, const double *ez, double m[3][3]);
inline double det3(const double mat[3][3]);
inline void diag_times3(const double *d, const double m[3][3], double ans[3][3]);
inline void times3_diag(const double m[3][3], const double *d, double ans[3][3]);
inline void plus3(const double m[3][3], const double m2[3][3], double ans[3][3]);
inline void plus3(const double m[3][3], double **m2, double **ans);
inline void minus3(const double m[3][3], const double m2[3][3], double ans[3][3]);
inline void minus3(double **m, const double m2[3][3], double ans[3][3]);

inline void times3(const double m[3][3], const double m2[3][3], double ans[3][3]);
inline void transpose_times3(const double m[3][3], const double m2[3][3], double ans[3][3]);
inline void times3_transpose(const double m[3][3], const double m2[3][3], double ans[3][3]);
inline void invert3(const double mat[3][3], double ans[3][3]);
inline void matvec(const double mat[3][3], const double *vec, double *ans);
inline void matvec(const double *ex, const double *ey, const double *ez, const double *vec,
                   double *ans);
inline void transpose_matvec(const double mat[3][3], const double *vec, double *ans);
inline void transpose_matvec(const double *ex, const double *ey, const double *ez, const double *v,
                             double *ans);
inline void transpose_diag3(const double m[3][3], const double *d, double ans[3][3]);
inline void vecmat(const double *v, const double m[3][3], double *ans);
inline void scalar_times3(const double f, double m[3][3]);
inline void outer3(const double *v1, const double *v2, double ans[3][3]);

void write3(const double mat[3][3]);
int mldivide3(const double mat[3][3], const double *vec, double *ans);
void rotate(double matrix[3][3], int i, int j, int k, int l, double s, double tau);
void richardson(double *q, double *m, double *w, double *moments, double dtq);
void richardson_sphere(double *q, double *w, double dtq);
void no_squish_rotate(int k, double *p, double *q, double *inertia, double dt);

// shape matrix operations
// upper-triangular 3x3 matrix stored in Voigt ordering as 6-vector

inline void multiply_shape_shape(const double *one, const double *two, double *ans);

// quaternion operations

inline void qnormalize(double *q);
inline void qconjugate(double *q, double *qc);
inline void vecquat(double *a, double *b, double *c);
inline void quatvec(double *a, double *b, double *c);
inline void quatquat(double *a, double *b, double *c);
inline void invquatvec(double *a, double *b, double *c);
inline void quatrotvec(double *a, double *b, double *c);
inline void axisangle_to_quat(const double *v, const double angle, double *quat);

void angmom_to_omega(double *m, double *ex, double *ey, double *ez, double *idiag, double *w);
void omega_to_angmom(double *w, double *ex, double *ey, double *ez, double *idiag, double *m);
void mq_to_omega(double *m, double *q, double *moments, double *w);
void exyz_to_q(double *ex, double *ey, double *ez, double *q);
void q_to_exyz(double *q, double *ex, double *ey, double *ez);
void quat_to_mat(const double *quat, double mat[3][3]);
void quat_to_mat_trans(const double *quat, double mat[3][3]);

// rotation operations

inline void rotation_generator_x(const double m[3][3], double ans[3][3]);
inline void rotation_generator_y(const double m[3][3], double ans[3][3]);
inline void rotation_generator_z(const double m[3][3], double ans[3][3]);

void BuildRxMatrix(double R[3][3], const double angle);
void BuildRyMatrix(double R[3][3], const double angle);
void BuildRzMatrix(double R[3][3], const double angle);

// moment of inertia operations

void inertia_ellipsoid(double *shape, double *quat, double mass, double *inertia);
void inertia_line(double length, double theta, double mass, double *inertia);
void inertia_triangle(double *v0, double *v1, double *v2, double mass, double *inertia);
void inertia_triangle(double *idiag, double *quat, double mass, double *inertia);
}    // namespace MathExtra

/* ----------------------------------------------------------------------
   copy a vector, return in ans
------------------------------------------------------------------------- */

inline void MathExtra::copy3(const double *v, double *ans)
{
  ans[0] = v[0];
  ans[1] = v[1];
  ans[2] = v[2];
}

/* ----------------------------------------------------------------------
   set vector equal to zero
------------------------------------------------------------------------- */

inline void MathExtra::zero3(double *v)
{
  v[0] = 0.0;
  v[1] = 0.0;
  v[2] = 0.0;
}

/* ----------------------------------------------------------------------
   normalize a vector in place
------------------------------------------------------------------------- */

inline void MathExtra::norm3(double *v)
{
  const double val = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
  if (val > 0.0) {
    const double scale = 1.0 / sqrt(val);
    v[0] *= scale;
    v[1] *= scale;
    v[2] *= scale;
  }
}

/* ----------------------------------------------------------------------
   normalize a vector, return in ans
------------------------------------------------------------------------- */

inline void MathExtra::normalize3(const double *v, double *ans)
{
  const double val = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
  if (val > 0.0) {
    double scale = 1.0 / sqrt(val);
    ans[0] = v[0] * scale;
    ans[1] = v[1] * scale;
    ans[2] = v[2] * scale;
  }
}

/* ----------------------------------------------------------------------
   scale a vector to length
------------------------------------------------------------------------- */

inline void MathExtra::snormalize3(const double length, const double *v, double *ans)
{
  const double val = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
  if (val > 0.0) {
    double scale = length / sqrt(val);
    ans[0] = v[0] * scale;
    ans[1] = v[1] * scale;
    ans[2] = v[2] * scale;
  }
}

/* ----------------------------------------------------------------------
   negate vector v
------------------------------------------------------------------------- */

inline void MathExtra::negate3(double *v)
{
  v[0] = -v[0];
  v[1] = -v[1];
  v[2] = -v[2];
}

/* ----------------------------------------------------------------------
   scale vector v by s
------------------------------------------------------------------------- */

inline void MathExtra::scale3(const double s, double *v)
{
  v[0] *= s;
  v[1] *= s;
  v[2] *= s;
}

/* ----------------------------------------------------------------------
   scale vector v by s and store in ans
------------------------------------------------------------------------- */

inline void MathExtra::scale3(const double s, const double *v, double *ans)
{
  ans[0] = s * v[0];
  ans[1] = s * v[1];
  ans[2] = s * v[2];
}

/* ----------------------------------------------------------------------
   ans = v1 + v2
------------------------------------------------------------------------- */

inline void MathExtra::add3(const double *v1, const double *v2, double *ans)
{
  ans[0] = v1[0] + v2[0];
  ans[1] = v1[1] + v2[1];
  ans[2] = v1[2] + v2[2];
}

/* ----------------------------------------------------------------------
   ans = s*v1 + v2
------------------------------------------------------------------------- */

inline void MathExtra::scaleadd3(const double s, const double *v1, const double *v2, double *ans)
{
  ans[0] = s * v1[0] + v2[0];
  ans[1] = s * v1[1] + v2[1];
  ans[2] = s * v1[2] + v2[2];
}

/* ----------------------------------------------------------------------
   ans = s1*v1 + s2*v2
------------------------------------------------------------------------- */

inline void MathExtra::scaleadd3(const double s1, const double *v1, const double s2,
                                 const double *v2, double *ans)
{
  ans[0] = s1 * v1[0] + s2 * v2[0];
  ans[1] = s1 * v1[1] + s2 * v2[1];
  ans[2] = s1 * v1[2] + s2 * v2[2];
}

/* ----------------------------------------------------------------------
   ans = v1 - v2
------------------------------------------------------------------------- */

inline void MathExtra::sub3(const double *v1, const double *v2, double *ans)
{
  ans[0] = v1[0] - v2[0];
  ans[1] = v1[1] - v2[1];
  ans[2] = v1[2] - v2[2];
}

/* ----------------------------------------------------------------------
   length of vector v
------------------------------------------------------------------------- */

inline double MathExtra::len3(const double *v)
{
  return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
}

/* ----------------------------------------------------------------------
   squared length of vector v, or dot product of v with itself
------------------------------------------------------------------------- */

inline double MathExtra::lensq3(const double *v)
{
  return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
}

/* ----------------------------------------------------------------------
   ans = distance squared between pts v1 and v2
------------------------------------------------------------------------- */

inline double MathExtra::distsq3(const double *v1, const double *v2)
{
  double dx = v1[0] - v2[0];
  double dy = v1[1] - v2[1];
  double dz = v1[2] - v2[2];
  return dx * dx + dy * dy + dz * dz;
}

/* ----------------------------------------------------------------------
   dot product of 2 vectors
------------------------------------------------------------------------- */

inline double MathExtra::dot3(const double *v1, const double *v2)
{
  return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}

/* ----------------------------------------------------------------------
   cross product of 2 vectors
------------------------------------------------------------------------- */

inline void MathExtra::cross3(const double *v1, const double *v2, double *ans)
{
  ans[0] = v1[1] * v2[2] - v1[2] * v2[1];
  ans[1] = v1[2] * v2[0] - v1[0] * v2[2];
  ans[2] = v1[0] * v2[1] - v1[1] * v2[0];
}

/* ----------------------------------------------------------------------
   construct matrix from 3 column vectors
------------------------------------------------------------------------- */

void MathExtra::col2mat(const double *ex, const double *ey, const double *ez, double m[3][3])
{
  m[0][0] = ex[0];
  m[1][0] = ex[1];
  m[2][0] = ex[2];
  m[0][1] = ey[0];
  m[1][1] = ey[1];
  m[2][1] = ey[2];
  m[0][2] = ez[0];
  m[1][2] = ez[1];
  m[2][2] = ez[2];
}

/* ----------------------------------------------------------------------
   determinant of a matrix
------------------------------------------------------------------------- */

inline double MathExtra::det3(const double m[3][3])
{
  double ans = m[0][0] * m[1][1] * m[2][2] - m[0][0] * m[1][2] * m[2][1] -
      m[1][0] * m[0][1] * m[2][2] + m[1][0] * m[0][2] * m[2][1] + m[2][0] * m[0][1] * m[1][2] -
      m[2][0] * m[0][2] * m[1][1];
  return ans;
}

/* ----------------------------------------------------------------------
   diagonal matrix times a full matrix
------------------------------------------------------------------------- */

inline void MathExtra::diag_times3(const double *d, const double m[3][3], double ans[3][3])
{
  ans[0][0] = d[0] * m[0][0];
  ans[0][1] = d[0] * m[0][1];
  ans[0][2] = d[0] * m[0][2];
  ans[1][0] = d[1] * m[1][0];
  ans[1][1] = d[1] * m[1][1];
  ans[1][2] = d[1] * m[1][2];
  ans[2][0] = d[2] * m[2][0];
  ans[2][1] = d[2] * m[2][1];
  ans[2][2] = d[2] * m[2][2];
}

/* ----------------------------------------------------------------------
   full matrix times a diagonal matrix
------------------------------------------------------------------------- */

void MathExtra::times3_diag(const double m[3][3], const double *d, double ans[3][3])
{
  ans[0][0] = m[0][0] * d[0];
  ans[0][1] = m[0][1] * d[1];
  ans[0][2] = m[0][2] * d[2];
  ans[1][0] = m[1][0] * d[0];
  ans[1][1] = m[1][1] * d[1];
  ans[1][2] = m[1][2] * d[2];
  ans[2][0] = m[2][0] * d[0];
  ans[2][1] = m[2][1] * d[1];
  ans[2][2] = m[2][2] * d[2];
}

/* ----------------------------------------------------------------------
   add two matrices
------------------------------------------------------------------------- */

inline void MathExtra::plus3(const double m[3][3], const double m2[3][3], double ans[3][3])
{
  ans[0][0] = m[0][0] + m2[0][0];
  ans[0][1] = m[0][1] + m2[0][1];
  ans[0][2] = m[0][2] + m2[0][2];
  ans[1][0] = m[1][0] + m2[1][0];
  ans[1][1] = m[1][1] + m2[1][1];
  ans[1][2] = m[1][2] + m2[1][2];
  ans[2][0] = m[2][0] + m2[2][0];
  ans[2][1] = m[2][1] + m2[2][1];
  ans[2][2] = m[2][2] + m2[2][2];
}

/* ----------------------------------------------------------------------
   multiply mat1 times mat2
------------------------------------------------------------------------- */

inline void MathExtra::times3(const double m[3][3], const double m2[3][3], double ans[3][3])
{
  ans[0][0] = m[0][0] * m2[0][0] + m[0][1] * m2[1][0] + m[0][2] * m2[2][0];
  ans[0][1] = m[0][0] * m2[0][1] + m[0][1] * m2[1][1] + m[0][2] * m2[2][1];
  ans[0][2] = m[0][0] * m2[0][2] + m[0][1] * m2[1][2] + m[0][2] * m2[2][2];
  ans[1][0] = m[1][0] * m2[0][0] + m[1][1] * m2[1][0] + m[1][2] * m2[2][0];
  ans[1][1] = m[1][0] * m2[0][1] + m[1][1] * m2[1][1] + m[1][2] * m2[2][1];
  ans[1][2] = m[1][0] * m2[0][2] + m[1][1] * m2[1][2] + m[1][2] * m2[2][2];
  ans[2][0] = m[2][0] * m2[0][0] + m[2][1] * m2[1][0] + m[2][2] * m2[2][0];
  ans[2][1] = m[2][0] * m2[0][1] + m[2][1] * m2[1][1] + m[2][2] * m2[2][1];
  ans[2][2] = m[2][0] * m2[0][2] + m[2][1] * m2[1][2] + m[2][2] * m2[2][2];
}

/* ----------------------------------------------------------------------
   multiply the transpose of mat1 times mat2
------------------------------------------------------------------------- */

inline void MathExtra::transpose_times3(const double m[3][3], const double m2[3][3],
                                        double ans[3][3])
{
  ans[0][0] = m[0][0] * m2[0][0] + m[1][0] * m2[1][0] + m[2][0] * m2[2][0];
  ans[0][1] = m[0][0] * m2[0][1] + m[1][0] * m2[1][1] + m[2][0] * m2[2][1];
  ans[0][2] = m[0][0] * m2[0][2] + m[1][0] * m2[1][2] + m[2][0] * m2[2][2];
  ans[1][0] = m[0][1] * m2[0][0] + m[1][1] * m2[1][0] + m[2][1] * m2[2][0];
  ans[1][1] = m[0][1] * m2[0][1] + m[1][1] * m2[1][1] + m[2][1] * m2[2][1];
  ans[1][2] = m[0][1] * m2[0][2] + m[1][1] * m2[1][2] + m[2][1] * m2[2][2];
  ans[2][0] = m[0][2] * m2[0][0] + m[1][2] * m2[1][0] + m[2][2] * m2[2][0];
  ans[2][1] = m[0][2] * m2[0][1] + m[1][2] * m2[1][1] + m[2][2] * m2[2][1];
  ans[2][2] = m[0][2] * m2[0][2] + m[1][2] * m2[1][2] + m[2][2] * m2[2][2];
}

/* ----------------------------------------------------------------------
   multiply mat1 times transpose of mat2
------------------------------------------------------------------------- */

inline void MathExtra::times3_transpose(const double m[3][3], const double m2[3][3],
                                        double ans[3][3])
{
  ans[0][0] = m[0][0] * m2[0][0] + m[0][1] * m2[0][1] + m[0][2] * m2[0][2];
  ans[0][1] = m[0][0] * m2[1][0] + m[0][1] * m2[1][1] + m[0][2] * m2[1][2];
  ans[0][2] = m[0][0] * m2[2][0] + m[0][1] * m2[2][1] + m[0][2] * m2[2][2];
  ans[1][0] = m[1][0] * m2[0][0] + m[1][1] * m2[0][1] + m[1][2] * m2[0][2];
  ans[1][1] = m[1][0] * m2[1][0] + m[1][1] * m2[1][1] + m[1][2] * m2[1][2];
  ans[1][2] = m[1][0] * m2[2][0] + m[1][1] * m2[2][1] + m[1][2] * m2[2][2];
  ans[2][0] = m[2][0] * m2[0][0] + m[2][1] * m2[0][1] + m[2][2] * m2[0][2];
  ans[2][1] = m[2][0] * m2[1][0] + m[2][1] * m2[1][1] + m[2][2] * m2[1][2];
  ans[2][2] = m[2][0] * m2[2][0] + m[2][1] * m2[2][1] + m[2][2] * m2[2][2];
}

/* ----------------------------------------------------------------------
   invert a matrix
   does NOT check for singular or badly scaled matrix
------------------------------------------------------------------------- */

inline void MathExtra::invert3(const double m[3][3], double ans[3][3])
{
  double den = m[0][0] * m[1][1] * m[2][2] - m[0][0] * m[1][2] * m[2][1];
  den += -m[1][0] * m[0][1] * m[2][2] + m[1][0] * m[0][2] * m[2][1];
  den += m[2][0] * m[0][1] * m[1][2] - m[2][0] * m[0][2] * m[1][1];

  ans[0][0] = (m[1][1] * m[2][2] - m[1][2] * m[2][1]) / den;
  ans[0][1] = -(m[0][1] * m[2][2] - m[0][2] * m[2][1]) / den;
  ans[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) / den;
  ans[1][0] = -(m[1][0] * m[2][2] - m[1][2] * m[2][0]) / den;
  ans[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) / den;
  ans[1][2] = -(m[0][0] * m[1][2] - m[0][2] * m[1][0]) / den;
  ans[2][0] = (m[1][0] * m[2][1] - m[1][1] * m[2][0]) / den;
  ans[2][1] = -(m[0][0] * m[2][1] - m[0][1] * m[2][0]) / den;
  ans[2][2] = (m[0][0] * m[1][1] - m[0][1] * m[1][0]) / den;
}

/* ----------------------------------------------------------------------
   matrix times vector
------------------------------------------------------------------------- */

inline void MathExtra::matvec(const double m[3][3], const double *v, double *ans)
{
  ans[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2];
  ans[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2];
  ans[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2];
}

/* ----------------------------------------------------------------------
   matrix times vector
------------------------------------------------------------------------- */

inline void MathExtra::matvec(const double *ex, const double *ey, const double *ez, const double *v,
                              double *ans)
{
  ans[0] = ex[0] * v[0] + ey[0] * v[1] + ez[0] * v[2];
  ans[1] = ex[1] * v[0] + ey[1] * v[1] + ez[1] * v[2];
  ans[2] = ex[2] * v[0] + ey[2] * v[1] + ez[2] * v[2];
}

/* ----------------------------------------------------------------------
   transposed matrix times vector
------------------------------------------------------------------------- */

inline void MathExtra::transpose_matvec(const double m[3][3], const double *v, double *ans)
{
  ans[0] = m[0][0] * v[0] + m[1][0] * v[1] + m[2][0] * v[2];
  ans[1] = m[0][1] * v[0] + m[1][1] * v[1] + m[2][1] * v[2];
  ans[2] = m[0][2] * v[0] + m[1][2] * v[1] + m[2][2] * v[2];
}

/* ----------------------------------------------------------------------
   transposed matrix times vector
------------------------------------------------------------------------- */

inline void MathExtra::transpose_matvec(const double *ex, const double *ey, const double *ez,
                                        const double *v, double *ans)
{
  ans[0] = ex[0] * v[0] + ex[1] * v[1] + ex[2] * v[2];
  ans[1] = ey[0] * v[0] + ey[1] * v[1] + ey[2] * v[2];
  ans[2] = ez[0] * v[0] + ez[1] * v[1] + ez[2] * v[2];
}

/* ----------------------------------------------------------------------
   transposed matrix times diagonal matrix
------------------------------------------------------------------------- */

inline void MathExtra::transpose_diag3(const double m[3][3], const double *d, double ans[3][3])
{
  ans[0][0] = m[0][0] * d[0];
  ans[0][1] = m[1][0] * d[1];
  ans[0][2] = m[2][0] * d[2];
  ans[1][0] = m[0][1] * d[0];
  ans[1][1] = m[1][1] * d[1];
  ans[1][2] = m[2][1] * d[2];
  ans[2][0] = m[0][2] * d[0];
  ans[2][1] = m[1][2] * d[1];
  ans[2][2] = m[2][2] * d[2];
}

/* ----------------------------------------------------------------------
   row vector times matrix
------------------------------------------------------------------------- */

inline void MathExtra::vecmat(const double *v, const double m[3][3], double *ans)
{
  ans[0] = v[0] * m[0][0] + v[1] * m[1][0] + v[2] * m[2][0];
  ans[1] = v[0] * m[0][1] + v[1] * m[1][1] + v[2] * m[2][1];
  ans[2] = v[0] * m[0][2] + v[1] * m[1][2] + v[2] * m[2][2];
}

/* ----------------------------------------------------------------------
   matrix times scalar, in place
------------------------------------------------------------------------- */

inline void MathExtra::scalar_times3(const double f, double m[3][3])
{
  m[0][0] *= f;
  m[0][1] *= f;
  m[0][2] *= f;
  m[1][0] *= f;
  m[1][1] *= f;
  m[1][2] *= f;
  m[2][0] *= f;
  m[2][1] *= f;
  m[2][2] *= f;
}

/* ----------------------------------------------------------------------
   multiply 2 shape matrices
   upper-triangular 3x3, stored as 6-vector in Voigt ordering
------------------------------------------------------------------------- */

inline void MathExtra::multiply_shape_shape(const double *one, const double *two, double *ans)
{
  ans[0] = one[0] * two[0];
  ans[1] = one[1] * two[1];
  ans[2] = one[2] * two[2];
  ans[3] = one[1] * two[3] + one[3] * two[2];
  ans[4] = one[0] * two[4] + one[5] * two[3] + one[4] * two[2];
  ans[5] = one[0] * two[5] + one[5] * two[1];
}

/* ----------------------------------------------------------------------
   normalize a quaternion
------------------------------------------------------------------------- */

inline void MathExtra::qnormalize(double *q)
{
  double norm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
  q[0] *= norm;
  q[1] *= norm;
  q[2] *= norm;
  q[3] *= norm;
}

/* ----------------------------------------------------------------------
   conjugate of a quaternion: qc = conjugate of q
   assume q is of unit length
------------------------------------------------------------------------- */

inline void MathExtra::qconjugate(double *q, double *qc)
{
  qc[0] = q[0];
  qc[1] = -q[1];
  qc[2] = -q[2];
  qc[3] = -q[3];
}

/* ----------------------------------------------------------------------
   vector-quaternion multiply: c = a*b, where a = (0,a)
------------------------------------------------------------------------- */

inline void MathExtra::vecquat(double *a, double *b, double *c)
{
  c[0] = -a[0] * b[1] - a[1] * b[2] - a[2] * b[3];
  c[1] = b[0] * a[0] + a[1] * b[3] - a[2] * b[2];
  c[2] = b[0] * a[1] + a[2] * b[1] - a[0] * b[3];
  c[3] = b[0] * a[2] + a[0] * b[2] - a[1] * b[1];
}

/* ----------------------------------------------------------------------
   quaternion-vector multiply: c = a*b, where b = (0,b)
------------------------------------------------------------------------- */

inline void MathExtra::quatvec(double *a, double *b, double *c)
{
  c[0] = -a[1] * b[0] - a[2] * b[1] - a[3] * b[2];
  c[1] = a[0] * b[0] + a[2] * b[2] - a[3] * b[1];
  c[2] = a[0] * b[1] + a[3] * b[0] - a[1] * b[2];
  c[3] = a[0] * b[2] + a[1] * b[1] - a[2] * b[0];
}

/* ----------------------------------------------------------------------
   quaternion-quaternion multiply: c = a*b
------------------------------------------------------------------------- */

inline void MathExtra::quatquat(double *a, double *b, double *c)
{
  c[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
  c[1] = a[0] * b[1] + b[0] * a[1] + a[2] * b[3] - a[3] * b[2];
  c[2] = a[0] * b[2] + b[0] * a[2] + a[3] * b[1] - a[1] * b[3];
  c[3] = a[0] * b[3] + b[0] * a[3] + a[1] * b[2] - a[2] * b[1];
}

/* ----------------------------------------------------------------------
   quaternion multiply: c = inv(a)*b
   a is a quaternion
   b is a four component vector
   c is a three component vector
------------------------------------------------------------------------- */

inline void MathExtra::invquatvec(double *a, double *b, double *c)
{
  c[0] = -a[1] * b[0] + a[0] * b[1] + a[3] * b[2] - a[2] * b[3];
  c[1] = -a[2] * b[0] - a[3] * b[1] + a[0] * b[2] + a[1] * b[3];
  c[2] = -a[3] * b[0] + a[2] * b[1] - a[1] * b[2] + a[0] * b[3];
}

/* ----------------------------------------------------------------------
   quaternion rotation of vector: c = a*b*conj(a)
   a is a quaternion
   b is a three component vector
   c is a three component vector
------------------------------------------------------------------------- */

inline void MathExtra::quatrotvec(double *a, double *b, double *c)
{
  double temp[4];

  // temp = a*b
  temp[0] = -a[1] * b[0] - a[2] * b[1] - a[3] * b[2];
  temp[1] = a[0] * b[0] + a[2] * b[2] - a[3] * b[1];
  temp[2] = a[0] * b[1] + a[3] * b[0] - a[1] * b[2];
  temp[3] = a[0] * b[2] + a[1] * b[1] - a[2] * b[0];

  // c = temp*conj(a)
  c[0] = -a[1] * temp[0] + a[0] * temp[1] - a[3] * temp[2] + a[2] * temp[3];
  c[1] = -a[2] * temp[0] + a[3] * temp[1] + a[0] * temp[2] - a[1] * temp[3];
  c[2] = -a[3] * temp[0] - a[2] * temp[1] + a[1] * temp[2] + a[0] * temp[3];
}

/* ----------------------------------------------------------------------
   compute quaternion from axis-angle rotation
   v MUST be a unit vector
------------------------------------------------------------------------- */

inline void MathExtra::axisangle_to_quat(const double *v, const double angle, double *quat)
{
  double halfa = 0.5 * angle;
  double sina = sin(halfa);
  quat[0] = cos(halfa);
  quat[1] = v[0] * sina;
  quat[2] = v[1] * sina;
  quat[3] = v[2] * sina;
}

/* ----------------------------------------------------------------------
   Apply principal rotation generator about x to rotation matrix m
------------------------------------------------------------------------- */

inline void MathExtra::rotation_generator_x(const double m[3][3], double ans[3][3])
{
  ans[0][0] = 0;
  ans[0][1] = -m[0][2];
  ans[0][2] = m[0][1];
  ans[1][0] = 0;
  ans[1][1] = -m[1][2];
  ans[1][2] = m[1][1];
  ans[2][0] = 0;
  ans[2][1] = -m[2][2];
  ans[2][2] = m[2][1];
}

/* ----------------------------------------------------------------------
   Apply principal rotation generator about y to rotation matrix m
------------------------------------------------------------------------- */

inline void MathExtra::rotation_generator_y(const double m[3][3], double ans[3][3])
{
  ans[0][0] = m[0][2];
  ans[0][1] = 0;
  ans[0][2] = -m[0][0];
  ans[1][0] = m[1][2];
  ans[1][1] = 0;
  ans[1][2] = -m[1][0];
  ans[2][0] = m[2][2];
  ans[2][1] = 0;
  ans[2][2] = -m[2][0];
}

/* ----------------------------------------------------------------------
   Apply principal rotation generator about z to rotation matrix m
------------------------------------------------------------------------- */

inline void MathExtra::rotation_generator_z(const double m[3][3], double ans[3][3])
{
  ans[0][0] = -m[0][1];
  ans[0][1] = m[0][0];
  ans[0][2] = 0;
  ans[1][0] = -m[1][1];
  ans[1][1] = m[1][0];
  ans[1][2] = 0;
  ans[2][0] = -m[2][1];
  ans[2][1] = m[2][0];
  ans[2][2] = 0;
}

// set matrix to zero

inline void MathExtra::zeromat3(double m[3][3])
{
  m[0][0] = m[0][1] = m[0][2] = 0.0;
  m[1][0] = m[1][1] = m[1][2] = 0.0;
  m[2][0] = m[2][1] = m[2][2] = 0.0;
}

inline void MathExtra::zeromat3(double **m)
{
  m[0][0] = m[0][1] = m[0][2] = 0.0;
  m[1][0] = m[1][1] = m[1][2] = 0.0;
  m[2][0] = m[2][1] = m[2][2] = 0.0;
}

// add two matrices

inline void MathExtra::plus3(const double m[3][3], double **m2, double **ans)
{
  ans[0][0] = m[0][0] + m2[0][0];
  ans[0][1] = m[0][1] + m2[0][1];
  ans[0][2] = m[0][2] + m2[0][2];
  ans[1][0] = m[1][0] + m2[1][0];
  ans[1][1] = m[1][1] + m2[1][1];
  ans[1][2] = m[1][2] + m2[1][2];
  ans[2][0] = m[2][0] + m2[2][0];
  ans[2][1] = m[2][1] + m2[2][1];
  ans[2][2] = m[2][2] + m2[2][2];
}

// subtract two matrices

inline void MathExtra::minus3(const double m[3][3], const double m2[3][3], double ans[3][3])
{
  ans[0][0] = m[0][0] - m2[0][0];
  ans[0][1] = m[0][1] - m2[0][1];
  ans[0][2] = m[0][2] - m2[0][2];
  ans[1][0] = m[1][0] - m2[1][0];
  ans[1][1] = m[1][1] - m2[1][1];
  ans[1][2] = m[1][2] - m2[1][2];
  ans[2][0] = m[2][0] - m2[2][0];
  ans[2][1] = m[2][1] - m2[2][1];
  ans[2][2] = m[2][2] - m2[2][2];
}

inline void MathExtra::minus3(double **m, const double m2[3][3], double ans[3][3])
{
  ans[0][0] = m[0][0] - m2[0][0];
  ans[0][1] = m[0][1] - m2[0][1];
  ans[0][2] = m[0][2] - m2[0][2];
  ans[1][0] = m[1][0] - m2[1][0];
  ans[1][1] = m[1][1] - m2[1][1];
  ans[1][2] = m[1][2] - m2[1][2];
  ans[2][0] = m[2][0] - m2[2][0];
  ans[2][1] = m[2][1] - m2[2][1];
  ans[2][2] = m[2][2] - m2[2][2];
}

// compute outer product of two vectors

inline void MathExtra::outer3(const double *v1, const double *v2, double ans[3][3])
{
  ans[0][0] = v1[0] * v2[0];
  ans[0][1] = v1[0] * v2[1];
  ans[0][2] = v1[0] * v2[2];
  ans[1][0] = v1[1] * v2[0];
  ans[1][1] = v1[1] * v2[1];
  ans[1][2] = v1[1] * v2[2];
  ans[2][0] = v1[2] * v2[0];
  ans[2][1] = v1[2] * v2[1];
  ans[2][2] = v1[2] * v2[2];
}

#endif