| 871 | } |
| 872 | |
| 873 | static PxU32 D6JointSolverPrep(Px1DConstraint* constraints, |
| 874 | PxVec3& body0WorldOffset, |
| 875 | PxU32 /*maxConstraints*/, |
| 876 | PxConstraintInvMassScale& invMassScale, |
| 877 | const void* constantBlock, |
| 878 | const PxTransform& bA2w, |
| 879 | const PxTransform& bB2w, |
| 880 | bool useExtendedLimits, |
| 881 | PxVec3& cA2wOut, PxVec3& cB2wOut) |
| 882 | { |
| 883 | const D6JointData& data = *reinterpret_cast<const D6JointData*>(constantBlock); |
| 884 | |
| 885 | PxTransform cA2w, cB2w; |
| 886 | joint::ConstraintHelper ch(constraints, invMassScale, cA2w, cB2w, body0WorldOffset, data, bA2w, bB2w); |
| 887 | |
| 888 | const PxU32 SWING1_FLAG = 1<<PxD6Axis::eSWING1; |
| 889 | const PxU32 SWING2_FLAG = 1<<PxD6Axis::eSWING2; |
| 890 | const PxU32 TWIST_FLAG = 1<<PxD6Axis::eTWIST; |
| 891 | |
| 892 | const PxU32 ANGULAR_MASK = SWING1_FLAG | SWING2_FLAG | TWIST_FLAG; |
| 893 | const PxU32 LINEAR_MASK = 1<<PxD6Axis::eX | 1<<PxD6Axis::eY | 1<<PxD6Axis::eZ; |
| 894 | |
| 895 | const PxD6JointDrive* drives = data.drive; |
| 896 | PxU32 locked = data.locked; |
| 897 | const PxU32 limited = data.limited; |
| 898 | const PxU32 driving = data.driving; |
| 899 | |
| 900 | // PT: it is a mistake to use the neighborhood operator since it |
| 901 | // prevents us from using the quat's double-cover feature. |
| 902 | if(!useExtendedLimits && cA2w.q.dot(cB2w.q)<0.0f) // minimum dist quat (equiv to flipping cB2bB.q, which we don't use anywhere) |
| 903 | cB2w.q = -cB2w.q; |
| 904 | |
| 905 | const PxTransform cB2cA = cA2w.transformInv(cB2w); |
| 906 | |
| 907 | PX_ASSERT(data.c2b[0].isValid()); |
| 908 | PX_ASSERT(data.c2b[1].isValid()); |
| 909 | PX_ASSERT(cA2w.isValid()); |
| 910 | PX_ASSERT(cB2w.isValid()); |
| 911 | PX_ASSERT(cB2cA.isValid()); |
| 912 | |
| 913 | const PxMat33 cA2w_m(cA2w.q); |
| 914 | const PxMat33 cB2w_m(cB2w.q); |
| 915 | |
| 916 | // handy for swing computation |
| 917 | const PxVec3& bX = cB2w_m.column0; |
| 918 | const PxVec3& aY = cA2w_m.column1; |
| 919 | const PxVec3& aZ = cA2w_m.column2; |
| 920 | |
| 921 | if(driving & ((1<<PxD6Drive::eX)|(1<<PxD6Drive::eY)|(1<<PxD6Drive::eZ))) |
| 922 | { |
| 923 | // TODO: make drive unilateral if we are outside the limit |
| 924 | const PxVec3 posErr = data.drivePosition.p - cB2cA.p; |
| 925 | for(PxU32 i=0; i<3; i++) |
| 926 | { |
| 927 | // -driveVelocity because velTarget is child (body1) - parent (body0) and Jacobian is 1 for body0 and -1 for parent |
| 928 | if(driving & (1<<(PxD6Drive::eX+i))) |
| 929 | ch.linear(cA2w_m[i], -data.driveLinearVelocity[i], posErr[i], drives[PxD6Drive::eX+i]); |
| 930 | } |
nothing calls this directly
no test coverage detected