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);
}
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);
}