btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal )
{
//project the center of the sphere on the closest face of the box
btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
btScalar minDist = faceDist;
closestPoint.setX( boxHalfExtent.getX() );
normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f));
faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setX( -boxHalfExtent.getX() );
normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f));
}
faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setY( boxHalfExtent.getY() );
normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f));
}
faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setY( -boxHalfExtent.getY() );
normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f));
}
faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setZ( boxHalfExtent.getZ() );
normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f));
}
faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setZ( -boxHalfExtent.getZ() );
normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f));
}
return minDist;
}
void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
{
// the swing axis is computed as the "twist-free" cone rotation,
// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
// so, if we're outside the limits, the closest way back inside the cone isn't
// along the vector back to the center. better (and more stable) to use the ellipse normal.
// convert swing axis to direction from center to surface of ellipse
// (ie. rotate 2D vector by PI/2)
btScalar y = -vSwingAxis.z();
btScalar z = vSwingAxis.y();
// do the math...
if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
{
// compute gradient/normal of ellipse surface at current "point"
btScalar grad = y/z;
grad *= m_swingSpan2 / m_swingSpan1;
// adjust y/z to represent normal at point (instead of vector to point)
if (y > 0)
y = fabs(grad * z);
else
y = -fabs(grad * z);
// convert ellipse direction back to swing axis
vSwingAxis.setZ(-y);
vSwingAxis.setY( z);
vSwingAxis.normalize();
}
}
// 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;
}
void Physics::QuaternionToEuler( const CIwFQuat& TQuat, btVector3 &TEuler )
{
float a[3];
const float w = TQuat.s;
const float x = TQuat.x;
const float y = TQuat.y;
const float z = TQuat.z;
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 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] );
}
inline void Assign( btVector3& vec, const Vec3D& other )
{
vec.setX( other.X );
vec.setY( other.Y );
vec.setZ( other.Z );
}
void ConvertAngularImpulseToBull(const AngularImpulse& angularimp, btVector3& bull) {
bull.setX(DEG2RAD(angularimp.x));
bull.setY(DEG2RAD(angularimp.z));
bull.setZ(-DEG2RAD(angularimp.y));
}
void ConvertDirectionToBull(const Vector& dir, btVector3& bull) {
bull.setX(dir.x);
bull.setY(dir.z);
bull.setZ(-dir.y);
}
void ConvertPosToBull(const Vector& pos, btVector3& bull) {
bull.setX(HL2BULL(pos.x));
bull.setY(HL2BULL(pos.z));
bull.setZ(-HL2BULL(pos.y));
}