Function getTorsoTransform

Rendering/VRModels/vtkOpenGLAvatar.cxx:52–97

calculate a rotation purely around Vup, using an approximate Vr (right) that isn't orthogonal. Reverse Vr, front if torso isn't facing the same way as the head.

// that isn't orthogonal.
// Reverse Vr, front if torso isn't facing the same way as the head.
void getTorsoTransform(
  vtkTransform* trans, double Vup[3], double inVr[3], double HeadOrientation[3])
{
  double Vr[3] = { inVr[0], inVr[1], inVr[2] };
  // temporarily use trans for head orientation
  setOrientation(trans, HeadOrientation);
  if (Vr[0] == 0 && Vr[1] == 0 && Vr[2] == 0)
  {
    // no information from hands, use head orientation and Vup.
    Vr[2] = 1;
    trans->TransformPoint(Vr, Vr);
  }

  // make Vr orthogonal to Vup
  double Vtemp[3] = { Vup[0], Vup[1], Vup[2] };
  vtkMath::MultiplyScalar(Vtemp, vtkMath::Dot(Vup, Vr));
  vtkMath::Subtract(Vr, Vtemp, Vr);
  vtkMath::Normalize(Vr);
  // get third basis vector
  double Vfr[3];
  vtkMath::Cross(Vup, Vr, Vfr);
  // temporarily use trans to test Vfr versus head orientation
  double Vhead[3] = { 1, 0, 0 };
  trans->TransformPoint(Vhead, Vhead);
  if (vtkMath::Dot(Vfr, Vhead) < 0)
  {
    // torso is facing behind the head. Swap.
    Vr[0] = -Vr[0];
    Vr[1] = -Vr[1];
    Vr[2] = -Vr[2];
    Vfr[0] = -Vfr[0];
    Vfr[1] = -Vfr[1];
    Vfr[2] = -Vfr[2];
  }
  // make new rotation matrix. Basis vectors form the rotation piece.
  trans->Identity();
  vtkNew<vtkMatrix4x4> mat;
  trans->GetMatrix(mat);
  for (int i = 0; i < 3; ++i)
  {
    mat->SetElement(i, 0, Vfr[i]);
    mat->SetElement(i, 1, Vup[i]);
    mat->SetElement(i, 2, Vr[i]);
  }
  trans->SetMatrix(mat);
}

// create a triangle between the shoulder and hand, in the plane
// parallel to the up vector, so elbow is always "down".