amino  1.0-beta2
Lightweight Robot Utility Library
planar.h
Go to the documentation of this file.
1 /* -*- mode: C; c-basic-offset: 4 -*- */
2 /* ex: set shiftwidth=4 tabstop=4 expandtab: */
3 /*
4  * Copyright (c) 2022, Colorado School of Mines
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or
8  * without modification, are permitted provided that the following
9  * conditions are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  *
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
20  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
21  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
22  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
24  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
25  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
26  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
27  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  */
34 
35 #ifndef AMINO_TF_PLANAR_H
36 #define AMINO_TF_PLANAR_H
37 
46 #define AA_TF_CMPLX(x, y) ((x) + _Complex_I * (y))
47 
49 #define AA_TF_CMPLX_REAL(c) (creal(c))
50 
52 #define AA_TF_CMPLX_IMAG(c) (cimag(c))
53 
55 #define AA_TF_VEC2(x, y) AA_TF_CMPLX(x, y)
56 
58 #define AA_TF_VEC2_X(v) AA_TF_CMPLX_REAL(v)
59 
61 #define AA_TF_VEC2_Y(v) AA_TF_CMPLX_IMAG(v)
62 
64 #define AA_TF_VEC2_LD(ptr) AA_TF_VEC2((ptr)[0], (ptr)[1])
65 
67 #define AA_TF_VEC2_ST(v, ptr) \
68  do { \
69  (ptr)[0] = AA_TF_VEC2_X(v); \
70  (ptr)[1] = AA_TF_VEC2_Y(v); \
71  } while (0)
72 
74 #define AA_TF_ROTMATP_IDENT_INITIALIZER {1,0, 0,1};
75 
76 
78 #define AA_TF_COTR_IDENT_INITIALIZER {1,0, 0,0 };
79 
81 #define AA_TF_TFMATP_IDENT_INITIALIZER {1,0, 0,1, 0,0};
82 
84 #define AA_TF_CMPLX_IDENT AA_TF_CMPLX(1, 0)
85 
86 
94 aa_tf_cpexp(double theta);
95 
102 AA_API double
104 
107 AA_API void
108 aa_tf_rotmatp_pexp(double angle, double R[AA_RESTRICT 4]);
109 
113 AA_API double
114 aa_tf_rotmatp_uln(const double R[4]);
115 
121 
126 aa_tf_rotmatp_rot(const double R[AA_RESTRICT 4], const aa_tf_vec2 v);
127 
133 
137 AA_API void aa_tf_rotmatp_inv2(const double R[AA_RESTRICT 4],
138  double Ri[AA_RESTRICT 4]);
139 
144 aa_tf_cmul(const aa_tf_cmplx c1, const aa_tf_cmplx c2);
145 
150 AA_API void aa_tf_rotmatp_mul(const double R1[AA_RESTRICT 4],
151  const double R2[AA_RESTRICT 4],
152  double R12[AA_RESTRICT 4]);
153 
155 AA_API double
157 
159 AA_API void
161 
164 aa_tf_angle2cmplx(double angle);
165 
167 AA_API void
168 aa_tf_angle2rotmatp(double angle, double R[AA_RESTRICT 4]);
169 
172 aa_tf_rotmatp2cmplx(const double R[AA_RESTRICT 4]);
173 
175 AA_API double
176 aa_tf_rotmatp2angle(const double R[AA_RESTRICT 4]);
177 
178 /* ---------------------- */
179 /* --- Transformation --- */
180 /* ---------------------- */
181 
190 aa_tf_cv_tf(const aa_tf_cmplx c, const aa_tf_vec2 v, const aa_tf_vec2 p);
191 
196 aa_tf_tfmatp_tf(const double T[AA_RESTRICT 6], const aa_tf_vec2 p);
197 
201 AA_API void
202 aa_tf_cv2tfmatp(const aa_tf_cmplx c, const aa_tf_vec2 v, double T[AA_RESTRICT 6]);
203 
208 AA_API void
210 
214 AA_API void
216  const aa_tf_cmplx c2, const aa_tf_vec2 v2,
217  aa_tf_cmplx *c12, aa_tf_vec2 *v12);
218 
222 AA_API void
223 aa_tf_tfmatp_mul(const double T1[AA_RESTRICT 6], const double T2[AA_RESTRICT 6],
224  double T12[AA_RESTRICT 6]);
225 
229 AA_API void
231  aa_tf_cmplx *ci, aa_tf_vec2 *vi);
232 
236 AA_API void
237 aa_tf_tfmatp_inv2(const double T[AA_RESTRICT 6], double Ti[AA_RESTRICT 6]);
238 
239 #endif //AMINO_TFP_H
#define AA_API
calling and name mangling convention for functions
Definition: amino.h:95
#define AA_RESTRICT
Defined restrict keyword based on language flavor.
Definition: amino.h:99
AA_API void aa_tf_cv_inv(const aa_tf_cmplx c, const aa_tf_vec2 v, aa_tf_cmplx *ci, aa_tf_vec2 *vi)
Invert a complex number and translation vector planar transform.
AA_API void aa_tf_rotmatp_mul(const double R1[AA_RESTRICT 4], const double R2[AA_RESTRICT 4], double R12[AA_RESTRICT 4])
Multiply planar rotation matrices.
AA_API double aa_tf_rotmatp2angle(const double R[AA_RESTRICT 4])
Convert a planar rotation matrix to a rotation angle.
AA_API void aa_tf_angle2rotmatp(double angle, double R[AA_RESTRICT 4])
Convert a rotation angle to a planar rotation matrix.
AA_API double aa_tf_rotmatp_uln(const double R[4])
Logarithm of a planar rotation matrix.
AA_API aa_tf_vec2 aa_tf_cv_tf(const aa_tf_cmplx c, const aa_tf_vec2 v, const aa_tf_vec2 p)
Apply a planar transform represented with a complex number and translation vector.
AA_API double aa_tf_cmplx2angle(const aa_tf_cmplx c)
Convert a complex number to a rotation angle.
AA_API aa_tf_cmplx aa_tf_cpexp(double theta)
Exponential of a pure (zero real part) complex number.
AA_API void aa_tf_tfmatp_mul(const double T1[AA_RESTRICT 6], const double T2[AA_RESTRICT 6], double T12[AA_RESTRICT 6])
Multiply (chain) two complex number and translation vector planar transforms.
AA_API void aa_tf_tfmatp2cv(const double T[AA_RESTRICT 6], aa_tf_cmplx *c, aa_tf_vec2 *v)
Convert a planar transformation matrix to a complex number and translation vector.
AA_API void aa_tf_rotmatp_pexp(double angle, double R[AA_RESTRICT 4])
Exponential of a pure (zero real part) complex number as a rotation matrix.
AA_API double aa_tf_culn(const aa_tf_cmplx c)
Logarithm of a unit complex number.
AA_API aa_tf_cmplx aa_tf_cconj(const aa_tf_cmplx c)
Conjugate a complex number, giving the inverse rotation.
AA_API void aa_tf_tfmatp_inv2(const double T[AA_RESTRICT 6], double Ti[AA_RESTRICT 6])
Invert a planar transformation matrix.
AA_API void aa_tf_rotmatp_inv2(const double R[AA_RESTRICT 4], double Ri[AA_RESTRICT 4])
Invert a planar rotation matrix.
AA_API void aa_tf_cv2tfmatp(const aa_tf_cmplx c, const aa_tf_vec2 v, double T[AA_RESTRICT 6])
Convert a complex number and translation vector to a planar transformation matrix.
AA_API aa_tf_vec2 aa_tf_crot(const aa_tf_cmplx c, const aa_tf_vec2 v)
Apply a planar rotation via complex number.
AA_API void aa_tf_cmplx2rotmatp(const aa_tf_cmplx c, double R[AA_RESTRICT 4])
Convert a unit complex number to a rotation matrix.
AA_API aa_tf_vec2 aa_tf_tfmatp_tf(const double T[AA_RESTRICT 6], const aa_tf_vec2 p)
Apply a planar transform represented matrix.
AA_API aa_tf_vec2 aa_tf_rotmatp_rot(const double R[AA_RESTRICT 4], const aa_tf_vec2 v)
Apply a planar rotation via rotation matrix.
AA_API aa_tf_cmplx aa_tf_cmul(const aa_tf_cmplx c1, const aa_tf_cmplx c2)
Multiply complex numbers.
AA_API aa_tf_cmplx aa_tf_rotmatp2cmplx(const double R[AA_RESTRICT 4])
Convert a planar rotation matrix to a complex number.
AA_API void aa_tf_cv_mul(const aa_tf_cmplx c1, const aa_tf_vec2 v1, const aa_tf_cmplx c2, const aa_tf_vec2 v2, aa_tf_cmplx *c12, aa_tf_vec2 *v12)
Multiply (chain) two complex number and translation vector planar transforms.
AA_API aa_tf_cmplx aa_tf_angle2cmplx(double angle)
Convert a rotation angle to a complex number.
double _Complex aa_tf_cmplx
Typedef for C99 complex numbers.
Definition: type.h:57
aa_tf_cmplx aa_tf_vec2
Represent 2D vectors as complex numbers.
Definition: type.h:60