c2-utopia/lib/RTIMULib/RTMath.cpp

419 lines
11 KiB
C++

////////////////////////////////////////////////////////////////////////////
//
// 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 <Arduino.h>
#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