| 490 | |
| 491 | |
| 492 | void NpArticulationLink::visualizeJoint(PxConstraintVisualizer& jointViz) |
| 493 | { |
| 494 | NpArticulationLink* parent = getParent(); |
| 495 | if(parent) |
| 496 | { |
| 497 | PxTransform cA2w = getGlobalPose().transform(mInboundJoint->getChildPose()); |
| 498 | PxTransform cB2w = parent->getGlobalPose().transform(mInboundJoint->getParentPose()); |
| 499 | |
| 500 | jointViz.visualizeJointFrames(cA2w, cB2w); |
| 501 | |
| 502 | PxArticulationJointImpl* impl = mInboundJoint->getImpl(); |
| 503 | |
| 504 | if (impl->mType == PxArticulationBase::eMaximumCoordinate) |
| 505 | { |
| 506 | |
| 507 | PxTransform parentFrame = cB2w; |
| 508 | |
| 509 | if (cA2w.q.dot(cB2w.q) < 0) |
| 510 | cB2w.q = -cB2w.q; |
| 511 | |
| 512 | PxTransform cB2cA = cA2w.transformInv(cB2w); |
| 513 | |
| 514 | PxQuat swing, twist; |
| 515 | Ps::separateSwingTwist(cB2cA.q, swing, twist); |
| 516 | |
| 517 | PxMat33 cA2w_m(cA2w.q), cB2w_m(cB2w.q); |
| 518 | |
| 519 | PxReal tqPhi = Ps::tanHalf(twist.x, twist.w); // always support (-pi, +pi) |
| 520 | |
| 521 | PxReal lower, upper, yLimit, zLimit; |
| 522 | |
| 523 | impl->getScbArticulationJoint().getTwistLimit(lower, upper); |
| 524 | impl->getScbArticulationJoint().getSwingLimit(yLimit, zLimit); |
| 525 | PxReal swingPad = impl->getScbArticulationJoint().getSwingLimitContactDistance(), twistPad = impl->getScbArticulationJoint().getTwistLimitContactDistance(); |
| 526 | jointViz.visualizeAngularLimit(parentFrame, lower, upper, PxAbs(tqPhi) > PxTan(upper - twistPad)); |
| 527 | |
| 528 | PxVec3 tanQSwing = PxVec3(0, Ps::tanHalf(swing.z, swing.w), -Ps::tanHalf(swing.y, swing.w)); |
| 529 | Cm::ConeLimitHelper coneHelper(PxTan(yLimit / 4), PxTan(zLimit / 4), PxTan(swingPad / 4)); |
| 530 | jointViz.visualizeLimitCone(parentFrame, PxTan(yLimit / 4), PxTan(zLimit / 4), !coneHelper.contains(tanQSwing)); |
| 531 | } |
| 532 | else |
| 533 | { |
| 534 | //(1) visualize any angular dofs/limits... |
| 535 | |
| 536 | const PxMat33 cA2w_m(cA2w.q), cB2w_m(cB2w.q); |
| 537 | |
| 538 | PxTransform parentFrame = cB2w; |
| 539 | |
| 540 | if (cA2w.q.dot(cB2w.q) < 0) |
| 541 | cB2w.q = -cB2w.q; |
| 542 | |
| 543 | //const PxTransform cB2cA = cA2w.transformInv(cB2w); |
| 544 | |
| 545 | const PxTransform cA2cB = cB2w.transformInv(cA2w); |
| 546 | |
| 547 | Sc::ArticulationJointCore& joint = impl->getScbArticulationJoint().getScArticulationJoint(); |
| 548 | |
| 549 | PxQuat swing1, swing2, twist; |
no test coverage detected