| 842 | } |
| 843 | |
| 844 | bool CPHElement::AnimToVel(float dt, float l_limit, float a_limit) |
| 845 | { |
| 846 | VERIFY(m_shell); |
| 847 | VERIFY(m_shell->PKinematics()); |
| 848 | // CBoneInstance *BI = &m_shell->PKinematics()->LL_GetBoneInstance(m_SelfID); |
| 849 | // |
| 850 | // Fmatrix bp;BoneGlPos(bp,BI); |
| 851 | // |
| 852 | CPhysicsShellHolder* ph = PhysicsRefObject(); |
| 853 | VERIFY(ph); |
| 854 | Fmatrix bpl; |
| 855 | GetAnimBonePos(bpl); |
| 856 | Fmatrix bp; |
| 857 | bp.mul_43(ph->XFORM(), bpl); |
| 858 | // BoneGlPos(bp,BI); |
| 859 | |
| 860 | Fmatrix cp; |
| 861 | GetGlobalTransformDynamic(&cp); |
| 862 | |
| 863 | // Fquaternion q0; q0.set(cp); |
| 864 | |
| 865 | cp.invert(); |
| 866 | Fmatrix diff; |
| 867 | diff.mul_43(cp, bp); |
| 868 | if (dt < EPS_S) |
| 869 | dt = EPS_S; |
| 870 | Fvector mc1; |
| 871 | CPHGeometryOwner::get_mc_vs_transform(mc1, bp); |
| 872 | Fvector mc0 = cast_fv(dBodyGetPosition(m_body)); |
| 873 | // Fvector mc1;diff.transform_tiny(mc1,mc0); |
| 874 | Fvector lv; |
| 875 | lv.mul(Fvector().sub(mc1, mc0), (1.f / dt)); |
| 876 | Fvector aw; |
| 877 | aw.set((diff._32 - diff._23) / 2.f / dt, (diff._13 - diff._31) / 2.f / dt, (diff._21 - diff._12) / 2.f / dt); |
| 878 | |
| 879 | // Fquaternion q1; q1.set(bp); |
| 880 | // twoq_2w(q0,q1,dt,aw); |
| 881 | |
| 882 | bool ret = aw.square_magnitude() < a_limit * a_limit && lv.square_magnitude() < l_limit * l_limit; |
| 883 | |
| 884 | put_in_range(lv, m_l_limit); |
| 885 | put_in_range(aw, m_w_limit); |
| 886 | |
| 887 | VERIFY(_valid(lv)); |
| 888 | VERIFY(_valid(aw)); |
| 889 | |
| 890 | dBodySetLinearVel(m_body, lv.x, lv.y, lv.z); |
| 891 | dBodySetAngularVel(m_body, aw.x, aw.y, aw.z); |
| 892 | // set_LinearVel(lv); |
| 893 | // set_AngularVel(aw); |
| 894 | return ret; |
| 895 | } |
| 896 | |
| 897 | void CPHElement::ToBonePos(CBoneInstance* B) |
| 898 | { |
no test coverage detected