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 
26 #include <eclipseclass.h>
27 #include <tf/types.h>
28 #include <LinearMath/btQuaternion.h>
29 
30 #include <cstring>
31 #include <cstdlib>
32 
33 using namespace fawkes;
34 /* This class is a wrapper for tf/types.h
35  * Currently it only includes one method to transform quaternions to a yaw value.
36 */
37 
38 int
39 p_get_yaw()
40 {
41  int res;
42  double quad[4];
43  EC_word list(EC_arg(1));
44  EC_word head, tail;
45  if (list.is_list(head,tail) != EC_succeed){
46  printf("p_get_yaw(): first argument is not a list!\n");
47  }
48  for (int i=0 ; list.is_list(head,tail) == EC_succeed and i < 4; list=tail, i++)
49  {
50  res = head.is_double(&quad[i]);
51  if (res != EC_succeed){
52  printf( "p_get_yaw(): quaternion is not a list of 4 doubles/floats\n" );
53  return EC_fail;
54  }
55  }
56  double yaw = fawkes::tf::get_yaw(btQuaternion((float) quad[0], (float) quad[1], (float) quad[2], (float) quad[3]));
57  if ( EC_succeed != EC_arg( 2 ).unify( EC_word(yaw) ) )
58  {
59  printf( "p_get_yaw(): could not bind return value\n" );
60  return EC_fail;
61  }
62  return EC_succeed;
63 }
64 
Fawkes library namespace.