Beispiel #1
0
void  BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
	//todo(erwincoumans)
	//the link->m_inertia is NOT necessarily aligned with the inertial frame
	//so an additional transform might need to be computed
	UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
	
	
	btAssert(linkPtr);
	if (linkPtr)
	{
		UrdfLink* link = *linkPtr;
		if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
		{
			mass = 0.f;
			localInertiaDiagonal.setValue(0,0,0);
		}
		else
		{
			mass = link->m_inertia.m_mass;
			localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
										  link->m_inertia.m_izz);
		}
		inertialFrame = link->m_inertia.m_linkLocalFrame;
		
	}
	else
    {
        mass = 1.f;
        localInertiaDiagonal.setValue(1,1,1);
        inertialFrame.setIdentity();
    }
}
Beispiel #2
0
static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger, bool lastThree = false)
{
	vec3.setZero();
	btArray<std::string> pieces;
	btArray<float> rgba;
	urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" "));
	for (int i = 0; i < pieces.size(); ++i)
	{
		if (!pieces[i].empty())
		{
			rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
		}
	}
	if (rgba.size() < 3)
	{
		logger->reportWarning("Couldn't parse vector3");
		return false;
	}
    if (lastThree) {
        vec3.setValue(rgba[rgba.size()-3], rgba[rgba.size()-2], rgba[rgba.size()-1]);
    }
    else
    {
        vec3.setValue(rgba[0],rgba[1],rgba[2]);

    }
	return true;
}
void	PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)
{

	gEnableRealTimeSimVR = true;

	if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
	{
		printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);
		return;
	}
	if (controllerId == gGraspingController)
	{
		gVRGripperAnalog = analogAxis;
		gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
		btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]);
		gVRGripperOrn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
	}
	else
	{
		gVRGripper2Analog = analogAxis;
		gVRController2Pos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
		btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]);
		gVRController2Orn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
		
		m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
		m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
	}

}
Beispiel #4
0
void btSurfaceShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{
	const felt::UrSurface3D::IsoGrid::Child& child =
		m_psurface->isogrid().children().get(m_pos_child);
	felt::Vec3i pos_min = child.offset();
	felt::Vec3i pos_max = pos_min + child.size().template cast<felt::INT>();

	aabbMin.setValue(btScalar(pos_min(0)),btScalar(pos_min(1)),btScalar(pos_min(2)));
	aabbMax.setValue(btScalar(pos_max(0)),btScalar(pos_max(1)),btScalar(pos_max(2)));
}
Beispiel #5
0
void	btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const
{

    btAssert(x>=0);
    btAssert(y>=0);
    btAssert(x<m_heightStickWidth);
    btAssert(y<m_heightStickLength);


    btScalar	height = getHeightFieldValue(x,y);

    switch (m_upAxis)
    {
    case 0:
    {
        vertex.setValue(
            height,
            (-m_width/btScalar(2.0)) + x,
            (-m_length/btScalar(2.0) ) + y
        );
        break;
    }
    case 1:
    {
        vertex.setValue(
            (-m_width/btScalar(2.0)) + x,
            height,
            (-m_length/btScalar(2.0)) + y
        );
        break;
    };
    case 2:
    {
        vertex.setValue(
            (-m_width/btScalar(2.0)) + x,
            (-m_length/btScalar(2.0)) + y,
            height
        );
        break;
    }
    default:
    {
        //need to get valid m_upAxis
        btAssert(0);
    }
    }

    vertex*=m_localScaling;

}
void  ROSURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
    if ((*m_data->m_links[linkIndex]).inertial)
    {
        mass = (*m_data->m_links[linkIndex]).inertial->mass;
        localInertiaDiagonal.setValue((*m_data->m_links[linkIndex]).inertial->ixx,(*m_data->m_links[linkIndex]).inertial->iyy,(*m_data->m_links[linkIndex]).inertial->izz);
        inertialFrame.setOrigin(btVector3((*m_data->m_links[linkIndex]).inertial->origin.position.x,(*m_data->m_links[linkIndex]).inertial->origin.position.y,(*m_data->m_links[linkIndex]).inertial->origin.position.z));
        inertialFrame.setRotation(btQuaternion((*m_data->m_links[linkIndex]).inertial->origin.rotation.x,(*m_data->m_links[linkIndex]).inertial->origin.rotation.y,(*m_data->m_links[linkIndex]).inertial->origin.rotation.z,(*m_data->m_links[linkIndex]).inertial->origin.rotation.w));
    } else
    {
        mass = 1.f;
        localInertiaDiagonal.setValue(1,1,1);
        inertialFrame.setIdentity();
    }
}
Beispiel #7
0
void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{

#ifdef CALC_EXACT_INERTIA
	inertia.setValue(0.f,0.f,0.f);

	int i = this->getMeshPartCount();
	btScalar partmass = mass/btScalar(i);

	while(i--)
	{
		btVector3 partinertia;
		getMeshPart(i)->calculateLocalInertia(partmass,partinertia);
		inertia+=partinertia;
	}

#else

	// Calc box inertia

	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
	const btScalar x2 = lx*lx;
	const btScalar y2 = ly*ly;
	const btScalar z2 = lz*lz;
	const btScalar scaledmass = mass * btScalar(0.08333333);

	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));

#endif
}
Beispiel #8
0
//------------------------------------------------------------------------
//	NodeyBSPTree::calculateLocalInertia
//------------------------------------------------------------------------
//
void NodeyBSPTree::calculateLocalInertia( btScalar mass, btVector3& inertia ) const
{
	(void) mass;

	// Moving concave objects not supported.
	inertia.setValue( btScalar(0.),btScalar(0.),btScalar(0.) );
}
void	btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
{
	(void)mass;
	//moving concave objects not supported
	btAssert(0);
	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
}
void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
{
	int n = m_children.size();

	btScalar totalMass = 0;
	btVector3 center(0, 0, 0);
	int k;

	for (k = 0; k < n; k++)
	{
		btAssert(masses[k]>0);
		center += m_children[k].m_transform.getOrigin() * masses[k];
		totalMass += masses[k];
	}

	btAssert(totalMass>0);

	center /= totalMass;
	principal.setOrigin(center);

	btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
	for ( k = 0; k < n; k++)
	{
		btVector3 i;
		m_children[k].m_childShape->calculateLocalInertia(masses[k], i);

		const btTransform& t = m_children[k].m_transform;
		btVector3 o = t.getOrigin() - center;

		//compute inertia tensor in coordinate system of compound shape
		btMatrix3x3 j = t.getBasis().transpose();
		j[0] *= i[0];
		j[1] *= i[1];
		j[2] *= i[2];
		j = t.getBasis() * j;

		//add inertia tensor
		tensor[0] += j[0];
		tensor[1] += j[1];
		tensor[2] += j[2];

		//compute inertia tensor of pointmass at o
		btScalar o2 = o.length2();
		j[0].setValue(o2, 0, 0);
		j[1].setValue(0, o2, 0);
		j[2].setValue(0, 0, o2);
		j[0] += o * -o.x(); 
		j[1] += o * -o.y(); 
		j[2] += o * -o.z();

		//add inertia tensor of pointmass
		tensor[0] += masses[k] * j[0];
		tensor[1] += masses[k] * j[1];
		tensor[2] += masses[k] * j[2];
	}

	tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
	inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
}
void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{
	(void)t;
	/*
	btVector3 infvec (btScalar(1e30),btScalar(1e30),btScalar(1e30));

	btVector3 center = m_planeNormal*m_planeConstant;
	aabbMin = center + infvec*m_planeNormal;
	aabbMax = aabbMin;
	aabbMin.setMin(center - infvec*m_planeNormal);
	aabbMax.setMax(center - infvec*m_planeNormal); 
	*/

	aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
	aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));

}
void btBU_Simplex1to4::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{
#if 1
	btPolyhedralConvexAabbCachingShape::getAabb(t, aabbMin, aabbMax);
#else
	aabbMin.setValue(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
	aabbMax.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);

	//just transform the vertices in worldspace, and take their AABB
	for (int i=0;i<m_numVertices;i++)
	{
		btVector3 worldVertex = t(m_vertices[i]);
		aabbMin.setMin(worldVertex);
		aabbMax.setMax(worldVertex);
	}
#endif
}
void	btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
{
	(void)mass;

	//moving concave objects not supported
	
	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
}
Beispiel #14
0
bool parseVector3(const Value &val, btVector3 &retVec) {
	if (!val.isArray() && val.size() == 3) 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;
	retVec.setValue(val[0].asDouble(), val[1].asDouble(),
					val[2].asDouble());
	return true;
}
Beispiel #15
0
void convertToBtTransform(const Vector3d& start_pos, const Vector3d& end_pos, btVector3& origin, btMatrix3x3& basis)
{
	Matrix3d rotation;
	rotation_from_tangent((start_pos - end_pos).normalized(), rotation);
	basis.setValue(rotation(0,0), rotation(0,1), rotation(0,2),
								 rotation(1,0), rotation(1,1), rotation(1,2),
								 rotation(2,0), rotation(2,1), rotation(2,2));
	Vector3d mid_point = (start_pos + end_pos)/2.0;
	origin.setValue(mid_point(0), mid_point(1), mid_point(2));
}
bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
{
    jointLowerLimit = 0.f;
    jointUpperLimit = 0.f;
        
    if ((*m_data->m_links[urdfLinkIndex]).parent_joint)
    {
        my_shared_ptr<Joint> pj =(*m_data->m_links[urdfLinkIndex]).parent_joint;
            
        const urdf::Vector3 pos = pj->parent_to_joint_origin_transform.position;
        const urdf::Rotation orn = pj->parent_to_joint_origin_transform.rotation;
            
        jointAxisInJointSpace.setValue(pj->axis.x,pj->axis.y,pj->axis.z);
        parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
        parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));

        switch (pj->type)
        {
            case Joint::REVOLUTE:
                jointType = URDFRevoluteJoint;
                break;
            case Joint::FIXED:
                jointType = URDFFixedJoint;
                break;
            case Joint::PRISMATIC:
                jointType = URDFPrismaticJoint;
                break;
            case Joint::PLANAR:
                jointType = URDFPlanarJoint;
                break;
            case Joint::CONTINUOUS:
				jointType = URDFContinuousJoint;
                break;
            default:
            {
                printf("Error: unknown joint type %d\n", pj->type);
                btAssert(0);
            }
                    
        };
            
        if (pj->limits)
        {
            jointLowerLimit = pj->limits.get()->lower;
            jointUpperLimit = pj->limits.get()->upper;
        }
        return true;
    } else
    {
        parent2joint.setIdentity();
        return false;
    }
}
void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr)
{
	const btMatrix3x3& rot = tr.getBasis();
	const btVector3& r0 = rot[0];
	const btVector3& r1 = rot[1];
	const btVector3& r2 = rot[2];

	const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z();
	const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z();
	const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z();

	out.setValue(x, y, z);
}
void btBox2dShape::calculateLocalInertia(btScalar mass, btVector3& inertia) const
{
	//btScalar margin = btScalar(0.);
	btVector3 halfExtents = getHalfExtentsWithMargin();

	btScalar lx = btScalar(2.) * (halfExtents.x());
	btScalar ly = btScalar(2.) * (halfExtents.y());
	btScalar lz = btScalar(2.) * (halfExtents.z());

	inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
					 mass / (btScalar(12.0)) * (lx * lx + lz * lz),
					 mass / (btScalar(12.0)) * (lx * lx + ly * ly));
}
Beispiel #19
0
void	btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{
	//approximation of box shape, todo: implement cylinder shape inertia before people notice ;-)
	btVector3 halfExtents = getHalfExtentsWithMargin();

	btScalar lx=btScalar(2.)*(halfExtents.x());
	btScalar ly=btScalar(2.)*(halfExtents.y());
	btScalar lz=btScalar(2.)*(halfExtents.z());

	inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
					mass/(btScalar(12.0)) * (lx*lx + lz*lz),
					mass/(btScalar(12.0)) * (lx*lx + ly*ly));

}
void	btStridingMeshInterface::calculateAabbBruteForce(btVector3& aabbMin,btVector3& aabbMax)
{

	struct	AabbCalculationCallback : public btInternalTriangleIndexCallback
	{
		btVector3	m_aabbMin;
		btVector3	m_aabbMax;

		AabbCalculationCallback()
		{
			m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
			m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
		}

		virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
		{
			(void)partId;
			(void)triangleIndex;

			m_aabbMin.setMin(triangle[0]);
			m_aabbMax.setMax(triangle[0]);
			m_aabbMin.setMin(triangle[1]);
			m_aabbMax.setMax(triangle[1]);
			m_aabbMin.setMin(triangle[2]);
			m_aabbMax.setMax(triangle[2]);
		}
	};

	//first calculate the total aabb for all triangles
	AabbCalculationCallback	aabbCallback;
	aabbMin.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
	aabbMax.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
	InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax);

	aabbMin = aabbCallback.m_aabbMin;
	aabbMax = aabbCallback.m_aabbMax;
}
	void rotate(btVector3 & v) const
	{
	NxQuat myInverse;
	myInverse.x = -x;
	myInverse.y = -y;
	myInverse.z = -z;
	myInverse.w =  w;

	NxQuat left;
	left.multiply(*this,v);
	float vx = left.w*myInverse.x + myInverse.w*left.x + left.y*myInverse.z - myInverse.y*left.z;
	float vy = left.w*myInverse.y + myInverse.w*left.y + left.z*myInverse.x - myInverse.z*left.x;
	float vz = left.w*myInverse.z + myInverse.w*left.z + left.x*myInverse.y - myInverse.x*left.y;
	v.setValue(vx, vy, vz);
	}
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	btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{
	//as an approximation, take the inertia of the box that bounds the spheres

	btVector3 localAabbMin,localAabbMax;
	getCachedLocalAabb(localAabbMin,localAabbMax);
	btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5);

	btScalar lx=btScalar(2.)*(halfExtents.x());
	btScalar ly=btScalar(2.)*(halfExtents.y());
	btScalar lz=btScalar(2.)*(halfExtents.z());

	inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
					mass/(btScalar(12.0)) * (lx*lx + lz*lz),
					mass/(btScalar(12.0)) * (lx*lx + ly*ly));

}
void ConvexDecompositionDemo::setupEmptyDynamicsWorld()
{
m_collisionConfiguration = new btDefaultCollisionConfiguration();


#ifdef USE_PARALLEL_DISPATCHER
#ifdef USE_WIN32_THREADING

	int maxNumOutstandingTasks = 4;//number of maximum outstanding tasks
	Win32ThreadSupport* threadSupport = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo(
								"collision",
								processCollisionTask,
								createCollisionLocalStoreMemory,
								maxNumOutstandingTasks));
#else
///@todo other platform threading
///Playstation 3 SPU (SPURS)  version is available through PS3 Devnet
///Libspe2 SPU support will be available soon
///pthreads version
///you can hook it up to your custom task scheduler by deriving from btThreadSupportInterface
#endif

	m_dispatcher = new	SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks,m_collisionConfiguration);
#else
	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
#endif//USE_PARALLEL_DISPATCHER

	gCompoundChildShapePairCallback = MyCompoundChildShapeCallback;

	convexDecompositionObjectOffset.setValue(10,0,0);

	btVector3 worldAabbMin(-10000,-10000,-10000);
	btVector3 worldAabbMax(10000,10000,10000);

	m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax);
	//m_broadphase = new btSimpleBroadphase();

	m_solver = new btSequentialImpulseConstraintSolver();
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);

#ifdef USE_PARALLEL_DISPATCHER
	m_dynamicsWorld->getDispatchInfo().m_enableSPU = true;
#endif //USE_PARALLEL_DISPATCHER

}
Beispiel #25
0
void btGImpactCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{
	lockChildShapes();
#ifdef CALC_EXACT_INERTIA
	inertia.setValue(0.f,0.f,0.f);

	int i = this->getNumChildShapes();
	btScalar shapemass = mass/btScalar(i);

	while(i--)
	{
		btVector3 temp_inertia;
		m_childShapes[i]->calculateLocalInertia(shapemass,temp_inertia);
		if(childrenHasTransform())
		{
			inertia = gim_inertia_add_transformed( inertia,temp_inertia,m_childTransforms[i]);
		}
		else
		{
			inertia = gim_inertia_add_transformed( inertia,temp_inertia,btTransform::getIdentity());
		}

	}

#else

	// Calc box inertia

	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
	const btScalar x2 = lx*lx;
	const btScalar y2 = ly*ly;
	const btScalar z2 = lz*lz;
	const btScalar scaledmass = mass * btScalar(0.08333333);

	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));

#endif
	unlockChildShapes();
}
Beispiel #26
0
void btGImpactMeshShapePart::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{
	lockChildShapes();


#ifdef CALC_EXACT_INERTIA
	inertia.setValue(0.f,0.f,0.f);

	int i = this->getVertexCount();
	btScalar pointmass = mass/btScalar(i);

	while(i--)
	{
		btVector3 pointintertia;
		this->getVertex(i,pointintertia);
		pointintertia = gim_get_point_inertia(pointintertia,pointmass);
		inertia+=pointintertia;
	}

#else

	// Calc box inertia

	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
	const btScalar x2 = lx*lx;
	const btScalar y2 = ly*ly;
	const btScalar z2 = lz*lz;
	const btScalar scaledmass = mass * btScalar(0.08333333);

	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));

#endif

	unlockChildShapes();
}
static void RenderCallback()
{
	// Clear buffers
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    
	// Setup camera
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0f, ((float)glutGet(GLUT_WINDOW_WIDTH))/((float)glutGet(GLUT_WINDOW_HEIGHT)), 1.0f, 10000.0f);
	gluLookAt(Eye.x(), Eye.y(), Eye.z(), Eye.x() + Dir.x(), Eye.y() + Dir.y(), Eye.z() + Dir.z(), 0.0f, 1.0f, 0.0f);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	glEnable(GL_LIGHTING);

	//clear previous frames result
	gNormal.setValue(10,0,0);
	gPoint.setValue(0,0,0);
	gDepth = 999.999;
	gLastUsedMethod = -1;
	gNumGjkIterations = -1;


	TestEPA(gConvex0, gConvex1);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	btVector3 RefSep(btScalar(0.), btScalar(0.), btScalar(0.));
	float RefDMin=0.f;
	bool RefResult = false;
	if(gRefMode)
		RefResult = ReferenceCode(gConvex0, gConvex1, RefDMin, RefSep);

//	DrawLine(gPoint, gPoint + gNormal*20.0f, btVector3(1,0,0), 2.0f);
//	printf("%f:  %f  %f  %f\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z());

#ifdef VERBOSE_TEXT_ONSCREEN
	glColor3f(255.f, 255.f, 255.f);

	setOrthographicProjection();
	float xOffset = 10.f;
	float yStart = 20.f;
	float yIncr = 20.f;
	char buf[124];

	sprintf(buf,"gDepth=%f:  gNormal=(%f  %f  %f)\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z());
	GLDebugDrawString(xOffset,yStart,buf);
	yStart += yIncr;

	sprintf(buf,"num GJK iterations =%d\n", gNumGjkIterations);
	GLDebugDrawString(xOffset,yStart,buf);
	yStart += yIncr;

	sprintf(buf,"gLastUsedMethod=%d\n", gLastUsedMethod);
	GLDebugDrawString(xOffset,yStart,buf);
	yStart += yIncr;

	

	

	if (gLastUsedMethod >= 3)
	{
		switch (	gMethod)
		{
		case 0:
			sprintf(buf,"Bullet GjkEpa Penetration depth solver (zlib free\n" );
			break;
		case 1:
			sprintf(buf,"Bullet Minkowski sampling Penetration depth solver\n" );
			break;
		case 2:
				sprintf(buf,"Solid35 EPA Penetration depth solver\n" );
				break;
		case 3:
			sprintf(buf,"EPA Penetration depth solver (Experimental/WorkInProgress, zlib free\n" );
			break;
		default:
				sprintf(buf,"Unknown Penetration Depth\n" );
		}
		GLDebugDrawString(xOffset,yStart,buf);
		yStart += yIncr;

	} else
	{
		sprintf(buf,"Hybrid GJK method %d\n", gLastUsedMethod);
		GLDebugDrawString(xOffset,yStart,buf);
		yStart += yIncr;
	}

	if (gLastDegenerateSimplex)
	{
		sprintf(buf,"DegenerateSimplex %d\n", gLastDegenerateSimplex);
		GLDebugDrawString(xOffset,yStart,buf);
		yStart += yIncr;
	}

	


	resetPerspectiveProjection();
#endif //VERBOSE_TEXT_ONSCREEN

	btVector3 color(0,0,0);
	gConvex0.Render(false, color);
	gConvex1.Render(false, color);

	if(gDepth<0.0f)
	{
		btTransform Saved = gConvex0.mTransform;
		gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() - btVector3(gNormal*gDepth));
		gConvex0.Render(true, btVector3(1.0f, 0.5f, 0.0f));
		gConvex0.mTransform = Saved;
	}
	else
	{
		DrawLine(gPoint, gPoint + gNormal, btVector3(0,1,0), 2.0f);
	}

	if(RefResult & gRefMode)
	{
		btTransform Saved = gConvex0.mTransform;
		gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(RefSep*RefDMin));
		gConvex0.Render(true, btVector3(0.0f, 0.5f, 1.0f));
		gConvex0.mTransform = Saved;
	}

	glutSwapBuffers();	
}
		AabbCalculationCallback()
		{
			m_aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
			m_aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
		}
Beispiel #29
0
// Request that the player moves in this direction
extern "C" void bullet_setPlayerDir(float x, float y, float z)
{ g_walkDirection.setValue(x,y,z); }
void	btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{
	btScalar elem = btScalar(0.4) * mass * getMargin()*getMargin();
	inertia.setValue(elem,elem,elem);

}