//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;
}
//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;
}
//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;
}
// 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;
}*/
}
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() ) );
}
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 );
}
// given a twist rotation in constraint space, (pre: cone must already be removed)
// this method computes its corresponding angle and axis.
void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
btScalar& twistAngle, // out
btVector3& vTwistAxis) // out
{
btQuaternion qMinTwist = qTwist;
twistAngle = qTwist.getAngle();
if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qMinTwist = operator-(qTwist);
twistAngle = qMinTwist.getAngle();
}
if (twistAngle < 0)
{
// this should never happen
int wtf = 0; wtf = wtf;
}
vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
if (twistAngle > SIMD_EPSILON)
vTwistAxis.normalize();
}
void MatrixStack::rotate(btQuaternion rot) {
btVector3 *axis = &rot.getAxis();
Matrix4 rot_matrix;
rot_matrix.rotate(RADTODEG(rot.getAngle()), axis->x(), axis->y(), axis->z());
m_currentMatrix = m_currentMatrix * rot_matrix;
}