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();
}
}
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]);
}
}
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)));
}
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();
}
}
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
}
//------------------------------------------------------------------------
// 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.));
}
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;
}
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));
}
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
}
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();
}
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));
}
// 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);
}