Open Dynamics Engine: matrix.h Source File

00001 /*************************************************************************
-00002  *                                                                       *
-00003  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
-00004  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
-00005  *                                                                       *
-00006  * This library is free software; you can redistribute it and/or         *
-00007  * modify it under the terms of EITHER:                                  *
-00008  *   (1) The GNU Lesser General Public License as published by the Free  *
-00009  *       Software Foundation; either version 2.1 of the License, or (at  *
-00010  *       your option) any later version. The text of the GNU Lesser      *
-00011  *       General Public License is included with this library in the     *
-00012  *       file LICENSE.TXT.                                               *
-00013  *   (2) The BSD-style license that is included with this library in     *
-00014  *       the file LICENSE-BSD.TXT.                                       *
-00015  *                                                                       *
-00016  * This library is distributed in the hope that it will be useful,       *
-00017  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
-00018  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
-00019  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
-00020  *                                                                       *
-00021  *************************************************************************/
-00022 
-00023 /* optimized and unoptimized vector and matrix functions */
-00024 
-00025 #ifndef _ODE_MATRIX_H_
-00026 #define _ODE_MATRIX_H_
-00027 
-00028 #include <ode/common.h>
-00029 
-00030 
-00031 #ifdef __cplusplus
-00032 extern "C" {
-00033 #endif
-00034 
-00035 
-00036 /* set a vector/matrix of size n to all zeros, or to a specific value. */
-00037 
-00038 ODE_API void dSetZero (dReal *a, int n);
-00039 ODE_API void dSetValue (dReal *a, int n, dReal value);
-00040 
-00041 
-00042 /* get the dot product of two n*1 vectors. if n <= 0 then
-00043  * zero will be returned (in which case a and b need not be valid).
-00044  */
-00045 
-00046 ODE_API dReal dDot (const dReal *a, const dReal *b, int n);
-00047 
-00048 
-00049 /* get the dot products of (a0,b), (a1,b), etc and return them in outsum.
-00050  * all vectors are n*1. if n <= 0 then zeroes will be returned (in which case
-00051  * the input vectors need not be valid). this function is somewhat faster
-00052  * than calling dDot() for all of the combinations separately.
-00053  */
-00054 
-00055 /* NOT INCLUDED in the library for now.
-00056 void dMultidot2 (const dReal *a0, const dReal *a1,
-00057        const dReal *b, dReal *outsum, int n);
-00058 */
-00059 
-00060 
-00061 /* matrix multiplication. all matrices are stored in standard row format.
-00062  * the digit refers to the argument that is transposed:
-00063  *   0:   A = B  * C   (sizes: A:p*r B:p*q C:q*r)
-00064  *   1:   A = B' * C   (sizes: A:p*r B:q*p C:q*r)
-00065  *   2:   A = B  * C'  (sizes: A:p*r B:p*q C:r*q)
-00066  * case 1,2 are equivalent to saying that the operation is A=B*C but
-00067  * B or C are stored in standard column format.
-00068  */
-00069 
-00070 ODE_API void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
-00071 ODE_API void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
-00072 ODE_API void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
-00073 
-00074 
-00075 /* do an in-place cholesky decomposition on the lower triangle of the n*n
-00076  * symmetric matrix A (which is stored by rows). the resulting lower triangle
-00077  * will be such that L*L'=A. return 1 on success and 0 on failure (on failure
-00078  * the matrix is not positive definite).
-00079  */
-00080 
-00081 ODE_API int dFactorCholesky (dReal *A, int n);
-00082 
-00083 
-00084 /* solve for x: L*L'*x = b, and put the result back into x.
-00085  * L is size n*n, b is size n*1. only the lower triangle of L is considered.
-00086  */
-00087 
-00088 ODE_API void dSolveCholesky (const dReal *L, dReal *b, int n);
-00089 
-00090 
-00091 /* compute the inverse of the n*n positive definite matrix A and put it in
-00092  * Ainv. this is not especially fast. this returns 1 on success (A was
-00093  * positive definite) or 0 on failure (not PD).
-00094  */
-00095 
-00096 ODE_API int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n);
-00097 
-00098 
-00099 /* check whether an n*n matrix A is positive definite, return 1/0 (yes/no).
-00100  * positive definite means that x'*A*x > 0 for any x. this performs a
-00101  * cholesky decomposition of A. if the decomposition fails then the matrix
-00102  * is not positive definite. A is stored by rows. A is not altered.
-00103  */
-00104 
-00105 ODE_API int dIsPositiveDefinite (const dReal *A, int n);
-00106 
-00107 
-00108 /* factorize a matrix A into L*D*L', where L is lower triangular with ones on
-00109  * the diagonal, and D is diagonal.
-00110  * A is an n*n matrix stored by rows, with a leading dimension of n rounded
-00111  * up to 4. L is written into the strict lower triangle of A (the ones are not
-00112  * written) and the reciprocal of the diagonal elements of D are written into
-00113  * d.
-00114  */
-00115 ODE_API void dFactorLDLT (dReal *A, dReal *d, int n, int nskip);
-00116 
-00117 
-00118 /* solve L*x=b, where L is n*n lower triangular with ones on the diagonal,
-00119  * and x,b are n*1. b is overwritten with x.
-00120  * the leading dimension of L is `nskip'.
-00121  */
-00122 ODE_API void dSolveL1 (const dReal *L, dReal *b, int n, int nskip);
-00123 
-00124 
-00125 /* solve L'*x=b, where L is n*n lower triangular with ones on the diagonal,
-00126  * and x,b are n*1. b is overwritten with x.
-00127  * the leading dimension of L is `nskip'.
-00128  */
-00129 ODE_API void dSolveL1T (const dReal *L, dReal *b, int n, int nskip);
-00130 
-00131 
-00132 /* in matlab syntax: a(1:n) = a(1:n) .* d(1:n) */
-00133 
-00134 ODE_API void dVectorScale (dReal *a, const dReal *d, int n);
-00135 
-00136 
-00137 /* given `L', a n*n lower triangular matrix with ones on the diagonal,
-00138  * and `d', a n*1 vector of the reciprocal diagonal elements of an n*n matrix
-00139  * D, solve L*D*L'*x=b where x,b are n*1. x overwrites b.
-00140  * the leading dimension of L is `nskip'.
-00141  */
-00142 
-00143 ODE_API void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip);
-00144 
-00145 
-00146 /* given an L*D*L' factorization of an n*n matrix A, return the updated
-00147  * factorization L2*D2*L2' of A plus the following "top left" matrix:
-00148  *
-00149  *    [ b a' ]     <-- b is a[0]
-00150  *    [ a 0  ]     <-- a is a[1..n-1]
-00151  *
-00152  *   - L has size n*n, its leading dimension is nskip. L is lower triangular
-00153  *     with ones on the diagonal. only the lower triangle of L is referenced.
-00154  *   - d has size n. d contains the reciprocal diagonal elements of D.
-00155  *   - a has size n.
-00156  * the result is written into L, except that the left column of L and d[0]
-00157  * are not actually modified. see ldltaddTL.m for further comments. 
-00158  */
-00159 ODE_API void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip);
-00160 
-00161 
-00162 /* given an L*D*L' factorization of a permuted matrix A, produce a new
-00163  * factorization for row and column `r' removed.
-00164  *   - A has size n1*n1, its leading dimension in nskip. A is symmetric and
-00165  *     positive definite. only the lower triangle of A is referenced.
-00166  *     A itself may actually be an array of row pointers.
-00167  *   - L has size n2*n2, its leading dimension in nskip. L is lower triangular
-00168  *     with ones on the diagonal. only the lower triangle of L is referenced.
-00169  *   - d has size n2. d contains the reciprocal diagonal elements of D.
-00170  *   - p is a permutation vector. it contains n2 indexes into A. each index
-00171  *     must be in the range 0..n1-1.
-00172  *   - r is the row/column of L to remove.
-00173  * the new L will be written within the old L, i.e. will have the same leading
-00174  * dimension. the last row and column of L, and the last element of d, are
-00175  * undefined on exit.
-00176  *
-00177  * a fast O(n^2) algorithm is used. see ldltremove.m for further comments.
-00178  */
-00179 ODE_API void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d,
-00180         int n1, int n2, int r, int nskip);
-00181 
-00182 
-00183 /* given an n*n matrix A (with leading dimension nskip), remove the r'th row
-00184  * and column by moving elements. the new matrix will have the same leading
-00185  * dimension. the last row and column of A are untouched on exit.
-00186  */
-00187 ODE_API void dRemoveRowCol (dReal *A, int n, int nskip, int r);
-00188 
-00189 
-00190 #ifdef __cplusplus
-00191 }
-00192 #endif
-00193 
-00194 #endif
-