World of Rigid Bodies (WoRB)
|
00001 #ifndef _MEX_WORB_H_INCLUDED 00002 #define _MEX_WORB_H_INCLUDED 00003 00004 /** 00005 * @file mexWoRB.h 00006 * @brief Definitions for `Matrix`, `Scalar`, `Logical` and `String` classes 00007 * which simplify access to MATLAB array objects. 00008 * @author Mikica Kocic 00009 * @version 0.3 00010 * @date 2012-05-14 00011 * @copyright GNU Public License. 00012 */ 00013 00014 #include "mex.h" 00015 00016 namespace Mex 00017 { 00018 /** Represents a simple matrix class that keeps components in a column-major order. 00019 */ 00020 class Matrix 00021 { 00022 /** Holds the number of rows in matrix. 00023 */ 00024 mwIndex NRows; 00025 00026 /** Holds the number of columns in matrix. 00027 */ 00028 mwIndex NCols; 00029 00030 /** Holds the matrix data in column-major order. 00031 */ 00032 double* Data; 00033 00034 /** Initializes the matrix to refer to the given MATLAB array. 00035 * This method *does not* copy data from the given MATLAB array. 00036 */ 00037 void Initialize( const mxArray* arg, const char* argDesc ) 00038 { 00039 if ( arg == NULL ) { 00040 return; 00041 } 00042 00043 // We expect a scalar, vector or a matrix of real numbers. 00044 // 00045 int ndim = mxGetNumberOfDimensions( arg ); 00046 if ( ! mxIsDouble( arg ) || mxIsComplex( arg ) || ndim > 2 ) 00047 { 00048 WoRB::SevereError( "WoRB:invarg", 00049 "'%s' must be a scalar, vector or a matrix of real numbers.", 00050 argDesc 00051 ); 00052 } 00053 00054 // Get the matrix dimensions and the first element of the real data 00055 const mwSize* dims = mxGetDimensions( arg ); 00056 NRows = ndim >= 1 ? dims[0] : 1; 00057 NCols = ndim >= 2 ? dims[1] : 1; 00058 Data = mxGetPr( arg ); 00059 } 00060 00061 public: 00062 00063 /** Creates an uninitialized matrix allocated using `mxAlloc`. 00064 */ 00065 Matrix( mwIndex rowCount, mwIndex columnCount ) 00066 : NRows( rowCount ) 00067 , NCols( columnCount ) 00068 , Data( NULL ) 00069 { 00070 Data = (double*)mxMalloc( MemorySize () ); 00071 } 00072 00073 /** Constructs the matrix from a `mexFunction` argument given by the argument number. 00074 */ 00075 Matrix( int nArgIn, const mxArray* argIn[], int argNo, const char* argDesc ) 00076 : NRows( 0 ) 00077 , NCols( 0 ) 00078 , Data( NULL ) 00079 { 00080 if ( argNo < nArgIn ) { 00081 Initialize( argIn[ argNo ], argDesc ); 00082 } 00083 } 00084 00085 /** Constructs the matrix from MATLAB variable from the specified workspace. 00086 */ 00087 Matrix( const char* varName, const char* workspace = "caller" ) 00088 { 00089 Initialize( mexGetVariable( workspace, varName ), varName ); 00090 } 00091 00092 /** Constructs the matrix from the given MATLAB structure field. 00093 */ 00094 Matrix( const mxArray* obj, mwIndex index, const char* fieldName ) 00095 { 00096 Initialize( mxGetField( obj, index, fieldName ), fieldName ); 00097 } 00098 00099 /** Destructor. Does nothing. 00100 * MATLAB automatically frees all data allocated by `mxMalloc`. 00101 */ 00102 ~Matrix () 00103 { 00104 } 00105 00106 /** Converts an instance to a MATLAB double matrix (allocated on MATLAB heap) 00107 */ 00108 operator mxArray* () 00109 { 00110 // Note: The array creation with mxCreateDoubleMatrix initializes 00111 // the array memory by filling it with zeros. The following code avoids 00112 // this unnecessary initialization. 00113 00114 // Create an uninitialized empty array 00115 mxArray* result = mxCreateDoubleMatrix( 0, 0, mxREAL ); 00116 00117 // Set dimensions (note that either of NRows/NCols may be 0) 00118 mxSetM( result, NRows ); // Set M = row count 00119 mxSetN( result, NCols ); // Set N = column count 00120 00121 if ( NRows && NCols ) { 00122 mxSetData( result, Data ); // Set mxArray data to point to output 00123 } 00124 00125 return result; 00126 } 00127 00128 /** Returns true if the instance is an empty array. 00129 */ 00130 bool IsEmpty () const 00131 { 00132 return NRows * NCols == 0; 00133 } 00134 00135 /** Returns true if the instance is a scalar (1x1 matrix). 00136 */ 00137 bool IsScalar () const 00138 { 00139 return NRows == 1 && NCols == 1; 00140 } 00141 00142 /** Returns true if the instance is a vector (1xM or Nx1 matrix) or a scalar 00143 */ 00144 bool IsVector () const 00145 { 00146 return ( NRows == 1 && NCols > 0 ) || ( NCols == 1 && NRows > 0 ); 00147 } 00148 00149 /** Returns true if the instance is a row vector (1xM matrix). 00150 */ 00151 bool IsRowVector () const 00152 { 00153 return NRows == 1 && NCols > 0; 00154 } 00155 00156 /** Returns true if the instance is a column vector (Nx1 matrix). 00157 */ 00158 bool IsColumnVector () const 00159 { 00160 return NCols == 1 && NRows > 0; 00161 } 00162 00163 /** Returns true if the instance is non-empty matrix that is not a vector. 00164 */ 00165 bool IsFullMatrix () const 00166 { 00167 return NCols > 1 && NRows > 1; 00168 } 00169 00170 /** Checks the sizes of each dimension of the matrix. 00171 */ 00172 bool IsSize( mwIndex M, mwIndex N ) const 00173 { 00174 return M == NRows && N == NCols; 00175 } 00176 00177 /** Returns true if the specified index points to a valid row. 00178 */ 00179 bool IsValidRow( mwIndex r ) const 00180 { 00181 return 0 <= r && r < NRows; 00182 } 00183 00184 /** Returns true if the specified index points to a valid column. 00185 */ 00186 bool IsValidColumn( mwIndex c ) const 00187 { 00188 return 0 <= c && c < NCols; 00189 } 00190 00191 /** Returns the length of the (linear) array of the matrix. 00192 */ 00193 mwIndex Length () const 00194 { 00195 return NRows * NCols; 00196 } 00197 00198 /** Gets the number of rows in the matrix. 00199 */ 00200 mwIndex GetM () const 00201 { 00202 return NRows; 00203 } 00204 00205 /** Gets the number of columns in the matrix. 00206 */ 00207 mwIndex GetN () const 00208 { 00209 return NCols; 00210 } 00211 00212 /** Gets the memory size (in bytes) occupied by array. 00213 */ 00214 mwSize MemorySize () const 00215 { 00216 return NRows * NCols * sizeof( double ); 00217 } 00218 00219 /** Verifies sizes of each matrix dimension. 00220 * Reports severe error if dimensions do not agree. 00221 */ 00222 void VerifyDims( mwIndex M, mwIndex N, const char* error_message ) const 00223 { 00224 if ( IsSize( M, N ) || ( Length () == 0 && M * N == 0 ) ) { 00225 return; // OK 00226 } 00227 00228 WoRB::SevereError( 00229 "Matrix:VerifyDims:invsize", 00230 "%s; Required dimensions [ %d × %d ] have only [ %d × %d ]", 00231 error_message, M, N, NRows, NCols 00232 ); 00233 } 00234 00235 ////////////////////////////////////////////////////////////////////////////////////// 00236 00237 /** Returns matrix element given by the linear index. 00238 */ 00239 double& operator ()( mwIndex ix ) 00240 { 00241 if ( ix < 0 || ix >= Length () ) { 00242 WoRB::SevereError( 00243 "Matrix:elem:invdim", 00244 "Linear index %d outside dimensions [ %d × %d ]", ix, NRows, NCols 00245 ); 00246 } 00247 00248 return Data[ ix ]; 00249 } 00250 00251 /** Returns matrix element given by the linear index. 00252 */ 00253 const double& operator ()( mwIndex ix ) const 00254 { 00255 if ( ix < 0 || ix >= Length () ) { 00256 WoRB::SevereError( 00257 "Matrix:elem:invdim", 00258 "Linear index %d outside dimensions [ %d × %d ]", ix, NRows, NCols 00259 ); 00260 } 00261 00262 return Data[ ix ]; 00263 } 00264 00265 /** Returns matrix element given by row and column index. 00266 */ 00267 double& operator ()( mwIndex r, mwIndex c ) 00268 { 00269 mwIndex ix = r + c * NRows; 00270 if ( ix < 0 || ix >= Length () ) { 00271 WoRB::SevereError( 00272 "Matrix:elem:invdim", 00273 "Index (%d,%d) outside dimensions [ %d × %d ]", r, c, NRows, NCols 00274 ); 00275 } 00276 00277 return Data[ ix ]; 00278 } 00279 00280 /** Returns matrix element given by row and column index. 00281 */ 00282 const double& operator ()( mwIndex r, mwIndex c ) const 00283 { 00284 mwIndex ix = r + c * NRows; 00285 if ( ix < 0 || ix >= Length () ) { 00286 WoRB::SevereError( 00287 "Matrix:elem:invdim", 00288 "Index (%d,%d) outside dimensions [ %d × %d ]", r, c, NRows, NCols 00289 ); 00290 } 00291 00292 return Data[ ix ]; 00293 } 00294 }; 00295 00296 /** Represents a MATLAB scalar double value. 00297 */ 00298 class Scalar 00299 { 00300 double value; 00301 00302 /** Initializes the instance from a MATLAB array of doubles. 00303 */ 00304 void Initialize( const mxArray* arg, const char* argDesc, bool defaultValue = false ) 00305 { 00306 if ( arg == NULL ) { 00307 value = defaultValue; 00308 return; 00309 } 00310 00311 // Parse value from different argument types 00312 // int ndim = mxGetNumberOfDimensions( arg ); 00313 mwSize len = mxGetNumberOfElements( arg ); 00314 00315 if ( mxIsDouble( arg ) && ! mxIsComplex( arg ) && len >= 1 ) 00316 { 00317 double* data = mxGetPr( arg ); 00318 value = data != NULL ? data[0] : 0.0; 00319 return; 00320 } 00321 00322 // If we couldn't parse real value, return an error 00323 WoRB::SevereError( "WoRB:parseDouble:invarg", 00324 "'%s' must be a real number.", 00325 argDesc 00326 ); 00327 } 00328 00329 public: 00330 00331 /** Constructs the instance from the given MATLAB array of doubles. 00332 */ 00333 Scalar( const mxArray* arg, const char* argDesc ) 00334 { 00335 Initialize( arg, argDesc ); 00336 } 00337 00338 /** Constructs the instance from the given MATLAB structure field. 00339 */ 00340 Scalar( const mxArray* obj, mwIndex index, const char* fieldName ) 00341 { 00342 Initialize( mxGetField( obj, index, fieldName ), fieldName ); 00343 } 00344 00345 operator double () const 00346 { 00347 return value; 00348 } 00349 00350 operator int () const 00351 { 00352 return int( value ); 00353 } 00354 00355 operator unsigned () const 00356 { 00357 return unsigned( value ); 00358 } 00359 00360 operator double& () 00361 { 00362 return value; 00363 } 00364 00365 /** Converts the instance to MATLAB real scalar. 00366 */ 00367 mxArray* toMxArray() 00368 { 00369 return mxCreateDoubleScalar( value ); 00370 } 00371 }; 00372 00373 /** Represents a MATLAB logical value. 00374 */ 00375 class Logical 00376 { 00377 bool value; 00378 00379 /** Initializes the instance from a MATLAB array of doubles. 00380 */ 00381 void Initialize( const mxArray* arg, const char* argDesc, bool defaultValue ) 00382 { 00383 if ( arg == NULL ) { 00384 value = defaultValue; 00385 return; 00386 } 00387 00388 // Parse value from different argument types 00389 // int ndim = mxGetNumberOfDimensions( arg ); 00390 mwSize len = mxGetNumberOfElements( arg ); 00391 00392 if ( mxIsLogicalScalar( arg ) && len >= 1 ) 00393 { 00394 value = mxGetLogicals( arg )[0]; 00395 return; 00396 } 00397 else if ( mxIsDouble( arg ) && ! mxIsComplex( arg ) && len >= 1 ) 00398 { 00399 double* data = mxGetPr( arg ); 00400 value = data != NULL && data[0] != 0; 00401 return; 00402 } 00403 00404 // If we couldn't parse logical value, return an error 00405 WoRB::SevereError( "WoRB:parseLogical:invarg", 00406 "'%s' must be a logical value or a real number.", 00407 argDesc 00408 ); 00409 } 00410 00411 public: 00412 00413 /** Constructs the instance from the given MATLAB array of doubles. 00414 */ 00415 Logical( const mxArray* arg, const char* argDesc, bool defaultValue = false ) 00416 { 00417 Initialize( arg, argDesc, defaultValue ); 00418 } 00419 00420 /** Constructs the instance from the given MATLAB structure field. 00421 */ 00422 Logical( const mxArray* obj, mwIndex index, const char* fieldName, 00423 bool defaultValue = false ) 00424 { 00425 Initialize( mxGetField( obj, index, fieldName ), fieldName, defaultValue ); 00426 } 00427 00428 bool operator ! () const 00429 { 00430 return !value; 00431 } 00432 00433 operator bool () const 00434 { 00435 return value; 00436 } 00437 00438 /** Converts the instance to MATLAB logical scalar. 00439 */ 00440 mxArray* toMxArray() 00441 { 00442 return mxCreateLogicalScalar( value ); 00443 } 00444 00445 /** Sets the new value. 00446 */ 00447 Logical& operator = ( bool v ) 00448 { 00449 value = v; 00450 return* this; 00451 } 00452 }; 00453 00454 /** Represents a MATLAB character string value. 00455 */ 00456 class String 00457 { 00458 char* value; 00459 00460 /** Initializes the instance from a MATLAB `mxArray`. 00461 */ 00462 void Initialize( const mxArray* arg, const char* argDesc ) 00463 { 00464 if ( arg == NULL || ! mxIsChar( arg ) ) { 00465 // If we couldn't parse character string, return an error 00466 WoRB::SevereError( "WoRB:parseString:invarg", 00467 "'%s' must be a character array.", 00468 argDesc 00469 ); 00470 } 00471 00472 mwSize len = mxGetNumberOfElements( arg ); 00473 value = (char*)mxMalloc( len + 1 ); 00474 mxGetString( arg, value, len + 1 ); 00475 } 00476 00477 public: 00478 00479 /** Constructs the instance from the given MATLAB character array. 00480 */ 00481 String( const mxArray* arg, const char* argDesc ) 00482 { 00483 Initialize( arg, argDesc ); 00484 } 00485 00486 /** Constructs the instance from the given MATLAB structure field. 00487 */ 00488 String( const mxArray* obj, mwIndex index, const char* fieldName ) 00489 { 00490 Initialize( mxGetField( obj, index, fieldName ), fieldName ); 00491 } 00492 00493 /** Destructor. Does nothing. 00494 * MATLAB automatically frees all data allocated by `mxMalloc`. 00495 */ 00496 ~String () 00497 { 00498 } 00499 00500 /** Gets pointer to internal character string. 00501 */ 00502 operator const char* () const 00503 { 00504 return value; 00505 } 00506 00507 /** Converts the instance to MATLAB string. 00508 */ 00509 mxArray* toMxArray() 00510 { 00511 return mxCreateString( value ); 00512 } 00513 00514 /** Returns true if this instance has the same contents as the given string. 00515 */ 00516 bool operator == ( const char* v ) 00517 { 00518 for ( const char* p = value; *p && *v; ++p, ++v ) { 00519 if ( *p != *v ) { 00520 return false; 00521 } 00522 } 00523 return true; 00524 } 00525 00526 /** Returns true if this instance does not have the same contents as the given string. 00527 */ 00528 bool operator != ( const char* v ) 00529 { 00530 return ! operator ==( v ); 00531 } 00532 }; 00533 00534 } // namespace Mex 00535 00536 #endif // _MEX_WORB_H_INCLUDED