matrix.C

/*
 * This file is part of the "Archon" framework.
 * (http://files3d.sourceforge.net)
 *
 * Copyright © 2002 by Kristian Spangsege and Brian Kristiansen.
 *
 * Permission to use, copy, modify, and distribute this software and
 * its documentation under the terms of the GNU General Public License is
 * hereby granted. No representations are made about the suitability of
 * this software for any purpose. It is provided "as is" without express
 * or implied warranty. See the GNU General Public License
 * (http://www.gnu.org/copyleft/gpl.html) for more details.
 *
 * The characters in this file are ISO8859-1 encoded.
 *
 * The documentation in this file is in "Doxygen" style
 * (http://www.doxygen.org).
 */

#include <archon/math/matrix.H>

namespace Archon
{
  namespace Math
  {
    Matrix3x3 Matrix3x3::makeZero()
    {
      return Matrix3x3(Vector3::zero(), Vector3::zero(), Vector3::zero());
    }

    Matrix3x3 Matrix3x3::makeIdentity()
    {
       return Matrix3x3(Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1));
    }

    const Matrix3x3 &Matrix3x3::zero()
    {
      static Matrix3x3 v = makeZero();
      return v;
    }

    const Matrix3x3 &Matrix3x3::identity()
    {
      static Matrix3x3 v = makeIdentity();
      return v;
    }

    /*
     * The rotation matrix may be calculated by the following formula:
     *
     *                                             [ 0 -z  y ]
     *   (cos v) I + (1 - cos v) A * A^T + (sin v) [ z  0 -x ]
     *                                             [-y  x  0 ]
     *
     * where v is the angle, A is the axis and x, y and z are the
     * scalar components of the axis. Think of the last matrix as
     * describing the relationship between the standard coordinate
     * axes of a right handed orthogonal coordinate system. In fact
     * the last matrix expresses the cross-product with the axis of
     * rotation.
     *
     * In 2-D this would look like:
     *
     *                                       [ 0 -1 ]
     *   (cos v) I + (1 - cos v) 0 + (sin v) [      ]
     *                                       [ 1  0 ]
     *
     * That is, the 3rd term vanishes and the last matrix now
     * expresses the concept of "perpendicular vector".
     *
     * This is easily seen by setting the axis to [ 0 0 1 ]^T and then
     * discarding the 3rd dimention.
     *
     * In 4-D we should compute the double-axis (or plane) rotation
     * matrix. But how does the last matrix look in 4-D? Somehow it
     * ought to reflect the generalization of the right hand rule
     * (RHR) in 4-D. Again we should expect that when inserting [ 0 0
     * 0 1 ]^T for the second axis and discarding the 4th dimention,
     * we should end up with the 3-D formula above.
     */
    void Matrix3x3::setRotation(const Rotation3 &r)
    {
      const double ca = cos(r.angle);
      const double sa = sin(r.angle);

      const double cx = (1 - ca) * r.axis[0];
      const double cy = (1 - ca) * r.axis[1];
      const double cz = (1 - ca) * r.axis[2];

      const double cxy = cx * r.axis[1];
      const double cyz = cy * r.axis[2];
      const double czx = cz * r.axis[0];

      const double sx = r.axis[0] * sa;
      const double sy = r.axis[1] * sa;
      const double sz = r.axis[2] * sa;

      x.set(ca + cx * r.axis[0], cxy + sz, czx - sy);
      y.set(cxy - sz, ca + cy * r.axis[1], cyz + sx);
      z.set(czx + sy, cyz - sx, ca + cz * r.axis[2]);
    }

    void Matrix3x3::setRotation(const Quaternion &q)
    {
      double xx = q.v[0] * q.v[0];
      double xy = q.v[0] * q.v[1];
      double xz = q.v[0] * q.v[2];
      double xw = q.v[0] * q.w;

      double yy = q.v[1] * q.v[1];
      double yz = q.v[1] * q.v[2];
      double yw = q.v[1] * q.w;

      double zz = q.v[2] * q.v[2];
      double zw = q.v[2] * q.w;

      x[0] = 1 - 2 * (yy + zz);
      y[0] =     2 * (xy - zw);
      z[0] =     2 * (xz + yw);

      x[1] =     2 * (xy + zw);
      y[1] = 1 - 2 * (xx + zz);
      z[1] =     2 * (yz - xw);

      x[2] =     2 * (xz - yw);
      y[2] =     2 * (yz + xw);
      z[2] = 1 - 2 * (xx + yy);
    }

    void Matrix3x3::setInverseOf(const Matrix3x3 &m)
    {
      x[0] = m.y[1] * m.z[2] - m.z[1] * m.y[2];
      y[0] = m.y[2] * m.z[0] - m.y[0] * m.z[2];
      z[0] = m.y[0] * m.z[1] - m.y[1] * m.z[0];
      x[1] = m.z[1] * m.x[2] - m.x[1] * m.z[2];
      y[1] = m.x[0] * m.z[2] - m.x[2] * m.z[0];
      z[1] = m.x[1] * m.z[0] - m.x[0] * m.z[1];
      x[2] = m.x[1] * m.y[2] - m.x[2] * m.y[1];
      y[2] = m.x[2] * m.y[0] - m.x[0] * m.y[2];
      z[2] = m.x[0] * m.y[1] - m.y[0] * m.x[1];

      operator/=(m.det());
    }

    std::ostream &operator<<(std::ostream &out, const Matrix3x3 &m)
    {
      out << m.getRow1() <<  "\n";
      out << m.getRow2() <<  "\n";
      out << m.getRow3() <<  "\n";
      return out;
    }

    void Matrix3x3::rotate(const Rotation3 &r)
    {
      operator*=(Matrix3x3(r));
    }

    void Matrix3x3::pitch(double angle)
    {
      rotate(Rotation3(Vector3(1, 0, 0), angle));
    }

    void Matrix3x3::yaw(double angle)
    {
      rotate(Rotation3(Vector3(0, 1, 0), angle));
    }

    void Matrix3x3::roll(double angle)
    {
      rotate(Rotation3(Vector3(0, 0, 1), angle));
    }

    void Matrix3x3::rotateRightHandedOrthoNormal(const Rotation3 &r)
    {
      Matrix3x3 m(r); m.map(x); m.map(y); x.normalize(); z = x; z *= y; z.normalize(); y = z; y *= x;
    }

    void Matrix3x3::pitchRightHandedOrthoNormal(double angle)
    {
      const Matrix3x3 m(Rotation3(x, angle));
      m.map(y);
      z = x;
      z *= y;
      z.normalize();
      y = z;
      y *= x;
    }

    void Matrix3x3::yawRightHandedOrthoNormal(double angle)
    {
      const Matrix3x3 m(Rotation3(y, angle));
      m.map(z);
      x = y;
      x *= z;
      x.normalize();
      z = x;
      z *= y;
    }

    void Matrix3x3::rollRightHandedOrthoNormal(double angle)
    {
      const Matrix3x3 m(Rotation3(z, angle));
      m.map(x);
      y = z;
      y *= x;
      y.normalize();
      x = y;
      x *= z;
    }
  }
}

---