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;
}
}
}
