(self, T: ca.MX, R: ca.MX)
| 641 | return (v.X(), v.Y(), v.Z()), (a, b, c) |
| 642 | |
| 643 | def _build_transform(self, T: ca.MX, R: ca.MX) -> gp_Trsf: |
| 644 | |
| 645 | opti = self.opti |
| 646 | |
| 647 | rv = gp_Trsf() |
| 648 | |
| 649 | a, b, c = opti.value(R) |
| 650 | m = a ** 2 + b ** 2 + c ** 2 |
| 651 | |
| 652 | rv.SetRotation( |
| 653 | gp_Quaternion( |
| 654 | 2 * a / (m + 1), 2 * b / (m + 1), 2 * c / (m + 1), (1 - m) / (m + 1), |
| 655 | ) |
| 656 | ) |
| 657 | rv.SetTranslationPart(gp_Vec(*opti.value(T))) |
| 658 | |
| 659 | return rv |
| 660 | |
| 661 | def solve(self, verbosity: int = 0) -> Tuple[List[Location], Dict[str, Any]]: |
| 662 |