247 this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2));
249 0.5*(_moi(0, 1) + _moi(1, 0)),
250 0.5*(_moi(0, 2) + _moi(2, 0)),
251 0.5*(_moi(1, 2) + _moi(2, 1)));
252 return this->IsValid();
447 const T _tolerance = GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
449 T epsilon = Epsilon(_moments, _tolerance);
451 return _moments[0] + epsilon >= 0 &&
452 _moments[1] + epsilon >= 0 &&
453 _moments[2] + epsilon >= 0 &&
454 _moments[0] + _moments[1] + epsilon >= _moments[2] &&
455 _moments[1] + _moments[2] + epsilon >= _moments[0] &&
456 _moments[2] + _moments[0] + epsilon >= _moments[1];
473 T tol = _tol * this->Ixxyyzz.Max();
477 return this->Ixxyyzz;
488 T c = Id[0]*Id[1] -
std::pow(Ip[0], 2)
496 - 2*Ip[0]*Ip[1]*Ip[2];
511 T q = 2*
std::pow(b, 3) - 9*b*c - 27*d;
515 T delta = acos(clamp<T>(0.5 * q /
std::pow(p, 1.5), -1, 1));
518 T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
519 T moment1 = (b + 2*sqrt(p) * cos((delta + 2*
GZ_PI)/3.0)) / 3.0;
520 T moment2 = (b + 2*sqrt(p) * cos((delta - 2*
GZ_PI)/3.0)) / 3.0;
521 sort3(moment0, moment1, moment2);
538 Vector3<T> moments = this->PrincipalMoments(_tol);
540 T tol = _tol * this->Ixxyyzz.Max();
541 if (moments.
Equal(this->Ixxyyzz, tol) ||
542 (math::equal<T>(moments[0], moments[1], std::abs(tol)) &&
543 math::equal<T>(moments[0], moments[2], std::abs(tol))))
567 Vector2<T> f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]);
568 Vector2<T> f2(this->Ixxyyzz[1] - this->Ixxyyzz[2],
569 -2*this->Ixyxzyz[2]);
573 Vector2<T> momentsDiff(moments[0] - moments[1],
574 moments[1] - moments[2]);
577 int unequalMoment = -1;
578 if (equal<T>(momentsDiff[0], 0, std::abs(tol)))
580 else if (equal<T>(momentsDiff[1], 0, std::abs(tol)))
583 if (unequalMoment >= 0)
588 T momentsDiff3 = moments[1] - moments[unequalMoment];
592 T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3;
598 T phi2 = acos(clamp<T>(ClampedSqrt(s), -1, 1));
607 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
635 Vector2<T> g1a(0, 0.5*momentsDiff3 * sin(2*phi2));
638 math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
643 Vector2<T> g1b(0, 0.5*momentsDiff3 * sin(-2*phi2));
646 math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
675 if (unequalMoment == 0)
684 +(this->Ixxyyzz[0] - moments[2])
685 *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1]))
686 / ((moments[1] - moments[2]) * (moments[2] - moments[0]));
689 if (v < std::abs(tol))
698 w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v)
699 / ((moments[0] - moments[1]) * v);
704 T phi2 = acos(clamp<T>(ClampedSqrt(v), -1, 1));
706 T phi3 = acos(clamp<T>(ClampedSqrt(w), -1, 1));
711 0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3),
712 0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2));
714 (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v,
715 (moments[0]-moments[1])*sin(phi2)*sin(2*phi3));
730 if (f1small && f2small)
741 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
748 math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
756 math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
759 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
769 math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
770 math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol)));
779 signsPhi23.
Set(-1, 1);
786 math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
787 math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol)));
796 signsPhi23.
Set(1, -1);
803 math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol));
804 math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol)));
812 signsPhi23.
Set(-1, -1);
817 phi2 *= signsPhi23[0];
818 phi3 *= signsPhi23[1];