1 /*************************************************************************

     2  *                                                                       *

     3  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *

     4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *

     5  *                                                                       *

     6  * This library is free software; you can redistribute it and/or         *

     7  * modify it under the terms of EITHER:                                  *

     8  *   (1) The GNU Lesser General Public License as published by the Free  *

     9  *       Software Foundation; either version 2.1 of the License, or (at  *

    10  *       your option) any later version. The text of the GNU Lesser      *

    11  *       General Public License is included with this library in the     *

    12  *       file LICENSE.TXT.                                               *

    13  *   (2) The BSD-style license that is included with this library in     *

    14  *       the file LICENSE-BSD.TXT.                                       *

    15  *                                                                       *

    16  * This library is distributed in the hope that it will be useful,       *

    17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *

    18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *

    19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *

    20  *                                                                       *

    21  *************************************************************************/

    22 

    23 /* optimized and unoptimized vector and matrix functions */

    24 

    25 #ifndef _ODE_MATRIX_H_

    26 #define _ODE_MATRIX_H_

    27 

    28 #include <ode/common.h>

    29 

    30 

    31 #ifdef __cplusplus

    32 extern "C" {

    33 #endif

    34 

    35 

    36 /* set a vector/matrix of size n to all zeros, or to a specific value. */

    37 

    38 ODE_API IMPORT_C void dSetZero (dReal *a, int n);

    39 ODE_API IMPORT_C void dSetValue (dReal *a, int n, dReal value);

    40 

    41 

    42 /* get the dot product of two n*1 vectors. if n <= 0 then

    43  * zero will be returned (in which case a and b need not be valid).

    44  */

    45 

    46 ODE_API IMPORT_C dReal dDot (const dReal *a, const dReal *b, int n);

    47 

    48 

    49 /* get the dot products of (a0,b), (a1,b), etc and return them in outsum.

    50  * all vectors are n*1. if n <= 0 then zeroes will be returned (in which case

    51  * the input vectors need not be valid). this function is somewhat faster

    52  * than calling dDot() for all of the combinations separately.

    53  */

    54 

    55 /* NOT INCLUDED in the library for now.

    56 void dMultidot2 (const dReal *a0, const dReal *a1,

    57 		 const dReal *b, dReal *outsum, int n);

    58 */

    59 

    60 

    61 /* matrix multiplication. all matrices are stored in standard row format.

    62  * the digit refers to the argument that is transposed:

    63  *   0:   A = B  * C   (sizes: A:p*r B:p*q C:q*r)

    64  *   1:   A = B' * C   (sizes: A:p*r B:q*p C:q*r)

    65  *   2:   A = B  * C'  (sizes: A:p*r B:p*q C:r*q)

    66  * case 1,2 are equivalent to saying that the operation is A=B*C but

    67  * B or C are stored in standard column format.

    68  */

    69 

    70 ODE_API IMPORT_C void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);

    71 ODE_API IMPORT_C void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);

    72 ODE_API IMPORT_C void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);

    73 

    74 

    75 /* do an in-place cholesky decomposition on the lower triangle of the n*n

    76  * symmetric matrix A (which is stored by rows). the resulting lower triangle

    77  * will be such that L*L'=A. return 1 on success and 0 on failure (on failure

    78  * the matrix is not positive definite).

    79  */

    80 

    81 ODE_API IMPORT_C int dFactorCholesky (dReal *A, int n);

    82 

    83 

    84 /* solve for x: L*L'*x = b, and put the result back into x.

    85  * L is size n*n, b is size n*1. only the lower triangle of L is considered.

    86  */

    87 

    88 ODE_API IMPORT_C void dSolveCholesky (const dReal *L, dReal *b, int n);

    89 

    90 

    91 /* compute the inverse of the n*n positive definite matrix A and put it in

    92  * Ainv. this is not especially fast. this returns 1 on success (A was

    93  * positive definite) or 0 on failure (not PD).

    94  */

    95 

    96 ODE_API IMPORT_C int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n);

    97 

    98 

    99 /* check whether an n*n matrix A is positive definite, return 1/0 (yes/no).

   100  * positive definite means that x'*A*x > 0 for any x. this performs a

   101  * cholesky decomposition of A. if the decomposition fails then the matrix

   102  * is not positive definite. A is stored by rows. A is not altered.

   103  */

   104 

   105 ODE_API IMPORT_C int dIsPositiveDefinite (const dReal *A, int n);

   106 

   107 

   108 /* factorize a matrix A into L*D*L', where L is lower triangular with ones on

   109  * the diagonal, and D is diagonal.

   110  * A is an n*n matrix stored by rows, with a leading dimension of n rounded

   111  * up to 4. L is written into the strict lower triangle of A (the ones are not

   112  * written) and the reciprocal of the diagonal elements of D are written into

   113  * d.

   114  */

   115 ODE_API IMPORT_C void dFactorLDLT (dReal *A, dReal *d, int n, int nskip);

   116 

   117 

   118 /* solve L*x=b, where L is n*n lower triangular with ones on the diagonal,

   119  * and x,b are n*1. b is overwritten with x.

   120  * the leading dimension of L is `nskip'.

   121  */

   122 ODE_API IMPORT_C void dSolveL1 (const dReal *L, dReal *b, int n, int nskip);

   123 

   124 

   125 /* solve L'*x=b, where L is n*n lower triangular with ones on the diagonal,

   126  * and x,b are n*1. b is overwritten with x.

   127  * the leading dimension of L is `nskip'.

   128  */

   129 ODE_API IMPORT_C void dSolveL1T (const dReal *L, dReal *b, int n, int nskip);

   130 

   131 

   132 /* in matlab syntax: a(1:n) = a(1:n) .* d(1:n) */

   133 

   134 ODE_API IMPORT_C void dVectorScale (dReal *a, const dReal *d, int n);

   135 

   136 

   137 /* given `L', a n*n lower triangular matrix with ones on the diagonal,

   138  * and `d', a n*1 vector of the reciprocal diagonal elements of an n*n matrix

   139  * D, solve L*D*L'*x=b where x,b are n*1. x overwrites b.

   140  * the leading dimension of L is `nskip'.

   141  */

   142 

   143 ODE_API IMPORT_C void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip);

   144 

   145 

   146 /* given an L*D*L' factorization of an n*n matrix A, return the updated

   147  * factorization L2*D2*L2' of A plus the following "top left" matrix:

   148  *

   149  *    [ b a' ]     <-- b is a[0]

   150  *    [ a 0  ]     <-- a is a[1..n-1]

   151  *

   152  *   - L has size n*n, its leading dimension is nskip. L is lower triangular

   153  *     with ones on the diagonal. only the lower triangle of L is referenced.

   154  *   - d has size n. d contains the reciprocal diagonal elements of D.

   155  *   - a has size n.

   156  * the result is written into L, except that the left column of L and d[0]

   157  * are not actually modified. see ldltaddTL.m for further comments. 

   158  */

   159 ODE_API IMPORT_C void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip);

   160 

   161 

   162 /* given an L*D*L' factorization of a permuted matrix A, produce a new

   163  * factorization for row and column `r' removed.

   164  *   - A has size n1*n1, its leading dimension in nskip. A is symmetric and

   165  *     positive definite. only the lower triangle of A is referenced.

   166  *     A itself may actually be an array of row pointers.

   167  *   - L has size n2*n2, its leading dimension in nskip. L is lower triangular

   168  *     with ones on the diagonal. only the lower triangle of L is referenced.

   169  *   - d has size n2. d contains the reciprocal diagonal elements of D.

   170  *   - p is a permutation vector. it contains n2 indexes into A. each index

   171  *     must be in the range 0..n1-1.

   172  *   - r is the row/column of L to remove.

   173  * the new L will be written within the old L, i.e. will have the same leading

   174  * dimension. the last row and column of L, and the last element of d, are

   175  * undefined on exit.

   176  *

   177  * a fast O(n^2) algorithm is used. see ldltremove.m for further comments.

   178  */

   179 ODE_API IMPORT_C void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d,

   180 		  int n1, int n2, int r, int nskip);

   181 

   182 

   183 /* given an n*n matrix A (with leading dimension nskip), remove the r'th row

   184  * and column by moving elements. the new matrix will have the same leading

   185  * dimension. the last row and column of A are untouched on exit.

   186  */

   187 ODE_API IMPORT_C void dRemoveRowCol (dReal *A, int n, int nskip, int r);

   188 

   189 

   190 #ifdef __cplusplus

   191 }

   192 #endif

   193 

   194 #endif