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;
}
Ejemplo n.º 2
0
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();
	}
}
Ejemplo n.º 3
0
// 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;
}
Ejemplo n.º 4
0
// 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] );
}
Ejemplo n.º 8
0
inline void Assign( btVector3& vec, const Vec3D& other )
{
	vec.setX( other.X );
	vec.setY( other.Y );
	vec.setZ( other.Z );
}
Ejemplo n.º 9
0
void ConvertAngularImpulseToBull(const AngularImpulse& angularimp, btVector3& bull) {
	bull.setX(DEG2RAD(angularimp.x));
	bull.setY(DEG2RAD(angularimp.z));
	bull.setZ(-DEG2RAD(angularimp.y));
}
Ejemplo n.º 10
0
void ConvertDirectionToBull(const Vector& dir, btVector3& bull) {
	bull.setX(dir.x);
	bull.setY(dir.z);
	bull.setZ(-dir.y);
}
Ejemplo n.º 11
0
void ConvertPosToBull(const Vector& pos, btVector3& bull) {
	bull.setX(HL2BULL(pos.x));
	bull.setY(HL2BULL(pos.z));
	bull.setZ(-HL2BULL(pos.y));
}