Пример #1
0
void
EditorIkSolver::_checkPivotXZ(ArRef<Joint> joint, Vector3d /* requestedTranslation */, Quaterniond requestedRotation) {
  double prx, pry, prz;
  joint->getDeltaRotation().getEulerAngles(prx, pry, prz);

  requestedRotation = joint->getDeltaRotation() * requestedRotation;

  double rx, ry, rz;
  requestedRotation.getEulerAngles(rx, ry, rz);

  double* max[2];
  double* min[2];

  ar_down_cast<JointConstraint2DOF>(joint->getConstraint())->getLimitValues(min, max);

  double xmin = *min[0], xmax = *max[0], zmin = *min[1], zmax = *max[1];

  rx = _clampWithPreviousValue(xmin, xmax, prx, rx);
  rz = _clampWithPreviousValue(zmin, zmax, prz, rz);

  Quaterniond qx(Vector3d(1.0, 0.0, 0.0), rx);
  Quaterniond qz(Vector3d(0.0, 0.0, 1.0), rz);

  joint->setDeltaRotation(qx * qz);
}
Пример #2
0
void
EditorIkSolver::_checkHingeZ(ArRef<Joint> joint, Vector3d /* requestedTranslation */, Quaterniond requestedRotation) {
  double prx, pry, prz;
  joint->getDeltaRotation().getEulerAngles(prx, pry, prz);

  requestedRotation = joint->getDeltaRotation() * requestedRotation;

  double rx, ry, rz;
  requestedRotation.getEulerAngles(rx, ry, rz);

  double min, max;
  ar_down_cast<JointConstraint1DOF>(joint->getConstraint())->getLimitValues(min, max);

  rz = _clampWithPreviousValue(min, max, prz, rz);

  joint->setDeltaRotation(Quaterniond(Vector3d(0.0, 0.0, 1.0), rz));
}