//q1 and q2 must always have values within [0,2*PI]
//strong assumption here that q1 and q2 are not displaced by much from each other
double getQuaternionDiffShortestRad(btQuaternion q1, btQuaternion q2)
{
double th1 = q1.getAngle();
double th2 = q2.getAngle();
double diffval = (th1-th2);
///double diffval1 = (th1-th2);
/*
cout << "th1: " << (th1/PI*180.0) << "\n";
cout << "th2: " << (th2/PI*180.0) << "\n";
*/
if((diffval<(-PI))&&((th1<=(PI/2.0))&&(th1>=0.0))&&((th2>=(3*PI/2.0))&&(th2<=(2*PI))))
{
diffval = (diffval+2*PI);
}
else if((diffval>PI)&&((th2<=(PI/2.0))&&(th2>=0.0))&&((th1>=(3*PI/2.0))&&(th1<=(2*PI))))
{
diffval = (diffval-2*PI);
}
///cout << "2.diffval: " << (diffval/PI*180.0) << "\n";
/*
if(diffval<(-PI))
diffval=(diffval+2*PI);
else if(diffval>PI)
diffval=(diffval-2*PI);
*/
///cout << "diffval: " << (diffval/PI*180.0) << "\n";
return diffval;
}
//q1 and q2 must always have values within [0,2*PI]
btQuaternion getQuaternionDiff(btQuaternion q1, btQuaternion q2)
{
double th1 = q1.getAngle();
double th2 = q2.getAngle();
double diffval = (th1-th2);
/*
if((diffval<(-PI))&&((th1<(PI/2.0))&&(th1>0.0))&&((th2>(3*PI/2.0))&&(th2<(2*PI))))
{
diffval = (diffval+2*PI);
}
else if((diffval>PI)&&((th2<(PI/2.0))&&(th2>0.0))&&((th1>(3*PI/2.0))&&(th1<(2*PI))))
{
diffval = (diffval-2*PI);
}
*/
if(diffval<0)
diffval=(diffval+2*PI);
btQuaternion diffq;
diffq.setRPY(0.0,0.0,diffval);
return diffq;
}
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();
}
// convert quaternion
math::quat convert( const btQuaternion& q )
{
return math::quat(
q.x(),
q.y(),
q.z(),
q.w() );
}
btVector3 duCharacter::QmV3(const btVector3 & v, const btQuaternion & q)
{
// transform Vector3 with a given quaternion
btVector3 qv(q.x(), q.y(), q.z());
btVector3 uv = qv.cross(v);
btVector3 uuv = qv.cross(uv);
uv *= (2.0 * q.w());
uuv *= 2.0;
return v + uv + uuv;
}
// given a cone rotation in constraint space, (pre: twist must already be removed)
// this method computes its corresponding swing angle and axis.
// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
btScalar& swingAngle, // out
btVector3& vSwingAxis, // out
btScalar& swingLimit) // out
{
swingAngle = qCone.getAngle();
if (swingAngle > SIMD_EPSILON)
{
vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
vSwingAxis.normalize();
#if 0
// non-zero twist?! this should never happen.
btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
#endif
// Compute limit for given swing. tricky:
// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
// For starters, compute the direction from center to surface of ellipse.
// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
btScalar xEllipse = vSwingAxis.y();
btScalar yEllipse = -vSwingAxis.z();
// Now, we use the slope of the vector (using x/yEllipse) and find the length
// of the line that intersects the ellipse:
// x^2 y^2
// --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
// a^2 b^2
// Do the math and it should be clear.
swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
if (fabs(xEllipse) > SIMD_EPSILON)
{
btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
swingLimit = sqrt(swingLimit2);
}
// test!
/*swingLimit = m_swingSpan2;
if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
{
btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
btScalar phi = asin(sinphi);
btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
btScalar alpha = 3.14159f - theta - phi;
btScalar sinalpha = sin(alpha);
swingLimit = m_swingSpan1 * sinphi/sinalpha;
}*/
}
// 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 BulletDynamicsBody::GetTransform( Vector3& position, Quaternion& rotation )
{
// get updated parameters
const btTransform& transform( GetBody()->getWorldTransform() );
const btVector3& origin( transform.getOrigin() );
const btQuaternion currentRotation( transform.getRotation() );
const btVector3& axis( currentRotation.getAxis() );
const btScalar& angle( currentRotation.getAngle() );
position = Vector3( origin.x(), origin.y(), origin.z() );
rotation = Quaternion( float(angle), Vector3( axis.x(), axis.y(), axis.z() ) );
}
static inline btScalar qtdot_ref(btQuaternion& q1, btQuaternion& q2)
{
return q1.x() * q2.x() +
q1.y() * q2.y() +
q1.z() * q2.z() +
q1.w() * q2.w();
}
static bool IsEqual(const btQuaternion &pt0, const btQuaternion &pt1) {
float delta = fabs(pt0.x() - pt1.x());
delta += fabs(pt0.y() - pt1.y());
delta += fabs(pt0.z() - pt1.z());
delta += fabs(pt0.w() - pt1.w());
return delta < 1e-8f;
}
// 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;
}
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)
geometry_msgs::TransformStamped createTransform(btQuaternion q, btVector3 v, ros::Time stamp, const std::string& frame1, const std::string& frame2)
{
geometry_msgs::TransformStamped t;
t.header.frame_id = frame1;
t.child_frame_id = frame2;
t.header.stamp = stamp;
t.transform.translation.x = v.x();
t.transform.translation.y = v.y();
t.transform.translation.z = v.z();
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();
return t;
}
// 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, 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] );
}
// 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;
}
//works only if the quaternion represents a pure yaw rotation (yaw axis is the z axis)
double getQuaternionAngleDeg(btQuaternion qtarg)
{
double th = qtarg.getAngle();
if(fabs(th)>(2*PI))
{
cout << "Bad value returned by qtarg.getAngle, th: " << th << "\n";
return -9999;
}
/*
if((fabs(qtarg.z())>(1e-6))&&(fabs(qtarg.w())>(1e-6)))
{
if((qtarg.z()/qtarg.w())<0)
{
//the angle is in [-PI,0]
//converting it to [0,2*PI]
th=(th+2*PI);
}
}
*/
/*
if((qtarg.z()<0)||(qtarg.w()<0))
{
//the angle is in [-PI,0]
//converting it to [0,2*PI]
th=(th+2*PI);
}
*/
double dval = (th/PI*180.0);
return dval;
}
/* PMDBone::update: update internal transform for current position / rotation */
void PMDBone::update()
{
btQuaternion r;
const btQuaternion norot(0.0f, 0.0f, 0.0f, 1.0f);
m_trans.setOrigin(m_pos + m_offset);
if (m_type == FOLLOW_ROTATE) {
/* for co-rotate bone, further apply the rotation of child bone scaled by the rotation weight */
r = m_rot * norot.slerp(m_childBone->m_rot, m_rotateCoef);
m_trans.setRotation(r);
} else {
m_trans.setRotation(m_rot);
}
if (m_parentBone)
m_trans = m_parentBone->m_trans * m_trans;
}
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 DynamicsMotionState::setWorldTransform(const btTransform& transform)
{
// DALI_LOG_INFO(Debug::Filter::gDynamics, Debug::Verbose, "%s\n", __PRETTY_FUNCTION__);
// get updated parameters
const btVector3& origin( transform.getOrigin() );
const btQuaternion rotation( transform.getRotation() );
const btVector3& axis( rotation.getAxis() );
const btScalar& angle( rotation.getAngle() );
Vector3 newPosition( origin.x(), origin.y(), origin.z() );
const Vector3 newAxis( axis.x(), axis.y(), axis.z() );
Quaternion newRotation( float(angle), newAxis );
// set the nodes updated params
mDynamicsBody.SetNodePositionAndRotation( newPosition, newRotation );
}
static int operator!= ( const btQuaternion &a, const btQuaternion &b )
{
if( fabs(a.x() - b.x()) +
fabs(a.y() - b.y()) +
fabs(a.z() - b.z()) +
fabs(a.w() - b.w()) > FLT_EPSILON * 4)
return 1;
return 0;
}
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));
}
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;
}
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 GameObjectStaticManager::CreateEntity(char * path, PhysicsEngine* engine, btQuaternion& orientation,
btVector3& position, btScalar mass)
{
std::string pathStr(path);
string base = pathStr.erase(pathStr.find_last_of('/'));
base.push_back('/');
// Set object position
glm::mat4x4 modelMatrix = mat4_cast(quat(orientation.w(), orientation.x(), orientation.y(), orientation.z()));
modelMatrix = glm::translate(modelMatrix, glm::vec3(position.x(), position.y(), position.z()));
// Get the mode pair
std::pair<int, Model*>modelPair = resource::ModelManager::GetSingleton()->GetModel(modelMatrix, path);
// If the first entry of the pair is -1 then the model has not yet been loaded in.
if (modelPair.first == -1)
{
// Load the obj files
std::vector<tinyobj::shape_t> shapes;
std::vector<tinyobj::material_t> materials;
tinyobj::LoadObj(shapes, materials, path, base.c_str());
StaticPhysicsObject* physStart = engine->CreateStaticObject(shapes, orientation, position,
btVector3(1, 1, 1), mass, path);
Model* modelStart = resource::ModelManager::GetSingleton()->CreateModels(modelMatrix, shapes, materials, base.c_str(), path);
new (&m_gameEntity[m_activeEntitys]) GameEntity(m_activeEntitys, shapes.size(), modelStart, physStart);
m_activeEntitys++;
}
else
{
std::pair<int, StaticPhysicsObject*>physPair = engine->GetBody(path, orientation, position, btVector3(1, 1, 1), mass);
new (&m_gameEntity[m_activeEntitys]) GameEntity(m_activeEntitys, modelPair.first, modelPair.second, physPair.second);
m_activeEntitys++;
}
#ifdef BUILDER_MODE
std::string temp = GetName(path);
temp.push_back('_' + m_entityMap.size());
m_entityMap.insert(std::pair<string, std::pair<std::string, GameEntity*>>(temp, std::pair<std::string, GameEntity*>(path,&m_gameEntity[m_activeEntitys - 1])));
#endif
}
/* PMDModel::smearAllBonesToDefault: smear all bone pos/rot into default value (rate 1.0 = keep, rate 0.0 = reset) */
void PMDModel::smearAllBonesToDefault(float rate)
{
unsigned short i;
const btVector3 v(0.0f, 0.0f, 0.0f);
const btQuaternion q(0.0f, 0.0f, 0.0f, 1.0f);
btVector3 tmpv;
btQuaternion tmpq;
for (i = 0; i < m_numBone; i++) {
m_boneList[i].getCurrentPosition(&tmpv);
tmpv = v.lerp(tmpv, rate);
m_boneList[i].setCurrentPosition(&tmpv);
m_boneList[i].getCurrentRotation(&tmpq);
tmpq = q.slerp(tmpq, rate);
m_boneList[i].setCurrentRotation(&tmpq);
}
for (i = 0; i < m_numFace; i++) {
m_faceList[i].setWeight(m_faceList[i].getWeight() * rate);
}
}
void Physics::EulerXYZToQuaternion( const CIwFVec3 &TEuler, btQuaternion &TQuat )
{
const double _heading = TEuler.z * 0.5;
const double _attitude = TEuler.y * 0.5;
const double _bank = TEuler.x * 0.5;
const double c1 = cos( _heading );
const double s1 = sin( _heading );
const double c2 = cos( _attitude );
const double s2 = sin( _attitude );
const double c3 = cos( _bank );
const double s3 = sin( _bank );
const double c1c2 = c1 * c2;
const double s1s2 = s1 * s2;
const float w = btScalar( c1c2 * c3 + s1s2 * s3 );
const float x = btScalar( c1c2 * s3 - s1s2 * c3 );
const float y = btScalar( c1 * s2 * c3 + s1 * c2 * s3 );
const float z = btScalar( s1 * c2 * c3 - c1 * s2 * s3 );
TQuat.setW( w );
TQuat.setX( x );
TQuat.setY( y );
TQuat.setZ( z );
}