Statistics
| Branch: | Revision:

root / rgbdslam / gicp / transform.h @ 9240aaa3

History | View | Annotate | Download (2.33 KB)

1
#ifndef DGC_TRANSFORM_H
2
#define DGC_TRANSFORM_H
3

    
4
#ifdef __cplusplus
5
//extern "C" {
6
#endif
7

    
8
  /** 3D transformation - can represent a combined translation and rotation */
9

    
10
typedef double dgc_transform_t[4][4];
11

    
12
  /** Print a 3D transform as a 4x4 matrix 
13
      @param t - transform to be printed 
14
      &param str - name of transform, which will be printed above matrix */
15

    
16
void dgc_transform_print(dgc_transform_t t, const char *str);
17

    
18
  /** Initializes a transform as the identity transform */
19

    
20
void dgc_transform_identity(dgc_transform_t t);
21

    
22
  /** Left multiply transform t1 by transform t2 */
23

    
24
void dgc_transform_left_multiply(dgc_transform_t t1, dgc_transform_t t2);
25

    
26
  /** Rotate tranform around global x axis */
27

    
28
void dgc_transform_rotate_x(dgc_transform_t t, double theta);
29

    
30
  /** Rotate transform around global y axis */
31

    
32
void dgc_transform_rotate_y(dgc_transform_t t, double theta);
33

    
34
  /** Rotate transform around global z axis */
35

    
36
void dgc_transform_rotate_z(dgc_transform_t t, double theta);
37

    
38
  /** Add translation to transform */
39

    
40
void dgc_transform_translate(dgc_transform_t t, double x, double y, double z);
41

    
42
  /** Apply transform to 3D point */
43

    
44
extern inline void dgc_transform_point(double *x, double *y, double *z, 
45
                                       dgc_transform_t t)
46
{
47
  double x2, y2, z2;
48

    
49
  x2 = t[0][0] * *x + t[0][1] * *y + t[0][2] * *z + t[0][3];
50
  y2 = t[1][0] * *x + t[1][1] * *y + t[1][2] * *z + t[1][3];
51
  z2 = t[2][0] * *x + t[2][1] * *y + t[2][2] * *z + t[2][3];
52
  *x = x2;
53
  *y = y2;
54
  *z = z2;
55
}
56

    
57
  /** Copy transform src to dest */
58

    
59
void dgc_transform_copy(dgc_transform_t dest, dgc_transform_t src);
60

    
61
  /** Read a transform from file */
62

    
63
int dgc_transform_read(dgc_transform_t t, const char *filename);
64

    
65
  /** Write a transform from file */
66

    
67
int dgc_transform_write(dgc_transform_t t, const char *filename);
68

    
69
  /** Read a transform from a string */
70

    
71
int dgc_transform_read_string(dgc_transform_t t, char *str);
72

    
73
  /** Get translation */
74

    
75
void dgc_transform_get_translation(dgc_transform_t t, double *x, double *y,
76
                                   double *z);
77

    
78
  /** Get rotation */
79

    
80
void dgc_transform_get_rotation(dgc_transform_t t, double *x, double *y,
81
                                double *z);
82

    
83
void dgc_transform_rpy(dgc_transform_t dest, dgc_transform_t src, double roll,
84
                       double pitch, double yaw);
85

    
86
void dgc_transform_inverse(dgc_transform_t in, dgc_transform_t out);
87

    
88
#ifdef __cplusplus
89
//}
90
#endif
91

    
92
#endif