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
32using 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
37int
38p_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 library namespace.