------------------------------------------------------------------------------
| 936 | |
| 937 | //------------------------------------------------------------------------------ |
| 938 | void vtkTensorRepresentation::UpdatePose( |
| 939 | const double* pos1, const double* orient1, const double* pos2, const double* orient2) |
| 940 | { |
| 941 | bool newSnap[3]; |
| 942 | vtkVector3d basis[3]; |
| 943 | double basisSize[3]; |
| 944 | |
| 945 | vtkQuaternion<double> q2; |
| 946 | q2.SetRotationAngleAndAxis( |
| 947 | vtkMath::RadiansFromDegrees(orient2[0]), orient2[1], orient2[2], orient2[3]); |
| 948 | |
| 949 | for (int i = 0; i < 3; ++i) |
| 950 | { |
| 951 | newSnap[i] = false; |
| 952 | // compute the net rotation |
| 953 | vtkQuaternion<double> q1; |
| 954 | if (this->SnappedOrientation[i]) |
| 955 | { |
| 956 | q1.SetRotationAngleAndAxis(vtkMath::RadiansFromDegrees(this->SnappedEventOrientations[i][0]), |
| 957 | this->SnappedEventOrientations[i][1], this->SnappedEventOrientations[i][2], |
| 958 | this->SnappedEventOrientations[i][3]); |
| 959 | } |
| 960 | else |
| 961 | { |
| 962 | q1.SetRotationAngleAndAxis( |
| 963 | vtkMath::RadiansFromDegrees(orient1[0]), orient1[1], orient1[2], orient1[3]); |
| 964 | } |
| 965 | q1.Conjugate(); |
| 966 | vtkQuaternion<double> q3 = q2 * q1; |
| 967 | double axis[4]; |
| 968 | axis[0] = vtkMath::DegreesFromRadians(q3.GetRotationAngleAndAxis(axis + 1)); |
| 969 | |
| 970 | // Manipulate the transform to reflect the rotation |
| 971 | this->Transform->Identity(); |
| 972 | this->Transform->RotateWXYZ(axis[0], axis[1], axis[2], axis[3]); |
| 973 | |
| 974 | // Set the corners |
| 975 | this->TmpPoints->Reset(); |
| 976 | this->Transform->TransformPoints(this->Points, this->TmpPoints); |
| 977 | |
| 978 | vtkVector3d p0(this->TmpPoints->GetPoint(0)); |
| 979 | vtkVector3d p1(this->TmpPoints->GetPoint((i > 0 ? i + 2 : 1))); |
| 980 | basis[i] = p1 - p0; |
| 981 | basisSize[i] = 0.5 * basis[i].Normalize(); |
| 982 | if (this->SnapToAxes) |
| 983 | { |
| 984 | // 14 degrees to snap in, 16 to snap out |
| 985 | // avoids noise on the boundary |
| 986 | newSnap[i] = snapToAxis(basis[i], basis[i], (this->SnappedOrientation[i] ? 16 : 14)); |
| 987 | } |
| 988 | } |
| 989 | |
| 990 | // orthogonalize the resulting basis |
| 991 | for (int i = 0; i < 3; ++i) |
| 992 | { |
| 993 | if (newSnap[i] || this->SnappedOrientation[i]) |
| 994 | { |
| 995 | // orthogonalize the other axes |
no test coverage detected