/*************************************************************************+
−
 *                                                                       *+
−
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *+
−
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *+
−
 *                                                                       *+
−
 * This library is free software; you can redistribute it and/or         *+
−
 * modify it under the terms of EITHER:                                  *+
−
 *   (1) 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. The text of the GNU Lesser      *+
−
 *       General Public License is included with this library in the     *+
−
 *       file LICENSE.TXT.                                               *+
−
 *   (2) The BSD-style license that is included with this library in     *+
−
 *       the file LICENSE-BSD.TXT.                                       *+
−
 *                                                                       *+
−
 * 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 files    *+
−
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *+
−
 *                                                                       *+
−
 *************************************************************************/+
−
+
−
#include <ode/config.h>+
−
#include <ode/misc.h>+
−
#include <ode/matrix.h>+
−
#include <ode/error.h>+
−
#include <ode/memory.h>+
−
#include "mat.h"+
−
+
−
+
−
dMatrix::dMatrix()+
−
{+
−
  n = 0;+
−
  m = 0;+
−
  data = 0;+
−
}+
−
+
−
+
−
dMatrix::dMatrix (int rows, int cols)+
−
{+
−
  n = rows;+
−
  m = cols;+
−
  data = (dReal*) dAlloc (n*m*sizeof(dReal));+
−
  dSetZero (data,n*m);+
−
}+
−
+
−
+
−
dMatrix::dMatrix (const dMatrix &a)+
−
{+
−
  n = a.n;+
−
  m = a.m;+
−
  data = (dReal*) dAlloc (n*m*sizeof(dReal));+
−
  memcpy (data,a.data,n*m*sizeof(dReal));+
−
}+
−
+
−
+
−
dMatrix::dMatrix (int rows, int cols,+
−
		    dReal *_data, int rowskip, int colskip)+
−
{+
−
  n = rows;+
−
  m = cols;+
−
  data = (dReal*) dAlloc (n*m*sizeof(dReal));+
−
  for (int i=0; i<n; i++) {+
−
    for (int j=0; j<m; j++) data[i*m+j] = _data[i*rowskip + j*colskip];+
−
  }+
−
}+
−
+
−
+
−
dMatrix::~dMatrix()+
−
{+
−
  if (data) dFree (data,n*m*sizeof(dReal));+
−
}+
−
+
−
+
−
dReal & dMatrix::operator () (int i, int j)+
−
{+
−
  return data [i*m+j];+
−
}+
−
+
−
+
−
void dMatrix::operator= (const dMatrix &a)+
−
{+
−
  if (data) dFree (data,n*m*sizeof(dReal));+
−
  n = a.n;+
−
  m = a.m;+
−
  if (n > 0 && m > 0) {+
−
    data = (dReal*) dAlloc (n*m*sizeof(dReal));+
−
    memcpy (data,a.data,n*m*sizeof(dReal));+
−
  }+
−
  else data = 0;+
−
}+
−
+
−
+
−
void dMatrix::operator= (dReal a)+
−
{+
−
  for (int i=0; i<n*m; i++) data[i] = a;+
−
}+
−
+
−
+
−
dMatrix dMatrix::transpose()+
−
{+
−
  dMatrix r (m,n);+
−
  for (int i=0; i<n; i++) {+
−
    for (int j=0; j<m; j++) r.data[j*n+i] = data[i*m+j];+
−
  }+
−
  return r;+
−
}+
−
+
−
+
−
dMatrix dMatrix::select (int np, int *p, int nq, int *q)+
−
{+
−
  dMatrix r (np,nq);+
−
  for (int i=0; i<np; i++) {+
−
    for (int j=0; j<nq; j++) {+
−
      if (p[i] < 0 || p[i] >= n || q[i] < 0 || q[i] >= m)+
−
      r.data[i*nq+j] = data[p[i]*m+q[j]];+
−
    }+
−
  }+
−
  return r;+
−
}+
−
+
−
+
−
dMatrix dMatrix::operator + (const dMatrix &a)+
−
{+
−
  dMatrix r (n,m);+
−
  for (int i=0; i<n*m; i++) r.data[i] = data[i] + a.data[i];+
−
  return r;+
−
}+
−
+
−
+
−
dMatrix dMatrix::operator - (const dMatrix &a)+
−
{+
−
  dMatrix r (n,m);+
−
  for (int i=0; i<n*m; i++) r.data[i] = data[i] - a.data[i];+
−
  return r;+
−
}+
−
+
−
+
−
dMatrix dMatrix::operator - ()+
−
{+
−
  dMatrix r (n,m);+
−
  for (int i=0; i<n*m; i++) r.data[i] = -data[i];+
−
  return r;+
−
}+
−
+
−
+
−
dMatrix dMatrix::operator * (const dMatrix &a)+
−
{+
−
  dMatrix r (n,a.m);+
−
  for (int i=0; i<n; i++) {+
−
    for (int j=0; j<a.m; j++) {+
−
      dReal sum = 0;+
−
      for (int k=0; k<m; k++) sum += dMUL(data[i*m+k],a.data[k*a.m+j]);+
−
      r.data [i*a.m+j] = sum;+
−
    }+
−
  }+
−
  return r;+
−
}+
−
+
−
+
−
void dMatrix::operator += (const dMatrix &a)+
−
{+
−
  for (int i=0; i<n*m; i++) data[i] += a.data[i];+
−
}+
−
+
−
+
−
void dMatrix::operator -= (const dMatrix &a)+
−
{+
−
  for (int i=0; i<n*m; i++) data[i] -= a.data[i];+
−
}+
−
+
−
+
−
void dMatrix::clearUpperTriangle()+
−
{+
−
  for (int i=0; i<n; i++) {+
−
    for (int j=i+1; j<m; j++) data[i*m+j] = 0;+
−
  }+
−
}+
−
+
−
+
−
void dMatrix::clearLowerTriangle()+
−
{+
−
  for (int i=0; i<n; i++) {+
−
    for (int j=0; j<i; j++) data[i*m+j] = 0;+
−
  }+
−
}+
−
+
−
+
−
void dMatrix::makeRandom (dReal range)+
−
{+
−
  for (int i=0; i<n; i++) {+
−
    for (int j=0; j<m; j++)+
−
      data[i*m+j] = dMUL((dMUL(dRandReal(),REAL(2.0))-REAL(1.0)),range);+
−
  }+
−
}+
−
+
−
+
−
void dMatrix::print (char *fmt, FILE *f)+
−
{+
−
  for (int i=0; i<n; i++) {+
−
    for (int j=0; j<m; j++) fprintf (f,fmt,data[i*m+j]);+
−
    fprintf (f,"\n");+
−
  }+
−
}+
−
+
−
+
−
dReal dMatrix::maxDifference (const dMatrix &a)+
−
{+
−
  dReal max = 0;+
−
  for (int i=0; i<n; i++) {+
−
    for (int j=0; j<m; j++) {+
−
      dReal diff = dFabs(data[i*m+j] - a.data[i*m+j]);+
−
      if (diff > max) max = diff;+
−
    }+
−
  }+
−
  return max;+
−
}+
−