void IK_QElbowSegment::SetBasis(const MT_Matrix3x3& basis) { m_basis = basis; m_twist = ComputeTwist(m_basis); RemoveTwist(m_basis); m_angle = EulerAngleFromMatrix(basis, m_axis); m_basis = RotationMatrix(m_angle, m_axis)*ComputeTwistMatrix(m_twist); }
static void RemoveTwist(MT_Matrix3x3& R) { // compute twist parameter MT_Scalar tau = ComputeTwist(R); // compute twist matrix MT_Matrix3x3 T = ComputeTwistMatrix(tau); // remove twist R = R*T.transposed(); }
void IK_QRevoluteSegment::SetBasis(const MT_Matrix3x3& basis) { if (m_axis == 1) { m_angle = ComputeTwist(basis); m_basis = ComputeTwistMatrix(m_angle); } else { m_angle = EulerAngleFromMatrix(basis, m_axis); m_basis = RotationMatrix(m_angle, m_axis); } }
static MT_Vector3 SphericalRangeParameters(const MT_Matrix3x3& R) { // compute twist parameter MT_Scalar tau = ComputeTwist(R); // compute swing parameters MT_Scalar num = 2.0*(1.0 + R[1][1]); // singularity at pi if (MT_abs(num) < MT_EPSILON) // TODO: this does now rotation of size pi over z axis, but could // be any axis, how to deal with this i'm not sure, maybe don't // enforce limits at all then return MT_Vector3(0.0, tau, 1.0); num = 1.0/sqrt(num); MT_Scalar ax = -R[2][1]*num; MT_Scalar az = R[0][1]*num; return MT_Vector3(ax, tau, az); }
//_____________________________________________________________________________ TGeoArb8::TGeoArb8(Double_t dz, Double_t *vertices) :TGeoBBox(0,0,0) { // constructor. If the array of vertices is not null, this should be // in the format : (x0, y0, x1, y1, ... , x7, y7) fDz = dz; fTwist = 0; SetShapeBit(kGeoArb8); if (vertices) { for (Int_t i=0; i<8; i++) { fXY[i][0] = vertices[2*i]; fXY[i][1] = vertices[2*i+1]; } ComputeTwist(); ComputeBBox(); } else { for (Int_t i=0; i<8; i++) { fXY[i][0] = 0.0; fXY[i][1] = 0.0; } } }