p80426359
/
pxmlw6n2f
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
pxmlw6n2f/Gazebo_Distributed_TCP/gazebo/math/Matrix4.cc

513 lines
16 KiB
C++
Raw Blame History

/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* 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 <string.h>
#include "gazebo/math/Helpers.hh"
#include "gazebo/math/Matrix4.hh"
#include "gazebo/math/Quaternion.hh"
#include "gazebo/math/Pose.hh"
using namespace gazebo;
using namespace math;
const Matrix4 Matrix4::IDENTITY(
1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0);
const Matrix4 Matrix4::ZERO(
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0);
//////////////////////////////////////////////////
Matrix4::Matrix4()
{
memset(this->m, 0, sizeof(this->m[0][0])*16);
}
//////////////////////////////////////////////////
Matrix4::Matrix4(const Matrix4 &_m)
{
memcpy(this->m, _m.m, sizeof(this->m[0][0])*16);
}
//////////////////////////////////////////////////
Matrix4::Matrix4(const ignition::math::Matrix4d &_m)
{
this->Set(_m(0, 0), _m(0, 1), _m(0, 2), _m(0, 3),
_m(1, 0), _m(1, 1), _m(1, 2), _m(1, 3),
_m(2, 0), _m(2, 1), _m(2, 2), _m(2, 3),
_m(3, 0), _m(3, 1), _m(3, 2), _m(3, 3));
}
//////////////////////////////////////////////////
Matrix4::Matrix4(double _v00, double _v01, double _v02, double _v03,
double _v10, double _v11, double _v12, double _v13,
double _v20, double _v21, double _v22, double _v23,
double _v30, double _v31, double _v32, double _v33)
{
this->Set(_v00, _v01, _v02, _v03,
_v10, _v11, _v12, _v13,
_v20, _v21, _v22, _v23,
_v30, _v31, _v32, _v33);
}
//////////////////////////////////////////////////
Matrix4::~Matrix4()
{
}
//////////////////////////////////////////////////
void Matrix4::Set(double _v00, double _v01, double _v02, double _v03,
double _v10, double _v11, double _v12, double _v13,
double _v20, double _v21, double _v22, double _v23,
double _v30, double _v31, double _v32, double _v33)
{
this->m[0][0] = _v00;
this->m[0][1] = _v01;
this->m[0][2] = _v02;
this->m[0][3] = _v03;
this->m[1][0] = _v10;
this->m[1][1] = _v11;
this->m[1][2] = _v12;
this->m[1][3] = _v13;
this->m[2][0] = _v20;
this->m[2][1] = _v21;
this->m[2][2] = _v22;
this->m[2][3] = _v23;
this->m[3][0] = _v30;
this->m[3][1] = _v31;
this->m[3][2] = _v32;
this->m[3][3] = _v33;
}
//////////////////////////////////////////////////
void Matrix4::SetTranslate(const Vector3 &_t)
{
this->m[0][3] = _t.x;
this->m[1][3] = _t.y;
this->m[2][3] = _t.z;
}
//////////////////////////////////////////////////
Vector3 Matrix4::GetTranslation() const
{
return Vector3(this->m[0][3], this->m[1][3], this->m[2][3]);
}
//////////////////////////////////////////////////
Quaternion Matrix4::GetRotation() const
{
Quaternion q;
/// algorithm from Ogre::Quaternion source, which in turn is based on
/// Ken Shoemake's article "Quaternion Calculus and Fast Animation".
double trace = this->m[0][0] + this->m[1][1] + this->m[2][2];
double root;
if (trace > 0)
{
root = sqrt(trace + 1.0);
q.w = root / 2.0;
root = 1.0 / (2.0 * root);
q.x = (this->m[2][1] - this->m[1][2]) * root;
q.y = (this->m[0][2] - this->m[2][0]) * root;
q.z = (this->m[1][0] - this->m[0][1]) * root;
}
else
{
static unsigned int s_iNext[3] = {1, 2, 0};
unsigned int i = 0;
if (this->m[1][1] > this->m[0][0])
i = 1;
if (this->m[2][2] > this->m[i][i])
i = 2;
unsigned int j = s_iNext[i];
unsigned int k = s_iNext[j];
root = sqrt(this->m[i][i] - this->m[j][j] - this->m[k][k] + 1.0);
double* xyzQ[3] = { &q.x, &q.y, &q.z};
*xyzQ[i] = root / 2.0;
root = 1.0 / (2.0 * root);
q.w = (this->m[k][j] - this->m[j][k]) * root;
*xyzQ[j] = (this->m[j][i] + this->m[i][j]) * root;
*xyzQ[k] = (this->m[k][i] + this->m[i][k]) * root;
}
return q;
}
//////////////////////////////////////////////////
Vector3 Matrix4::GetEulerRotation(unsigned int solution_number) const
{
Vector3 euler;
Vector3 euler2;
double m31 = this->m[2][0];
double m11 = this->m[0][0];
double m12 = this->m[0][1];
double m13 = this->m[0][2];
double m32 = this->m[2][1];
double m33 = this->m[2][2];
double m21 = this->m[1][0];
if (fabs(m31) >= 1.0)
{
euler.z = 0.0;
euler2.z = 0.0;
if (m31 < 0.0)
{
euler.y = M_PI / 2.0;
euler2.y = M_PI / 2.0;
euler.x = atan2(m12, m13);
euler2.x = atan2(m12, m13);
}
else
{
euler.y = -M_PI / 2.0;
euler2.y = -M_PI / 2.0;
euler.x = atan2(-m12, -m13);
euler2.x = atan2(-m12, -m13);
}
}
else
{
euler.y = -asin(m31);
euler2.y = M_PI - euler.y;
euler.x = atan2(m32 / cos(euler.y), m33 / cos(euler.y));
euler2.x = atan2(m32 / cos(euler2.y), m33 / cos(euler2.y));
euler.z = atan2(m21 / cos(euler.y), m11 / cos(euler.y));
euler2.z = atan2(m21 / cos(euler2.y), m11 / cos(euler2.y));
}
if (solution_number == 1)
return euler;
else
return euler2;
}
//////////////////////////////////////////////////
void Matrix4::SetScale(const Vector3 &_s)
{
this->m[0][0] = _s.x;
this->m[1][1] = _s.y;
this->m[2][2] = _s.z;
this->m[3][3] = 1.0;
}
//////////////////////////////////////////////////
Matrix4 &Matrix4::operator =(const Matrix4 &_mat)
{
memcpy(this->m, _mat.m, sizeof(this->m[0][0])*16);
return *this;
}
//////////////////////////////////////////////////
Matrix4 &Matrix4::operator=(const ignition::math::Matrix4d &_m)
{
this->Set(_m(0, 0), _m(0, 1), _m(0, 2), _m(0, 3),
_m(1, 0), _m(1, 1), _m(1, 2), _m(1, 3),
_m(2, 0), _m(2, 1), _m(2, 2), _m(2, 3),
_m(3, 0), _m(3, 1), _m(3, 2), _m(3, 3));
return *this;
}
//////////////////////////////////////////////////
const Matrix4 &Matrix4::operator =(const Matrix3 &mat)
{
this->m[0][0] = mat.m[0][0];
this->m[0][1] = mat.m[0][1];
this->m[0][2] = mat.m[0][2];
this->m[1][0] = mat.m[1][0];
this->m[1][1] = mat.m[1][1];
this->m[1][2] = mat.m[1][2];
this->m[2][0] = mat.m[2][0];
this->m[2][1] = mat.m[2][1];
this->m[2][2] = mat.m[2][2];
return *this;
}
//////////////////////////////////////////////////
Matrix4 Matrix4::operator*(const Matrix3 &m2) const
{
Matrix4 r;
r = *this;
r.m[0][0] = m[0][0]*m2.m[0][0] + m[0][1]*m2.m[1][0] + m[0][2] * m2.m[2][0];
r.m[0][1] = m[0][0]*m2.m[0][1] + m[0][1]*m2.m[1][1] + m[0][2] * m2.m[2][1];
r.m[0][2] = m[0][0]*m2.m[0][2] + m[0][1]*m2.m[1][2] + m[0][2] * m2.m[2][2];
r.m[1][0] = m[1][0]*m2.m[0][0] + m[1][1]*m2.m[1][0] + m[1][2] * m2.m[2][0];
r.m[1][1] = m[1][0]*m2.m[0][1] + m[1][1]*m2.m[1][1] + m[1][2] * m2.m[2][1];
r.m[1][2] = m[1][0]*m2.m[0][2] + m[1][1]*m2.m[1][2] + m[1][2] * m2.m[2][2];
r.m[2][0] = m[2][0]*m2.m[0][0] + m[2][1]*m2.m[1][0] + m[2][2] * m2.m[2][0];
r.m[2][1] = m[2][0]*m2.m[0][1] + m[2][1]*m2.m[1][1] + m[2][2] * m2.m[2][1];
r.m[2][2] = m[2][0]*m2.m[0][2] + m[2][1]*m2.m[1][2] + m[2][2] * m2.m[2][2];
return r;
}
//////////////////////////////////////////////////
Matrix4 Matrix4::operator*(const Matrix4 &m2) const
{
Matrix4 r;
r.m[0][0] = this->m[0][0] * m2.m[0][0] +
this->m[0][1] * m2.m[1][0] +
this->m[0][2] * m2.m[2][0] +
this->m[0][3] * m2.m[3][0];
r.m[0][1] = this->m[0][0] * m2.m[0][1] +
this->m[0][1] * m2.m[1][1] +
this->m[0][2] * m2.m[2][1] +
this->m[0][3] * m2.m[3][1];
r.m[0][2] = this->m[0][0] * m2.m[0][2] +
this->m[0][1] * m2.m[1][2] +
this->m[0][2] * m2.m[2][2] +
this->m[0][3] * m2.m[3][2];
r.m[0][3] = this->m[0][0] * m2.m[0][3] +
this->m[0][1] * m2.m[1][3] +
this->m[0][2] * m2.m[2][3] +
this->m[0][3] * m2.m[3][3];
r.m[1][0] = this->m[1][0] * m2.m[0][0] +
this->m[1][1] * m2.m[1][0] +
this->m[1][2] * m2.m[2][0] +
this->m[1][3] * m2.m[3][0];
r.m[1][1] = this->m[1][0] * m2.m[0][1] +
this->m[1][1] * m2.m[1][1] +
this->m[1][2] * m2.m[2][1] +
this->m[1][3] * m2.m[3][1];
r.m[1][2] = this->m[1][0] * m2.m[0][2] +
this->m[1][1] * m2.m[1][2] +
this->m[1][2] * m2.m[2][2] +
this->m[1][3] * m2.m[3][2];
r.m[1][3] = this->m[1][0] * m2.m[0][3] +
this->m[1][1] * m2.m[1][3] +
this->m[1][2] * m2.m[2][3] +
this->m[1][3] * m2.m[3][3];
r.m[2][0] = this->m[2][0] * m2.m[0][0] +
this->m[2][1] * m2.m[1][0] +
this->m[2][2] * m2.m[2][0] +
this->m[2][3] * m2.m[3][0];
r.m[2][1] = this->m[2][0] * m2.m[0][1] +
this->m[2][1] * m2.m[1][1] +
this->m[2][2] * m2.m[2][1] +
this->m[2][3] * m2.m[3][1];
r.m[2][2] = this->m[2][0] * m2.m[0][2] +
this->m[2][1] * m2.m[1][2] +
this->m[2][2] * m2.m[2][2] +
this->m[2][3] * m2.m[3][2];
r.m[2][3] = this->m[2][0] * m2.m[0][3] +
this->m[2][1] * m2.m[1][3] +
this->m[2][2] * m2.m[2][3] +
this->m[2][3] * m2.m[3][3];
r.m[3][0] = this->m[3][0] * m2.m[0][0] +
this->m[3][1] * m2.m[1][0] +
this->m[3][2] * m2.m[2][0] +
this->m[3][3] * m2.m[3][0];
r.m[3][1] = this->m[3][0] * m2.m[0][1] +
this->m[3][1] * m2.m[1][1] +
this->m[3][2] * m2.m[2][1] +
this->m[3][3] * m2.m[3][1];
r.m[3][2] = this->m[3][0] * m2.m[0][2] +
this->m[3][1] * m2.m[1][2] +
this->m[3][2] * m2.m[2][2] +
this->m[3][3] * m2.m[3][2];
r.m[3][3] = this->m[3][0] * m2.m[0][3] +
this->m[3][1] * m2.m[1][3] +
this->m[3][2] * m2.m[2][3] +
this->m[3][3] * m2.m[3][3];
return r;
}
//////////////////////////////////////////////////
Vector3 Matrix4::operator*(const Vector3 &_vec) const
{
Vector3 result;
result.x = this->m[0][0]*_vec.x + this->m[0][1]*_vec.y +
this->m[0][2]*_vec.z + this->m[0][3];
result.y = this->m[1][0]*_vec.x + this->m[1][1]*_vec.y +
this->m[1][2]*_vec.z + this->m[1][3];
result.z = this->m[2][0]*_vec.x + this->m[2][1]*_vec.y +
this->m[2][2]*_vec.z + this->m[2][3];
return result;
}
//////////////////////////////////////////////////
bool Matrix4::IsAffine() const
{
return equal(this->m[3][0], 0.0) && equal(this->m[3][1], 0.0) &&
equal(this->m[3][2], 0.0) && equal(this->m[3][3], 1.0);
}
//////////////////////////////////////////////////
Vector3 Matrix4::TransformAffine(const Vector3 &_v) const
{
if (!this->IsAffine())
{
throw(std::string("Not and affine matrix"));
}
return Vector3(this->m[0][0]*_v.x + this->m[0][1]*_v.y +
this->m[0][2]*_v.z + this->m[0][3],
this->m[1][0]*_v.x + this->m[1][1]*_v.y +
this->m[1][2]*_v.z + this->m[1][3],
this->m[2][0]*_v.x + this->m[2][1]*_v.y +
this->m[2][2]*_v.z + this->m[2][3]);
}
//////////////////////////////////////////////////
bool Matrix4::operator==(const Matrix4 &_m) const
{
return math::equal(this->m[0][0], _m[0][0]) &&
math::equal(this->m[0][1], _m[0][1]) &&
math::equal(this->m[0][2], _m[0][2]) &&
math::equal(this->m[0][3], _m[0][3]) &&
math::equal(this->m[1][0], _m[1][0]) &&
math::equal(this->m[1][1], _m[1][1]) &&
math::equal(this->m[1][2], _m[1][2]) &&
math::equal(this->m[1][3], _m[1][3]) &&
math::equal(this->m[2][0], _m[2][0]) &&
math::equal(this->m[2][1], _m[2][1]) &&
math::equal(this->m[2][2], _m[2][2]) &&
math::equal(this->m[2][3], _m[2][3]) &&
math::equal(this->m[3][0], _m[3][0]) &&
math::equal(this->m[3][1], _m[3][1]) &&
math::equal(this->m[3][2], _m[3][2]) &&
math::equal(this->m[3][3], _m[3][3]);
}
//////////////////////////////////////////////////
Matrix4 Matrix4::Inverse() const
{
double v0 = this->m[2][0] * this->m[3][1] - this->m[2][1] * this->m[3][0];
double v1 = this->m[2][0] * this->m[3][2] - this->m[2][2] * this->m[3][0];
double v2 = this->m[2][0] * this->m[3][3] - this->m[2][3] * this->m[3][0];
double v3 = this->m[2][1] * this->m[3][2] - this->m[2][2] * this->m[3][1];
double v4 = this->m[2][1] * this->m[3][3] - this->m[2][3] * this->m[3][1];
double v5 = this->m[2][2] * this->m[3][3] - this->m[2][3] * this->m[3][2];
double t00 = + (v5 * this->m[1][1] - v4 * this->m[1][2] + v3 * this->m[1][3]);
double t10 = - (v5 * this->m[1][0] - v2 * this->m[1][2] + v1 * this->m[1][3]);
double t20 = + (v4 * this->m[1][0] - v2 * this->m[1][1] + v0 * this->m[1][3]);
double t30 = - (v3 * this->m[1][0] - v1 * this->m[1][1] + v0 * this->m[1][2]);
double invDet = 1 / (t00 * this->m[0][0] + t10 * this->m[0][1] +
t20 * this->m[0][2] + t30 * this->m[0][3]);
double d00 = t00 * invDet;
double d10 = t10 * invDet;
double d20 = t20 * invDet;
double d30 = t30 * invDet;
double d01 = - (v5 * this->m[0][1] - v4 * this->m[0][2] + v3 * this->m[0][3])
* invDet;
double d11 = + (v5 * this->m[0][0] - v2 * this->m[0][2] + v1 * this->m[0][3])
* invDet;
double d21 = - (v4 * this->m[0][0] - v2 * this->m[0][1] + v0 * this->m[0][3])
* invDet;
double d31 = + (v3 * this->m[0][0] - v1 * this->m[0][1] + v0 * this->m[0][2])
* invDet;
v0 = this->m[1][0] * this->m[3][1] - this->m[1][1] * this->m[3][0];
v1 = this->m[1][0] * this->m[3][2] - this->m[1][2] * this->m[3][0];
v2 = this->m[1][0] * this->m[3][3] - this->m[1][3] * this->m[3][0];
v3 = this->m[1][1] * this->m[3][2] - this->m[1][2] * this->m[3][1];
v4 = this->m[1][1] * this->m[3][3] - this->m[1][3] * this->m[3][1];
v5 = this->m[1][2] * this->m[3][3] - this->m[1][3] * this->m[3][2];
double d02 = + (v5 * this->m[0][1] - v4 * this->m[0][2] + v3 * this->m[0][3])
* invDet;
double d12 = - (v5 * this->m[0][0] - v2 * this->m[0][2] + v1 * this->m[0][3])
* invDet;
double d22 = + (v4 * this->m[0][0] - v2 * this->m[0][1] + v0 * this->m[0][3])
* invDet;
double d32 = - (v3 * this->m[0][0] - v1 * this->m[0][1] + v0 * this->m[0][2])
* invDet;
v0 = this->m[2][1] * this->m[1][0] - this->m[2][0] * this->m[1][1];
v1 = this->m[2][2] * this->m[1][0] - this->m[2][0] * this->m[1][2];
v2 = this->m[2][3] * this->m[1][0] - this->m[2][0] * this->m[1][3];
v3 = this->m[2][2] * this->m[1][1] - this->m[2][1] * this->m[1][2];
v4 = this->m[2][3] * this->m[1][1] - this->m[2][1] * this->m[1][3];
v5 = this->m[2][3] * this->m[1][2] - this->m[2][2] * this->m[1][3];
double d03 = - (v5 * this->m[0][1] - v4 * this->m[0][2] + v3 * this->m[0][3])
* invDet;
double d13 = + (v5 * this->m[0][0] - v2 * this->m[0][2] + v1 * this->m[0][3])
* invDet;
double d23 = - (v4 * this->m[0][0] - v2 * this->m[0][1] + v0 * this->m[0][3])
* invDet;
double d33 = + (v3 * this->m[0][0] - v1 * this->m[0][1] + v0 * this->m[0][2])
* invDet;
return Matrix4(d00, d01, d02, d03,
d10, d11, d12, d13,
d20, d21, d22, d23,
d30, d31, d32, d33);
}
//////////////////////////////////////////////////
math::Pose Matrix4::GetAsPose() const
{
return math::Pose(this->GetTranslation(), this->GetRotation());
}
//////////////////////////////////////////////////
ignition::math::Matrix4d Matrix4::Ign() const
{
return ignition::math::Matrix4d(
this->m[0][0], this->m[0][1], this->m[0][2], this->m[0][3],
this->m[1][0], this->m[1][1], this->m[1][2], this->m[1][3],
this->m[2][0], this->m[2][1], this->m[2][2], this->m[2][3],
this->m[3][0], this->m[3][1], this->m[3][2], this->m[3][3]);
}
Reference in New Issue View Git Blame Copy Permalink