MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
mbdyn_util_oct.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/utils/mbdyn_util_oct.cc,v 1.8 2017/01/12 15:10:27 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 /*
32  AUTHOR: Reinhard Resch <r.resch@secop.com>
33  Copyright (C) 2011(-2017) all rights reserved.
34 
35  The copyright of this code is transferred
36  to Pierangelo Masarati and Paolo Mantegazza
37  for use in the software MBDyn as described
38  in the GNU Public License version 2.1
39 */
40 
41 // FIXME: this is a dirty trick needed to compile
42 #define real mbdyn_real_type
43 #include <mbconfig.h> /* This goes first in every *.c,*.cc file */
44 
45 #include <matvec3.h>
46 #include <matvecexp.h>
47 #include <Rot.hh>
48 #undef real
49 
50 #define BOUNDS_CHECKING
51 
52 #include <octave/oct.h>
53 //
54 DEFUN_DLD(rotation_matrix_to_rotation_vector,args,nargout,"[phi] = rotation_matrix_to_rotation_vector(R)\n")
55 {
56  octave_value_list retval;
57 
58  int nargin = args.length();
59 
60  if ( nargin < 1 )
61  {
62  print_usage();
63  return retval;
64  }
65 
66  if ( !args(0).is_matrix_type() || args(0).rows() != 3 || args(0).columns() != 3 )
67  {
68  error("R must be a 3x3 matrix!");
69  return retval;
70  }
71 
72  Matrix R_oct = args(0).matrix_value();
73 
75 
76  for ( int i = 0; i < 3; ++i )
77  for ( int j = 0; j < 3; ++j )
78  R(i+1,j+1) = R_oct(i,j);
79 
80  Vec3 phi(RotManip::VecRot(R));
81 
82  ColumnVector phi_oct(3);
83 
84  for ( int i = 0; i < 3; ++i )
85  phi_oct(i) = phi(i+1);
86 
87  retval.append(octave_value(phi_oct));
88 
89  return retval;
90 }
91 
92 DEFUN_DLD(rotation_vector_to_rotation_matrix,args,nargout,"[R] = rotation_vector_to_rotation_matrix(phi)\n")
93 {
94  octave_value_list retval;
95 
96  int nargin = args.length();
97 
98  if ( nargin < 1 )
99  {
100  print_usage();
101  return retval;
102  }
103 
104  if ( !args(0).is_matrix_type() || args(0).rows() != 3 || args(0).columns() != 1 )
105  {
106  error("Phi must be a 3x1 matrix!");
107  return retval;
108  }
109 
110  ColumnVector phi_oct = args(0).matrix_value().column(0);
111 
112  Vec3 phi;
113 
114  for ( int i = 0; i < 3; ++i )
115  phi(i+1) = phi_oct(i);
116 
117  Mat3x3 R(RotManip::Rot(phi));
118 
119  Matrix R_oct(3,3);
120 
121  for ( int i = 0; i < 3; ++i )
122  for ( int j = 0; j < 3; ++j )
123  R_oct(i,j) = R(i+1,j+1);
124 
125  retval.append(octave_value(R_oct));
126 
127  return retval;
128 }
129 
130 DEFUN_DLD(rotation_matrix_to_euler123,args,nargout,"[phi] = rotation_matrix_to_euler123(R)\n")
131 {
132  octave_value_list retval;
133 
134  int nargin = args.length();
135 
136  if ( nargin < 1 )
137  {
138  print_usage();
139  return retval;
140  }
141 
142  if ( !args(0).is_matrix_type() || args(0).rows() != 3 || args(0).columns() != 3 )
143  {
144  error("R must be a 3x3 matrix!");
145  return retval;
146  }
147 
148  Matrix R_oct = args(0).matrix_value();
149 
150  Mat3x3 R;
151 
152  for ( int i = 0; i < 3; ++i )
153  for ( int j = 0; j < 3; ++j )
154  R(i+1,j+1) = R_oct(i,j);
155 
156  Vec3 phi(MatR2EulerAngles123(R));
157 
158  ColumnVector phi_oct(3);
159 
160  for ( int i = 0; i < 3; ++i )
161  phi_oct(i) = phi(i+1);
162 
163  retval.append(octave_value(phi_oct));
164 
165  return retval;
166 }
167 
168 DEFUN_DLD(euler123_to_rotation_matrix,args,nargout,"[R] = euler123_to_rotation_matrix(phi)\n")
169 {
170  octave_value_list retval;
171 
172  int nargin = args.length();
173 
174  if ( nargin < 1 )
175  {
176  print_usage();
177  return retval;
178  }
179 
180  if ( !args(0).is_matrix_type() || args(0).rows() != 3 || args(0).columns() != 1 )
181  {
182  error("Phi must be a 3x1 matrix!");
183  return retval;
184  }
185 
186  ColumnVector phi_oct = args(0).matrix_value().column(0);
187 
188  Vec3 phi;
189 
190  for ( int i = 0; i < 3; ++i )
191  phi(i+1) = phi_oct(i);
192 
193  const Mat3x3 R(EulerAngles123_2MatR(phi));
194 
195  Matrix R_oct(3,3);
196 
197  for ( int i = 0; i < 3; ++i )
198  for ( int j = 0; j < 3; ++j )
199  R_oct(i,j) = R(i+1,j+1);
200 
201  retval.append(octave_value(R_oct));
202 
203  return retval;
204 }
205 
206 DEFUN_DLD(rotation_matrix_to_euler313,args,nargout,"[phi] = rotation_matrix_to_euler313(R)\n")
207 {
208  octave_value_list retval;
209 
210  int nargin = args.length();
211 
212  if ( nargin < 1 )
213  {
214  print_usage();
215  return retval;
216  }
217 
218  if ( !args(0).is_matrix_type() || args(0).rows() != 3 || args(0).columns() != 3 )
219  {
220  error("R must be a 3x3 matrix!");
221  return retval;
222  }
223 
224  Matrix R_oct = args(0).matrix_value();
225 
226  Mat3x3 R;
227 
228  for ( int i = 0; i < 3; ++i )
229  for ( int j = 0; j < 3; ++j )
230  R(i+1,j+1) = R_oct(i,j);
231 
232  Vec3 phi(MatR2EulerAngles313(R));
233 
234  ColumnVector phi_oct(3);
235 
236  for ( int i = 0; i < 3; ++i )
237  phi_oct(i) = phi(i+1);
238 
239  retval.append(octave_value(phi_oct));
240 
241  return retval;
242 }
243 
244 DEFUN_DLD(euler313_to_rotation_matrix,args,nargout,"[R] = euler313_to_rotation_matrix(phi)\n")
245 {
246  octave_value_list retval;
247 
248  int nargin = args.length();
249 
250  if ( nargin < 1 )
251  {
252  print_usage();
253  return retval;
254  }
255 
256  if ( !args(0).is_matrix_type() || args(0).rows() != 3 || args(0).columns() != 1 )
257  {
258  error("Phi must be a 3x1 matrix!");
259  return retval;
260  }
261 
262  ColumnVector phi_oct = args(0).matrix_value().column(0);
263 
264  Vec3 phi;
265 
266  for ( int i = 0; i < 3; ++i )
267  phi(i+1) = phi_oct(i);
268 
269  const Mat3x3 R(EulerAngles313_2MatR(phi));
270 
271  Matrix R_oct(3,3);
272 
273  for ( int i = 0; i < 3; ++i )
274  for ( int j = 0; j < 3; ++j )
275  R_oct(i,j) = R(i+1,j+1);
276 
277  retval.append(octave_value(R_oct));
278 
279  return retval;
280 }
281 
282 DEFUN_DLD(rotation_matrix_to_euler321,args,nargout,"[phi] = rotation_matrix_to_euler321(R)\n")
283 {
284  octave_value_list retval;
285 
286  int nargin = args.length();
287 
288  if ( nargin < 1 )
289  {
290  print_usage();
291  return retval;
292  }
293 
294  if ( !args(0).is_matrix_type() || args(0).rows() != 3 || args(0).columns() != 3 )
295  {
296  error("R must be a 3x3 matrix!");
297  return retval;
298  }
299 
300  Matrix R_oct = args(0).matrix_value();
301 
302  Mat3x3 R;
303 
304  for ( int i = 0; i < 3; ++i )
305  for ( int j = 0; j < 3; ++j )
306  R(i+1,j+1) = R_oct(i,j);
307 
308  Vec3 phi(MatR2EulerAngles321(R));
309 
310  ColumnVector phi_oct(3);
311 
312  for ( int i = 0; i < 3; ++i )
313  phi_oct(i) = phi(i+1);
314 
315  retval.append(octave_value(phi_oct));
316 
317  return retval;
318 }
319 
320 DEFUN_DLD(euler321_to_rotation_matrix,args,nargout,"[R] = euler321_to_rotation_matrix(phi)\n")
321 {
322  octave_value_list retval;
323 
324  int nargin = args.length();
325 
326  if ( nargin < 1 )
327  {
328  print_usage();
329  return retval;
330  }
331 
332  if ( !args(0).is_matrix_type() || args(0).rows() != 3 || args(0).columns() != 1 )
333  {
334  error("Phi must be a 3x1 matrix!");
335  return retval;
336  }
337 
338  ColumnVector phi_oct = args(0).matrix_value().column(0);
339 
340  Vec3 phi;
341 
342  for ( int i = 0; i < 3; ++i )
343  phi(i+1) = phi_oct(i);
344 
345  const Mat3x3 R(EulerAngles321_2MatR(phi));
346 
347  Matrix R_oct(3,3);
348 
349  for ( int i = 0; i < 3; ++i )
350  for ( int j = 0; j < 3; ++j )
351  R_oct(i,j) = R(i+1,j+1);
352 
353  retval.append(octave_value(R_oct));
354 
355  return retval;
356 }
357 /*
358 %!test
359 %! for i = 1:100
360 %! phi = pi * ( 2 * rand() - 1 );
361 %! n = ( 2 * rand(3,1) - 1 );
362 %! n /= norm(n);
363 %! Phi = n * phi;
364 %! R = rotation_vector_to_rotation_matrix(Phi);
365 %! assert(R * R.',eye(3),sqrt(eps));
366 %! assert(R.' * R,eye(3),sqrt(eps));
367 %! Phi2 = rotation_matrix_to_rotation_vector(R);
368 %! assert(Phi2,Phi,sqrt(eps)*norm(Phi));
369 %! endfor
370 
371 %!test
372 %! for i = 1:100
373 %! phi = pi/2 * ( 2 * rand() - 1 );
374 %! n = ( 2 * rand(3,1) - 1 );
375 %! n /= norm(n);
376 %! Phi = n * phi;
377 %! R = euler123_to_rotation_matrix(Phi);
378 %! assert(R * R.',eye(3),sqrt(eps));
379 %! assert(R.' * R,eye(3),sqrt(eps));
380 %! Phi2 = rotation_matrix_to_euler123(R);
381 %! assert(Phi2,Phi,sqrt(eps)*norm(Phi));
382 %! endfor
383 
384 %!test
385 %! Phi = [ -pi/4, 0, 0, pi/3, 0;
386 %! pi/2, 0, pi/2, pi/10, 0;
387 %! pi/3, 0, 0, 0, pi/2 ];
388 %!
389 %! for i=1:columns(Phi)
390 %! R = euler313_to_rotation_matrix(Phi(:,i));
391 %! assert(R * R.',eye(3),sqrt(eps));
392 %! assert(R.' * R,eye(3),sqrt(eps));
393 %! Phi2 = rotation_matrix_to_euler313(R);
394 %! assert(fmod(2*pi+Phi2,2*pi),fmod(2*pi+Phi(:,i),2*pi),sqrt(eps)*norm(Phi));
395 %! endfor
396 
397 %!test
398 %! for i = 1:100
399 %! phi = pi/2 * ( 2 * rand() - 1 );
400 %! n = ( 2 * rand(3,1) - 1 );
401 %! n /= norm(n);
402 %! Phi = n * phi;
403 %! R = euler321_to_rotation_matrix(Phi);
404 %! assert(R * R.',eye(3),sqrt(eps));
405 %! assert(R.' * R,eye(3),sqrt(eps));
406 %! Phi2 = rotation_matrix_to_euler321(R);
407 %! assert(Phi2,Phi,sqrt(eps)*norm(Phi));
408 %! endfor
409 */
Definition: matvec3.h:98
Matrix R_oct
void print_usage(void)
Definition: ann_in.c:59
int error(const char *test, int value)
Mat3x3 EulerAngles313_2MatR(const Vec3 &v)
Definition: matvec3.cc:1039
Vec3 VecRot(const Mat3x3 &Phi)
Definition: Rot.cc:136
int nargin
Mat3x3 EulerAngles123_2MatR(const Vec3 &v)
Definition: matvec3.cc:1014
Vec3 MatR2EulerAngles313(const Mat3x3 &R)
Definition: matvec3.cc:927
octave_value_list retval
Vec3 MatR2EulerAngles123(const Mat3x3 &R)
Definition: matvec3.cc:893
Mat3x3 Rot(const Vec3 &phi)
Definition: Rot.cc:62
Mat3x3 EulerAngles321_2MatR(const Vec3 &v)
Definition: matvec3.cc:1064
Vec3 MatR2EulerAngles321(const Mat3x3 &R)
Definition: matvec3.cc:941
Mat3x3 R