sp_matrix.h
1 /*
2  J. Neira
3  J. A. Castellanos
4  Robotics and Real Time Group
5  University of Zaragoza
6 
7  sp_matrix.h
8  Implements basic MATRIX operations
9 */
10 /*
11  * This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program 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
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program; if not, write to the Free Software
23  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
24  *
25  */
26 
27 #ifndef _SP_MATRIX_H
28 #define _SP_MATRIX_H
29 
30 #ifdef __cplusplus
31 extern "C" {
32 #endif
33 
34 #define MAX_ROWS (7)
35 #define MAX_COLS (7)
36 
37 typedef struct {
38  int rows;
39  int cols;
40  float data[MAX_ROWS][MAX_COLS];
41 } MATRIX;
42 
43 typedef struct {
44  int elements;
45  float data[MAX_ROWS];
46 } VECTOR;
47 
48 #define DOF (3)
49 
50 typedef struct {
51  int mat[DOF];
52  int range;
53 } BMAT;
54 
55 #define MROWS(m) ((m).rows)
56 #define MCOLS(m) ((m).cols)
57 #define MDATA(m,i,j) ((m).data[i][j])
58 
59 #define VELEMENTS(v) ((v).elements)
60 #define VDATA(v,i) ((v).data[i])
61 
62 #define M_SQUARE(m) ((m).rows == (m).cols)
63 #define M_COMPAT_DIM(m, n) ((m).cols == (n).rows)
64 #define M_EQUAL_DIM(m, n) (((m).rows == (n).rows) && ((m).cols == (n).cols))
65 #define V_EQUAL_DIM(v, w) (((v).elements == (w).elements))
66 #define MV_COMPAT_DIM(m, v) ((m).cols == (v).elements)
67 
68 #define FIRST(b) ((b).mat[0])
69 #define SECOND(b) ((b).mat[1])
70 #define THIRD(b) ((b).mat[2])
71 #define RANGE(b) ((b).range)
72 
73 #define SQUARE(x) ((x)*(x))
74 
75 MATRIX create_matrix (int rows, int cols);
76 void initialize_matrix (MATRIX *m, int rows, int cols);
77 void diagonal_matrix (MATRIX *m, int dim, float el1, float el2, float el3);
78 void print_matrix (char *message, MATRIX const *m);
79 VECTOR create_vector (int elements);
80 void initialize_vector (VECTOR *v, int elements);
81 void print_vector (char *message, VECTOR const *v);
82 float cross_product (MATRIX const *m, int f1, int c1, int f2, int c2);
83 int determinant (MATRIX const *m, float *result);
84 int inverse_matrix (MATRIX const *m, MATRIX *n);
85 int multiply_matrix_vector (MATRIX const *m, VECTOR const *v, VECTOR *r);
86 
87 #ifdef __cplusplus
88 }
89 #endif
90 
91 #endif
92 
T min(T a, T b)
Return the minimum of a, b.
Definition: utility.h:91
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: toranger.cc:91
#define PLAYER_WARN1(msg, a)
Definition: error.h:90
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
Definition: sp_matrix.h:37
A rectangular bounding box, used to define the size of an object.
Definition: player.h:255
Generic message header.
Definition: player.h:162
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:658
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:664
Definition: sp_matrix.h:43
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:168
virtual void Main(void)=0
Main method for driver thread.
int ReadInt(int section, const char *name, int value)
Read an integer value.
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
virtual int Setup(void)
Initialize the driver.
Definition: toranger.cc:58
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
#define PLAYER_MSGTYPE_RESP_NACK
A negative response message.
Definition: player.h:125
double ReadTupleAngle(int section, const char *name, int index, double value)
Read an angle form a tuple (includes units conversion)
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
Class for loading configuration file information.
Definition: configfile.h:197
An autopointer for the message queue.
Definition: message.h:74
virtual int Shutdown(void)
Finalize the driver.
Definition: toranger.cc:70
A pose in space.
Definition: player.h:229
#define PLAYER_ERROR(msg)
Definition: error.h:81
Base class for drivers which oeprate with a thread.
Definition: driver.h:553
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:174
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
Base class for all drivers.
Definition: driver.h:109
Definition: sp_matrix.h:50
Definition: toranger.h:33
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76
T max(T a, T b)
Return the maximum of a, b.
Definition: utility.h:104