const btVector3 rotate( const btQuaternion& quat, const btVector3 & vec )
{
float tmpX, tmpY, tmpZ, tmpW;
tmpX = ( ( ( quat.getW() * vec.getX() ) + ( quat.getY() * vec.getZ() ) ) - ( quat.getZ() * vec.getY() ) );
tmpY = ( ( ( quat.getW() * vec.getY() ) + ( quat.getZ() * vec.getX() ) ) - ( quat.getX() * vec.getZ() ) );
tmpZ = ( ( ( quat.getW() * vec.getZ() ) + ( quat.getX() * vec.getY() ) ) - ( quat.getY() * vec.getX() ) );
tmpW = ( ( ( quat.getX() * vec.getX() ) + ( quat.getY() * vec.getY() ) ) + ( quat.getZ() * vec.getZ() ) );
return btVector3(
( ( ( ( tmpW * quat.getX() ) + ( tmpX * quat.getW() ) ) - ( tmpY * quat.getZ() ) ) + ( tmpZ * quat.getY() ) ),
( ( ( ( tmpW * quat.getY() ) + ( tmpY * quat.getW() ) ) - ( tmpZ * quat.getX() ) ) + ( tmpX * quat.getZ() ) ),
( ( ( ( tmpW * quat.getZ() ) + ( tmpZ * quat.getW() ) ) - ( tmpX * quat.getY() ) ) + ( tmpY * quat.getX() ) )
);
}
static inline void copy_quat_btquat(float quat[4], const btQuaternion &btquat)
{
quat[0] = btquat.getW();
quat[1] = btquat.getX();
quat[2] = btquat.getY();
quat[3] = btquat.getZ();
}
void btQuaternion_to_Quaternion(JNIEnv * const &jenv, jobject &target, const btQuaternion & source)
{
quaternion_ensurefields(jenv, target);
jenv->SetFloatField(target, quaternion_x, source.getX());
jenv->SetFloatField(target, quaternion_y, source.getY());
jenv->SetFloatField(target, quaternion_z, source.getZ());
jenv->SetFloatField(target, quaternion_w, source.getW());
}
Quaternion::Quaternion(const btQuaternion &aBulletQuaternion)
{
x = aBulletQuaternion.getX();
y = aBulletQuaternion.getY();
z = aBulletQuaternion.getZ();
w = aBulletQuaternion.getW();
}
// Taken from Irrlicht page
btVector3 quatToEuler(const btQuaternion & quat)
{
btVector3 ret;
btScalar w = quat.getW(), x = quat.getX(), y = quat.getY(), z = quat.getZ();
float ws = w*w, xs = x*x, ys = y*y, zs = z*z;
ret.setX(atan2f(2.0f*(y*z+x*w), -xs-ys+zs+ws));
ret.setY(asinf(-2.0f*(x*z-y*w)));
ret.setZ(atan2f(2.0f*(x*y+z*w), xs-ys-zs+ws));
ret *= irr::core::RADTODEG;
return ret;
}
void Physics::QuaternionToEuler( const btQuaternion &TQuat, btVector3 &TEuler )
{
float a[3];
const float w = TQuat.getW();
const float x = TQuat.getX();
const float y = TQuat.getY();
const float z = TQuat.getZ();
QuaternionToEuler( w, x, y, z, a );
TEuler.setX( a[0] );
TEuler.setY( a[1] );
TEuler.setZ( a[2] );
}
void Vec3::setHPR(const btQuaternion& q)
{
float W = q.getW();
float X = q.getX();
float Y = q.getY();
float Z = q.getZ();
float WSquared = W * W;
float XSquared = X * X;
float YSquared = Y * Y;
float ZSquared = Z * Z;
setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
setY(asinf(-2.0f * (X * Z - Y * W)));
setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
} // setHPR(btQuaternion)
// Converts a quaternion to an euler angle
void _Physics::QuaternionToEuler(const btQuaternion &Quat, btVector3 &Euler) {
btScalar W = Quat.getW();
btScalar X = Quat.getX();
btScalar Y = Quat.getY();
btScalar Z = Quat.getZ();
float WSquared = W * W;
float XSquared = X * X;
float YSquared = Y * Y;
float ZSquared = Z * Z;
Euler.setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
Euler.setY(asinf(-2.0f * (X * Z - Y * W)));
Euler.setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
Euler *= irr::core::RADTODEG;
}
// Converts a quaternion to an euler angle
void QuaternionToEuler(const btQuaternion &tQuat, btVector3 &tEuler) {
btScalar w = tQuat.getW();
btScalar x = tQuat.getX();
btScalar y = tQuat.getY();
btScalar z = tQuat.getZ();
float wSquared = w * w;
float xSquared = x * x;
float ySquared = y * y;
float zSquared = z * z;
tEuler.setX(atan2f(2.0f * (y * z + x * w), -xSquared - ySquared + zSquared + wSquared));
tEuler.setY(asinf(-2.0f * (x * z - y * w)));
tEuler.setZ(atan2f(2.0f * (x * y + z * w), xSquared - ySquared - zSquared + wSquared));
tEuler *= SIMD_DEGS_PER_RAD;
}
// Converts a quaternion to an euler angle
void CTBulletHelper::QuaternionToEuler(const btQuaternion &TQuat, btVector3 &TEuler) {
btScalar W = TQuat.getW();
btScalar X = TQuat.getX();
btScalar Y = TQuat.getY();
btScalar Z = TQuat.getZ();
float WSquared = W * W;
float XSquared = X * X;
float YSquared = Y * Y;
float ZSquared = Z * Z;
TEuler.setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
TEuler.setY(asinf(-2.0f * (X * Z - Y * W)));
TEuler.setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
TEuler *= core::RADTODEG;
}
void Physics::QuaternionToEuler( const btQuaternion &TQuat, CIwFVec3 &TEuler )
{
float a[3];
const float w = TQuat.getW();
const float x = TQuat.getX();
const float y = TQuat.getY();
const float z = TQuat.getZ();
QuaternionToEuler( w, x, y, z, a );
// heading
TEuler.z = a[2];
// bank
TEuler.x = a[0];
// attitude
TEuler.y = a[1];
}
// Converts a quaternion to an euler angle
core::vector3df quat_to_euler(const btQuaternion &TQuat) {
btScalar W = TQuat.getW();
btScalar X = TQuat.getX();
btScalar Y = TQuat.getY();
btScalar Z = TQuat.getZ();
float WSquared = W * W;
float XSquared = X * X;
float YSquared = Y * Y;
float ZSquared = Z * Z;
float x = (atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
float y = (asinf(-2.0f * (X * Z - Y * W)));
float z = (atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
core::vector3df euler = core::vector3df(x, y, z);
euler *= core::RADTODEG;
return euler;
}
Eigen::Quaternion<float> FromBullet(const btQuaternion& q) { return Eigen::Quaternion<float>(q.getW(), q.getX(), q.getY(), q.getZ()); }
void addQuaternion(const btQuaternion& q) { addFloat(q.getX());
addFloat(q.getY());
addFloat(q.getZ());
addFloat(q.getW()); }
Quatf fromBulletQuaternion( const btQuaternion &quat )
{
return ci::Quatf( quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
}
static ERL_NIF_TERM quaternion_to_tuple(ErlNifEnv* env, btQuaternion v) {
return enif_make_tuple4(env, enif_make_double(env, v.getX()), enif_make_double(env, v.getY()), enif_make_double(env,v.getZ()), enif_make_double(env,v.getW()));
}
inline Quaternion toAvQuat(const btQuaternion &quat)
{
return Quaternion(quat.getX(), quat.getY(), quat.getZ(), quat.getW());
}
quaternion(const btQuaternion& quad)
: Ogre::Quaternion(quad.getW(), quad.getX(), quad.getY(), quad.getZ())
{;}
quaternion::quaternion(const btQuaternion &quat)
: w(quat.getW())
, x(quat.getX())
, y(quat.getY())
, z(quat.getZ())
{;}
glm::quat convertBulletQuaternionToGLM(btQuaternion quaternion) {
return glm::quat(quaternion.getW(), quaternion.getX(), quaternion.getY(), quaternion.getZ());
}
// -------------------------------------------------------------------------
void Object::setTransform(const btVector3 &pos, const btQuaternion &quat)
{
mRootNode->setPosition(pos[0], pos[1], pos[2]);
mRootNode->setOrientation(quat.getW(),quat.getX(), quat.getY(), quat.getZ());
}
glm::quat toQuat(const btQuaternion& q)
{
return glm::quat(q.getW(), q.getX(), q.getY(), q.getZ());
}
Quaternion toOgreQuaternion(const btQuaternion& quat)
{
return Quaternion(quat.getW(), quat.getX(), quat.getY(), quat.getZ());
}
void ConvertRotationToHL(const btQuaternion& quat, QAngle& hl)
{
Quaternion q(quat.getX(), -quat.getZ(), quat.getY(), quat.getW());
RadianEuler radian(q);
hl = radian.ToQAngle();
}
// -------------------------------------------------------------------------
void Object::setOrientation(const btQuaternion &quat)
{
mRootNode->setOrientation(quat.getW(),quat.getX(), quat.getY(), quat.getZ());
}