Fawkes API  Fawkes Development Version
quaternion_helper.cpp
1 
2 /***************************************************************************
3  * quaternion_helper.cpp - External predicates to transform quaternions
4  *
5  * Created: Tue May 20 16:07:07 2014
6  * Copyright 2014 Gesche Gierse
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "quaternion_helper.h"
24 
25 #include <LinearMath/btQuaternion.h>
26 #include <tf/types.h>
27 
28 #include <cstdlib>
29 #include <cstring>
30 #include <eclipseclass.h>
31 
32 using namespace fawkes;
33 /* This class is a wrapper for tf/types.h
34  * Currently it only includes one method to transform quaternions to a yaw value.
35 */
36 
37 int
38 p_get_yaw()
39 {
40  double quad[4];
41  EC_word list(EC_arg(1));
42  EC_word head, tail;
43  if (list.is_list(head, tail) != EC_succeed) {
44  printf("p_get_yaw(): first argument is not a list!\n");
45  }
46  for (int i = 0; list.is_list(head, tail) == EC_succeed and i < 4; list = tail, i++) {
47  int res = head.is_double(&quad[i]);
48  if (res != EC_succeed) {
49  printf("p_get_yaw(): quaternion is not a list of 4 doubles/floats\n");
50  return EC_fail;
51  }
52  }
53  double yaw = fawkes::tf::get_yaw(
54  btQuaternion((float)quad[0], (float)quad[1], (float)quad[2], (float)quad[3]));
55  if (EC_succeed != EC_arg(2).unify(EC_word(yaw))) {
56  printf("p_get_yaw(): could not bind return value\n");
57  return EC_fail;
58  }
59  return EC_succeed;
60 }
fawkes
Fawkes library namespace.