17 #ifndef IGNITION_MATH_MASSMATRIX3_HH_
18 #define IGNITION_MATH_MASSMATRIX3_HH_
24 #include <ignition/math/config.hh>
35 inline namespace IGNITION_MATH_VERSION_NAMESPACE
55 : mass(_mass), Ixxyyzz(_ixxyyzz), Ixyxzyz(_ixyxzyz)
71 public:
bool Mass(
const T &_m)
93 const T &_ixy,
const T &_ixz,
const T &_iyz)
95 this->Ixxyyzz.Set(_ixx, _iyy, _izz);
96 this->Ixyxzyz.Set(_ixy, _ixz, _iyz);
104 return this->Ixxyyzz;
111 return this->Ixyxzyz;
119 this->Ixxyyzz = _ixxyyzz;
128 this->Ixyxzyz = _ixyxzyz;
136 return this->Ixxyyzz[0];
143 return this->Ixxyyzz[1];
150 return this->Ixxyyzz[2];
157 return this->Ixyxzyz[0];
164 return this->Ixyxzyz[1];
171 return this->Ixyxzyz[2];
177 public:
bool IXX(
const T &_v)
186 public:
bool IYY(
const T &_v)
195 public:
bool IZZ(
const T &_v)
204 public:
bool IXY(
const T &_v)
213 public:
bool IXZ(
const T &_v)
222 public:
bool IYZ(
const T &_v)
233 this->Ixxyyzz[0], this->Ixyxzyz[0], this->Ixyxzyz[1],
234 this->Ixyxzyz[0], this->Ixxyyzz[1], this->Ixyxzyz[2],
235 this->Ixyxzyz[1], this->Ixyxzyz[2], this->Ixxyyzz[2]);
245 this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2));
247 0.5*(_moi(0, 1) + _moi(1, 0)),
248 0.5*(_moi(0, 2) + _moi(2, 0)),
249 0.5*(_moi(1, 2) + _moi(2, 1)));
258 this->mass = _massMatrix.
Mass();
271 return equal<T>(this->mass, _m.
Mass()) &&
281 return !(*
this == _m);
291 return (this->mass > 0) &&
293 (this->
IXX()*this->
IYY() - std::pow(this->
IXY(), 2) > 0) &&
294 (this->
MOI().Determinant() > 0);
313 return _moments[0] > 0 &&
316 _moments[0] + _moments[1] > _moments[2] &&
317 _moments[1] + _moments[2] > _moments[0] &&
318 _moments[2] + _moments[0] > _moments[1];
335 T tol = _tol * this->Ixxyyzz.Max();
339 return this->Ixxyyzz;
350 T c = Id[0]*Id[1] - std::pow(Ip[0], 2)
351 + Id[0]*Id[2] - std::pow(Ip[1], 2)
352 + Id[1]*Id[2] - std::pow(Ip[2], 2);
354 T d = Id[0]*std::pow(Ip[2], 2)
355 + Id[1]*std::pow(Ip[1], 2)
356 + Id[2]*std::pow(Ip[0], 2)
358 - 2*Ip[0]*Ip[1]*Ip[2];
360 T p = std::pow(b, 2) - 3*c;
369 if (p < std::pow(tol, 2))
373 T q = 2*std::pow(b, 3) - 9*b*c - 27*d;
377 T delta = acos(clamp<T>(0.5 * q / std::pow(p, 1.5), -1, 1));
380 T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
381 T moment1 = (b + 2*sqrt(p) * cos((delta + 2*
IGN_PI)/3.0)) / 3.0;
382 T moment2 = (b + 2*sqrt(p) * cos((delta - 2*
IGN_PI)/3.0)) / 3.0;
383 sort3(moment0, moment1, moment2);
401 T tol = _tol * this->Ixxyyzz.Max();
403 if (moments.
Equal(this->Ixxyyzz, tol) ||
404 (math::equal<T>(moments[0], moments[1], std::abs(tol)) &&
405 math::equal<T>(moments[0], moments[2], std::abs(tol))))
429 Vector2<T> f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]);
430 Vector2<T> f2(this->Ixxyyzz[1] - this->Ixxyyzz[2],
431 -2*this->Ixyxzyz[2]);
435 Vector2<T> momentsDiff(moments[0] - moments[1],
436 moments[1] - moments[2]);
439 int unequalMoment = -1;
440 if (equal<T>(momentsDiff[0], 0, std::abs(tol)))
442 else if (equal<T>(momentsDiff[1], 0, std::abs(tol)))
445 if (unequalMoment >= 0)
450 T momentsDiff3 = moments[1] - moments[unequalMoment];
454 T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3;
460 T phi2 = acos(clamp<T>(ClampedSqrt(s), -1, 1));
469 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
497 Vector2<T> g1a(0, 0.5*momentsDiff3 * sin(2*phi2));
500 math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
505 Vector2<T> g1b(0, 0.5*momentsDiff3 * sin(-2*phi2));
508 math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
514 T erra = std::pow(sin(phi1) - sin(phi11a.
Radian()), 2)
515 + std::pow(cos(phi1) - cos(phi11a.
Radian()), 2);
516 T errb = std::pow(sin(phi1) - sin(phi11b.
Radian()), 2)
517 + std::pow(cos(phi1) - cos(phi11b.
Radian()), 2);
537 if (unequalMoment == 0)
545 T v = (std::pow(this->Ixyxzyz[0], 2) + std::pow(this->Ixyxzyz[1], 2)
546 +(this->Ixxyyzz[0] - moments[2])
547 *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1]))
548 / ((moments[1] - moments[2]) * (moments[2] - moments[0]));
551 if (v < std::abs(tol))
560 w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v)
561 / ((moments[0] - moments[1]) * v);
566 T phi2 = acos(clamp<T>(ClampedSqrt(v), -1, 1));
568 T phi3 = acos(clamp<T>(ClampedSqrt(w), -1, 1));
573 0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3),
574 0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2));
576 (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v,
577 (moments[0]-moments[1])*sin(phi2)*sin(2*phi3));
592 if (f1small && f2small)
603 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
610 math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
618 math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
621 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
623 T err = std::pow(sin(phi11.
Radian()) - sin(phi12.
Radian()), 2)
624 + std::pow(cos(phi11.
Radian()) - cos(phi12.
Radian()), 2);
631 math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
632 math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol)));
635 T erra = std::pow(sin(phi11a.
Radian()) - sin(phi12a.
Radian()), 2)
636 + std::pow(cos(phi11a.
Radian()) - cos(phi12a.
Radian()), 2);
641 signsPhi23.
Set(-1, 1);
648 math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
649 math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol)));
652 T errb = std::pow(sin(phi11b.
Radian()) - sin(phi12b.
Radian()), 2)
653 + std::pow(cos(phi11b.
Radian()) - cos(phi12b.
Radian()), 2);
658 signsPhi23.
Set(1, -1);
665 math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol));
666 math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol)));
669 T errc = std::pow(sin(phi11c.
Radian()) - sin(phi12c.
Radian()), 2)
670 + std::pow(cos(phi11c.
Radian()) - cos(phi12c.
Radian()), 2);
675 signsPhi23.
Set(-1, -1);
680 phi2 *= signsPhi23[0];
681 phi3 *= signsPhi23[1];
699 const T _tol = 1e-6)
const
719 _size.
X(sqrt(6*(moments.
Y() + moments.
Z() - moments.
X()) / this->mass));
720 _size.
Y(sqrt(6*(moments.
Z() + moments.
X() - moments.
Y()) / this->mass));
721 _size.
Z(sqrt(6*(moments.
X() + moments.
Y() - moments.
Z()) / this->mass));
764 if (this->
Mass() <= 0 || _size.
Min() <= 0 ||
772 T x2 = std::pow(_size.
X(), 2);
773 T y2 = std::pow(_size.
Y(), 2);
774 T z2 = std::pow(_size.
Z(), 2);
775 L(0, 0) = this->mass / 12.0 * (y2 + z2);
776 L(1, 1) = this->mass / 12.0 * (z2 + x2);
777 L(2, 2) = this->mass / 12.0 * (x2 + y2);
796 if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
817 if (this->
Mass() <= 0 || _length <= 0 || _radius <= 0 ||
824 T radius2 = std::pow(_radius, 2);
826 L(0, 0) = this->mass / 12.0 * (3*radius2 + std::pow(_length, 2));
828 L(2, 2) = this->mass / 2.0 * radius2;
840 if (_mass <= 0 || _radius <= 0)
855 if (this->
Mass() <= 0 || _radius <= 0)
861 T radius2 = std::pow(_radius, 2);
863 L(0, 0) = 0.4 * this->mass * radius2;
864 L(1, 1) = 0.4 * this->mass * radius2;
865 L(2, 2) = 0.4 * this->mass * radius2;
872 private:
static inline T ClampedSqrt(
const T &_x)
884 private:
static T Angle2(
const Vector2<T> &_v,
const T _eps = 1e-6)
888 return atan2(_v[1], _v[0]);
897 private: Vector3<T> Ixxyyzz;
902 private: Vector3<T> Ixyxzyz;