//////////////////////////////////////////////////////////////////////////// // // This file is part of RTIMULib-Arduino // // Copyright (c) 2014-2015, richards-tech // // Permission is hereby granted, free of charge, to any person obtaining a copy of // this software and associated documentation files (the "Software"), to deal in // the Software without restriction, including without limitation the rights to use, // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the // Software, and to permit persons to whom the Software is furnished to do so, // subject to the following conditions: // // The above copyright notice and this permission notice shall be included in all // copies or substantial portions of the Software. // // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. #include "RTMath.h" #include #ifndef RTARDULINK_MODE void RTMath::display(const char *label, RTVector3& vec) { Serial.print(label); Serial.print(" x:"); Serial.print(vec.x()); Serial.print(" y:"); Serial.print(vec.y()); Serial.print(" z:"); Serial.print(vec.z()); } void RTMath::displayDegrees(const char *label, RTVector3& vec) { Serial.print(label); Serial.print(" x:"); Serial.print(vec.x() * RTMATH_RAD_TO_DEGREE); Serial.print(" y:"); Serial.print(vec.y() * RTMATH_RAD_TO_DEGREE); Serial.print(" z:"); Serial.print(vec.z() * RTMATH_RAD_TO_DEGREE); } void RTMath::displayRollPitchYaw(const char *label, RTVector3& vec) { Serial.print(label); Serial.print(" roll:"); Serial.print(vec.x() * RTMATH_RAD_TO_DEGREE); Serial.print(" pitch:"); Serial.print(vec.y() * RTMATH_RAD_TO_DEGREE); Serial.print(" yaw:"); Serial.print(vec.z() * RTMATH_RAD_TO_DEGREE); } void RTMath::display(const char *label, RTQuaternion& quat) { Serial.print(label); Serial.print(" scalar:"); Serial.print(quat.scalar()); Serial.print(" x:"); Serial.print(quat.x()); Serial.print(" y:"); Serial.print(quat.y()); Serial.print(" z:"); Serial.print(quat.z()); } RTVector3 RTMath::poseFromAccelMag(const RTVector3& accel, const RTVector3& mag) { RTVector3 result; RTQuaternion m; RTQuaternion q; accel.accelToEuler(result); // q.fromEuler(result); // since result.z() is always 0, this can be optimized a little RTFLOAT cosX2 = cos(result.x() / 2.0f); RTFLOAT sinX2 = sin(result.x() / 2.0f); RTFLOAT cosY2 = cos(result.y() / 2.0f); RTFLOAT sinY2 = sin(result.y() / 2.0f); q.setScalar(cosX2 * cosY2); q.setX(sinX2 * cosY2); q.setY(cosX2 * sinY2); q.setZ(-sinX2 * sinY2); // q.normalize(); m.setScalar(0); m.setX(mag.x()); m.setY(mag.y()); m.setZ(mag.z()); m = q * m * q.conjugate(); result.setZ(-atan2(m.y(), m.x())); return result; } void RTMath::convertToVector(unsigned char *rawData, RTVector3& vec, RTFLOAT scale, bool bigEndian) { if (bigEndian) { vec.setX((RTFLOAT)((int16_t)(((uint16_t)rawData[0] << 8) | (uint16_t)rawData[1])) * scale); vec.setY((RTFLOAT)((int16_t)(((uint16_t)rawData[2] << 8) | (uint16_t)rawData[3])) * scale); vec.setZ((RTFLOAT)((int16_t)(((uint16_t)rawData[4] << 8) | (uint16_t)rawData[5])) * scale); } else { vec.setX((RTFLOAT)((int16_t)(((uint16_t)rawData[1] << 8) | (uint16_t)rawData[0])) * scale); vec.setY((RTFLOAT)((int16_t)(((uint16_t)rawData[3] << 8) | (uint16_t)rawData[2])) * scale); vec.setZ((RTFLOAT)((int16_t)(((uint16_t)rawData[5] << 8) | (uint16_t)rawData[4])) * scale); } } #endif // #ifndef RTARDULINK_MODE //---------------------------------------------------------- // // The RTVector3 class RTVector3::RTVector3() { zero(); } RTVector3::RTVector3(RTFLOAT x, RTFLOAT y, RTFLOAT z) { m_data[0] = x; m_data[1] = y; m_data[2] = z; } RTVector3& RTVector3::operator =(const RTVector3& vec) { if (this == &vec) return *this; m_data[0] = vec.m_data[0]; m_data[1] = vec.m_data[1]; m_data[2] = vec.m_data[2]; return *this; } const RTVector3& RTVector3::operator +=(RTVector3& vec) { for (int i = 0; i < 3; i++) m_data[i] += vec.m_data[i]; return *this; } const RTVector3& RTVector3::operator -=(RTVector3& vec) { for (int i = 0; i < 3; i++) m_data[i] -= vec.m_data[i]; return *this; } void RTVector3::zero() { for (int i = 0; i < 3; i++) m_data[i] = 0; } #ifndef RTARDULINK_MODE RTFLOAT RTVector3::dotProduct(const RTVector3& a, const RTVector3& b) { return a.x() * b.x() + a.y() * b.y() + a.z() * b.z(); } void RTVector3::crossProduct(const RTVector3& a, const RTVector3& b, RTVector3& d) { d.setX(a.y() * b.z() - a.z() * b.y()); d.setY(a.z() * b.x() - a.x() * b.z()); d.setZ(a.x() * b.y() - a.y() * b.x()); } void RTVector3::accelToEuler(RTVector3& rollPitchYaw) const { RTVector3 normAccel = *this; normAccel.normalize(); rollPitchYaw.setX(atan2(normAccel.y(), normAccel.z())); rollPitchYaw.setY(-atan2(normAccel.x(), sqrt(normAccel.y() * normAccel.y() + normAccel.z() * normAccel.z()))); rollPitchYaw.setZ(0); } void RTVector3::accelToQuaternion(RTQuaternion& qPose) const { RTVector3 normAccel = *this; RTVector3 vec; RTVector3 z(0, 0, 1.0); normAccel.normalize(); RTFLOAT angle = acos(RTVector3::dotProduct(z, normAccel)); RTVector3::crossProduct(normAccel, z, vec); vec.normalize(); qPose.fromAngleVector(angle, vec); } void RTVector3::normalize() { RTFLOAT length = (RTFLOAT)sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1] + m_data[2] * m_data[2]); if ((length == 0) || (length == 1)) return; m_data[0] /= length; m_data[1] /= length; m_data[2] /= length; } RTFLOAT RTVector3::length() { return (RTFLOAT)sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1] + m_data[2] * m_data[2]); } #endif // #ifndef RTARDULINK_MODE RTFLOAT RTVector3::squareLength() { return m_data[0] * m_data[0] + m_data[1] * m_data[1] + m_data[2] * m_data[2]; } #ifndef RTARDULINK_MODE //---------------------------------------------------------- // // The RTQuaternion class RTQuaternion::RTQuaternion() { zero(); } RTQuaternion::RTQuaternion(RTFLOAT scalar, RTFLOAT x, RTFLOAT y, RTFLOAT z) { m_data[0] = scalar; m_data[1] = x; m_data[2] = y; m_data[3] = z; } RTQuaternion& RTQuaternion::operator =(const RTQuaternion& quat) { if (this == &quat) return *this; m_data[0] = quat.m_data[0]; m_data[1] = quat.m_data[1]; m_data[2] = quat.m_data[2]; m_data[3] = quat.m_data[3]; return *this; } RTQuaternion& RTQuaternion::operator +=(const RTQuaternion& quat) { for (int i = 0; i < 4; i++) m_data[i] += quat.m_data[i]; return *this; } RTQuaternion& RTQuaternion::operator -=(const RTQuaternion& quat) { for (int i = 0; i < 4; i++) m_data[i] -= quat.m_data[i]; return *this; } RTQuaternion& RTQuaternion::operator -=(const RTFLOAT val) { for (int i = 0; i < 4; i++) m_data[i] -= val; return *this; } RTQuaternion& RTQuaternion::operator *=(const RTQuaternion& qb) { RTQuaternion qa; qa = *this; m_data[0] = qa.scalar() * qb.scalar() - qa.x() * qb.x() - qa.y() * qb.y() - qa.z() * qb.z(); m_data[1] = qa.scalar() * qb.x() + qa.x() * qb.scalar() + qa.y() * qb.z() - qa.z() * qb.y(); m_data[2] = qa.scalar() * qb.y() - qa.x() * qb.z() + qa.y() * qb.scalar() + qa.z() * qb.x(); m_data[3] = qa.scalar() * qb.z() + qa.x() * qb.y() - qa.y() * qb.x() + qa.z() * qb.scalar(); return *this; } RTQuaternion& RTQuaternion::operator *=(const RTFLOAT val) { m_data[0] *= val; m_data[1] *= val; m_data[2] *= val; m_data[3] *= val; return *this; } const RTQuaternion RTQuaternion::operator *(const RTQuaternion& qb) const { RTQuaternion result = *this; result *= qb; return result; } const RTQuaternion RTQuaternion::operator *(const RTFLOAT val) const { RTQuaternion result = *this; result *= val; return result; } const RTQuaternion RTQuaternion::operator -(const RTQuaternion& qb) const { RTQuaternion result = *this; result -= qb; return result; } const RTQuaternion RTQuaternion::operator -(const RTFLOAT val) const { RTQuaternion result = *this; result -= val; return result; } void RTQuaternion::zero() { for (int i = 0; i < 4; i++) m_data[i] = 0; } void RTQuaternion::normalize() { RTFLOAT length = sqrt(m_data[0] * m_data[0] + m_data[1] * m_data[1] + m_data[2] * m_data[2] + m_data[3] * m_data[3]); if ((length == 0) || (length == 1)) return; m_data[0] /= length; m_data[1] /= length; m_data[2] /= length; m_data[3] /= length; } void RTQuaternion::toEuler(RTVector3& vec) { vec.setX(atan2(2.0 * (m_data[2] * m_data[3] + m_data[0] * m_data[1]), 1 - 2.0 * (m_data[1] * m_data[1] + m_data[2] * m_data[2]))); vec.setY(asin(2.0 * (m_data[0] * m_data[2] - m_data[1] * m_data[3]))); vec.setZ(atan2(2.0 * (m_data[1] * m_data[2] + m_data[0] * m_data[3]), 1 - 2.0 * (m_data[2] * m_data[2] + m_data[3] * m_data[3]))); } void RTQuaternion::fromEuler(RTVector3& vec) { RTFLOAT cosX2 = cos(vec.x() / 2.0f); RTFLOAT sinX2 = sin(vec.x() / 2.0f); RTFLOAT cosY2 = cos(vec.y() / 2.0f); RTFLOAT sinY2 = sin(vec.y() / 2.0f); RTFLOAT cosZ2 = cos(vec.z() / 2.0f); RTFLOAT sinZ2 = sin(vec.z() / 2.0f); m_data[0] = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2; m_data[1] = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2; m_data[2] = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2; m_data[3] = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2; normalize(); } RTQuaternion RTQuaternion::conjugate() const { RTQuaternion q; q.setScalar(m_data[0]); q.setX(-m_data[1]); q.setY(-m_data[2]); q.setZ(-m_data[3]); return q; } void RTQuaternion::toAngleVector(RTFLOAT& angle, RTVector3& vec) { RTFLOAT halfTheta; RTFLOAT sinHalfTheta; halfTheta = acos(m_data[0]); sinHalfTheta = sin(halfTheta); if (sinHalfTheta == 0) { vec.setX(1.0); vec.setY(0); vec.setZ(0); } else { vec.setX(m_data[1] / sinHalfTheta); vec.setY(m_data[1] / sinHalfTheta); vec.setZ(m_data[1] / sinHalfTheta); } angle = 2.0 * halfTheta; } void RTQuaternion::fromAngleVector(const RTFLOAT& angle, const RTVector3& vec) { RTFLOAT sinHalfTheta = sin(angle / 2.0); m_data[0] = cos(angle / 2.0); m_data[1] = vec.x() * sinHalfTheta; m_data[2] = vec.y() * sinHalfTheta; m_data[3] = vec.z() * sinHalfTheta; } #endif // #ifndef RTARDULINK_MODE