Esempio n. 1
0
 private void buildLinearJacobian(ref btVector3 normalWorld,ref btVector3 pivotAInW, ref btVector3 pivotBInW,out JacobianEntry result )
 {
     /*return new JacobianEntry(
         m_rbA.CenterOfMassTransform.Basis.transpose(),
         m_rbB.CenterOfMassTransform.Basis.transpose(),
         pivotAInW - m_rbA.CenterOfMassPosition,
         pivotBInW - m_rbB.CenterOfMassPosition,
         normalWorld,
         m_rbA.InvInertiaDiagLocal,
         m_rbA.InvMass,
         m_rbB.InvInertiaDiagLocal,
         m_rbB.InvMass);*/
     btMatrix3x3 temp1, temp2;
     btVector3 temp3, temp4, temp5 = m_rbA.CenterOfMassPosition, temp6 = m_rbB.CenterOfMassPosition;
     m_rbA.CenterOfMassTransform.Basis.transpose(out temp1);
     m_rbB.CenterOfMassTransform.Basis.transpose(out temp2);
     btVector3.Subtract(ref pivotAInW, ref temp5, out temp3);
     btVector3.Subtract(ref pivotBInW, ref temp6, out temp4);
     result= new JacobianEntry(
         ref temp1,
         ref temp2,
         ref temp3,
         ref temp4,
         ref normalWorld,
         ref m_rbA.InvInertiaDiagLocal,
         m_rbA.InvMass,
         ref m_rbB.InvInertiaDiagLocal,
         m_rbB.InvMass);
 }
Esempio n. 2
0
 private void buildAngularJacobian(ref btVector3 jointAxisW,out JacobianEntry result)
 {
     /*return new JacobianEntry(jointAxisW,
                               m_rbA.CenterOfMassTransform.Basis.transpose(),
                               m_rbB.CenterOfMassTransform.Basis.transpose(),
                               m_rbA.InvInertiaDiagLocal,
                               m_rbB.InvInertiaDiagLocal);*/
     btMatrix3x3 temp1, temp2;
     m_rbA.CenterOfMassTransform.Basis.transpose(out temp1);
     m_rbB.CenterOfMassTransform.Basis.transpose(out temp2);
     result= new JacobianEntry(ref jointAxisW,
                               ref temp1,
                               ref temp2,
                               ref m_rbA.InvInertiaDiagLocal,
                               ref m_rbB.InvInertiaDiagLocal);
 }