| 2753 | |
| 2754 | |
| 2755 | extern "C" void dJointSetFixed (dxJointFixed *joint) |
| 2756 | { |
| 2757 | dUASSERT(joint,"bad joint argument"); |
| 2758 | dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed"); |
| 2759 | int i; |
| 2760 | |
| 2761 | // This code is taken from sJointSetSliderAxis(), we should really put the |
| 2762 | // common code in its own function. |
| 2763 | // compute the offset between the bodies |
| 2764 | if (joint->node[0].body) { |
| 2765 | if (joint->node[1].body) { |
| 2766 | dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); |
| 2767 | dReal ofs[4]; |
| 2768 | for (i=0; i<4; i++) ofs[i] = joint->node[0].body->pos[i]; |
| 2769 | for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->pos[i]; |
| 2770 | dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs); |
| 2771 | } |
| 2772 | else { |
| 2773 | // set joint->qrel to the transpose of the first body's q |
| 2774 | joint->qrel[0] = joint->node[0].body->q[0]; |
| 2775 | for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; |
| 2776 | for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->pos[i]; |
| 2777 | } |
| 2778 | } |
| 2779 | } |
| 2780 | |
| 2781 | extern "C" void dJointSetFixedQuaternionPos (dxJointFixed *joint,dQuaternion quaternion,dReal* pos) |
| 2782 | { |
no test coverage detected