From 896bab1d5dac7404cc0a3072f2cc50d9696e7818 Mon Sep 17 00:00:00 2001 From: Ramasamy Date: Thu, 19 Jun 2014 15:31:58 +0530 Subject: [PATCH] Adding euler_angles class implementation - Adding euler_angles template class - Adding implementation for the euler_angles template class - Adding euler_angles class testing code signed-off-by: Ramasamy Change-Id: Idd8498ce0decf89d0ae53cf2efa424013d692c86 --- src/sensor_fusion/standalone/util/euler_angles.cpp | 104 +++++++++++++++++++ src/sensor_fusion/standalone/util/euler_angles.h | 46 +++++++++ .../util/test/euler_angles_test/.cproject | 112 +++++++++++++++++++++ .../util/test/euler_angles_test/.project | 89 ++++++++++++++++ .../test/euler_angles_test/euler_angles_main.cpp | 73 ++++++++++++++ 5 files changed, 424 insertions(+) create mode 100644 src/sensor_fusion/standalone/util/euler_angles.cpp create mode 100644 src/sensor_fusion/standalone/util/euler_angles.h create mode 100644 src/sensor_fusion/standalone/util/test/euler_angles_test/.cproject create mode 100644 src/sensor_fusion/standalone/util/test/euler_angles_test/.project create mode 100644 src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp diff --git a/src/sensor_fusion/standalone/util/euler_angles.cpp b/src/sensor_fusion/standalone/util/euler_angles.cpp new file mode 100644 index 0000000..00f9439 --- /dev/null +++ b/src/sensor_fusion/standalone/util/euler_angles.cpp @@ -0,0 +1,104 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#if defined (_EULER_ANGLES_H) && defined (_VECTOR_H) + +#include + +#define EULER_SIZE 3 +#define RAD2DEG 57.2957795 +#define DEG2RAD 0.0174532925 + +template +euler_angles::euler_angles() : m_ang(EULER_SIZE) +{ +} + +template +euler_angles::euler_angles(const TYPE roll, const TYPE pitch, const TYPE yaw) +{ + TYPE euler_data[EULER_SIZE] = {roll, pitch, yaw}; + + vector v(EULER_SIZE, euler_data); + m_ang = v; +} + +template +euler_angles::euler_angles(const vector v) +{ + m_ang = v; +} + +template +euler_angles::euler_angles(const euler_angles& e) +{ + m_ang = e.m_ang; +} + +template +euler_angles::~euler_angles() +{ + +} + +template +euler_angles euler_angles::operator =(const euler_angles& e) +{ + m_ang = e.m_ang; +} + +template +euler_angles quat2euler(const quaternion q) +{ + T w, x, y, z; + T R11, R21, R31, R32, R33; + T phi, theta, psi; + + w = q.m_quat.m_vec[0]; + x = q.m_quat.m_vec[1]; + y = q.m_quat.m_vec[2]; + z = q.m_quat.m_vec[3]; + + R11 = 2 * (w * w) - 1 + 2 * (x * x); + R21 = 2 * ((x * y) - (w * z)); + R31 = 2 * ((x * z) + (w * y)); + R32 = 2 * ((y * z) - (w * x)); + R33 = 2 * (w * w) - 1 + 2 * (z * z); + + phi = atan2(R32, R33); + theta = -atan(R31 / sqrt(1 - (R31 * R31))); + psi = atan2(R21, R11); + + euler_angles e(phi, theta, psi); + + return e; +} + +template +euler_angles rad2deg(const euler_angles e) +{ + return (e.m_ang * (T) RAD2DEG); +} + +template +euler_angles deg2rad(const euler_angles e) +{ + return (e.m_ang * (T) DEG2RAD); +} + +#endif //_EULER_ANGLES_H diff --git a/src/sensor_fusion/standalone/util/euler_angles.h b/src/sensor_fusion/standalone/util/euler_angles.h new file mode 100644 index 0000000..bafe8df --- /dev/null +++ b/src/sensor_fusion/standalone/util/euler_angles.h @@ -0,0 +1,46 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef _EULER_ANGLES_H +#define _EULER_ANGLES_H + +#include "vector.h" +#include "quaternion.h" + +template +class euler_angles { +public: + vector m_ang; + + euler_angles(); + euler_angles(const TYPE roll, const TYPE pitch, const TYPE yaw); + euler_angles(const vector v); + euler_angles(const euler_angles& e); + ~euler_angles(); + + euler_angles operator =(const euler_angles& e); + + template friend euler_angles quat2euler(const quaternion q); + template friend euler_angles rad2deg(const euler_angles e); + template friend euler_angles deg2rad(const euler_angles e); +}; + +#include "euler_angles.cpp" + +#endif //_EULER_ANGLES_H diff --git a/src/sensor_fusion/standalone/util/test/euler_angles_test/.cproject b/src/sensor_fusion/standalone/util/test/euler_angles_test/.cproject new file mode 100644 index 0000000..7f29995 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/euler_angles_test/.cproject @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sensor_fusion/standalone/util/test/euler_angles_test/.project b/src/sensor_fusion/standalone/util/test/euler_angles_test/.project new file mode 100644 index 0000000..eca2f5e --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/euler_angles_test/.project @@ -0,0 +1,89 @@ + + + euler_angles_test + + + + + + org.eclipse.cdt.managedbuilder.core.genmakebuilder + clean,full,incremental, + + + ?name? + + + + org.eclipse.cdt.make.core.append_environment + true + + + org.eclipse.cdt.make.core.autoBuildTarget + all + + + org.eclipse.cdt.make.core.buildArguments + + + + org.eclipse.cdt.make.core.buildCommand + make + + + org.eclipse.cdt.make.core.buildLocation + ${workspace_loc:/euler_angles_test/Debug} + + + org.eclipse.cdt.make.core.cleanBuildTarget + clean + + + org.eclipse.cdt.make.core.contents + org.eclipse.cdt.make.core.activeConfigSettings + + + org.eclipse.cdt.make.core.enableAutoBuild + false + + + org.eclipse.cdt.make.core.enableCleanBuild + true + + + org.eclipse.cdt.make.core.enableFullBuild + true + + + org.eclipse.cdt.make.core.fullBuildTarget + all + + + org.eclipse.cdt.make.core.stopOnError + true + + + org.eclipse.cdt.make.core.useDefaultBuildCmd + true + + + + + org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder + full,incremental, + + + + + org.tizen.nativecpp.apichecker.core.builder + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.managedbuilder.core.managedBuildNature + org.eclipse.cdt.managedbuilder.core.ScannerConfigNature + org.tizen.nativecpp.apichecker.core.tizenCppNature + + diff --git a/src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp b/src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp new file mode 100644 index 0000000..592e782 --- /dev/null +++ b/src/sensor_fusion/standalone/util/test/euler_angles_test/euler_angles_main.cpp @@ -0,0 +1,73 @@ +/* + * sensord + * + * Copyright (c) 2014 Samsung Electronics Co., Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "../../euler_angles.h" + +int main() +{ + float arr0[3] = {234.98, 345.24, -56.12}; + float arr1[3] = {56, -34, 76}; + float arr2[4] = {0.6, 0.6, -.18, -.44}; + float arr3[4] = {-0.5, -0.36, .43, .03}; + + vector v0(3, arr0); + vector v1(3, arr1); + vector v2(4, arr2); + vector v3(4, arr3); + + quaternion q1(v2); + quaternion q2(v3); + + euler_angles e0(v0); + euler_angles e1(v1); + euler_angles e2((float)234.98, (float)345.24, (float)-56.12); + euler_angles e3(e1); + euler_angles e4; + + cout << "Constructor tests\n"; + cout << "input\t" << v0 << "\n"; + cout << "output\t" << e0.m_ang << "\n\n"; + cout << "input\t" << v1 << "\n"; + cout << "output\t" << e1.m_ang << "\n\n"; + cout << "input\t" << v0 << "\n"; + cout << "output\t" << e2.m_ang << "\n\n"; + cout << "input\t" << v1 << "\n"; + cout << "output\t" << e3.m_ang << "\n\n"; + cout << "default constructor\n"; + cout << "output\t" << e4.m_ang << "\n\n"; + + cout << "Quaternion to Euler\n"; + euler_angles e5 = quat2euler(q1); + cout << "input\t" << q1.m_quat << "\n"; + cout << "output\t" << e5.m_ang << "\n\n"; + euler_angles e8 = quat2euler(q2); + cout << "input\t" << q2.m_quat << "\n"; + cout << "output\t" << e8.m_ang << "\n\n"; + + cout << "Radians to Degrees\n"; + euler_angles e6 = deg2rad(e0); + cout << "input\t" << e0.m_ang << "\n"; + cout << "output\t" << e6.m_ang << "\n\n"; + + cout << "Degrees to Radians\n"; + euler_angles e7 = rad2deg(e6); + cout << "input\t" << e6.m_ang << "\n"; + cout << "output\t" << e7.m_ang << "\n\n"; +} + -- 2.7.4