static void M3x3getRot_ref(const btMatrix3x3 &m, btQuaternion &q)
{
btVector3 m_el[3] = {m[0], m[1], m[2]};
btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
btScalar temp[4];
if (trace > btScalar(0.0))
{
btScalar s = btSqrt(trace + btScalar(1.0));
temp[3] = (s * btScalar(0.5));
s = btScalar(0.5) / s;
temp[0] = ((m_el[2].y() - m_el[1].z()) * s);
temp[1] = ((m_el[0].z() - m_el[2].x()) * s);
temp[2] = ((m_el[1].x() - m_el[0].y()) * s);
}
else
{
int i = m_el[0].x() < m_el[1].y() ? (m_el[1].y() < m_el[2].z() ? 2 : 1) : (m_el[0].x() < m_el[2].z() ? 2 : 0);
int j = (i + 1) % 3;
int k = (i + 2) % 3;
btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
temp[i] = s * btScalar(0.5);
s = btScalar(0.5) / s;
temp[3] = (m_el[k][j] - m_el[j][k]) * s;
temp[j] = (m_el[j][i] + m_el[i][j]) * s;
temp[k] = (m_el[k][i] + m_el[i][k]) * s;
}
q.setValue(temp[0], temp[1], temp[2], temp[3]);
}
void Quaternion_to_btQuaternion(JNIEnv * const &jenv, btQuaternion &target, jobject &source)
{
quaternion_ensurefields(jenv, source);
target.setValue(
jenv->GetFloatField(source, quaternion_x),
jenv->GetFloatField(source, quaternion_y),
jenv->GetFloatField(source, quaternion_z),
jenv->GetFloatField(source, quaternion_w));
}
bool parseQuaternion(const Value &val, btQuaternion &retQuat) {
if (!(val.isArray() && val.size() == 4)) return false;
if (!val[0].isConvertibleTo(ValueType::realValue)) return false;
if (!val[1].isConvertibleTo(ValueType::realValue)) return false;
if (!val[2].isConvertibleTo(ValueType::realValue)) return false;
if (!val[3].isConvertibleTo(ValueType::realValue)) return false;
retQuat.setValue(val[0].asDouble(), val[1].asDouble(),
val[2].asDouble(), val[3].asDouble());
return true;
}
void ConvertRotationToBull(const QAngle& angles, btQuaternion& bull) {
RadianEuler radian(angles);
Quaternion q(radian);
bull.setValue(q.x, q.z, -q.y, q.w);
}