void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) const
{
btTransform tr;
tr.setIdentity();
btVector3 aabbMin,aabbMax;
getAabb(tr,aabbMin,aabbMax);
radius = (aabbMax-aabbMin).length()*btScalar(0.5);
center = (aabbMin+aabbMax)*btScalar(0.5);
}
void btHfFluidBuoyantConvexShape::generateShape (btScalar radius, btScalar gap)
{
btTransform T;
T.setIdentity ();
btVector3 aabbMin, aabbMax;
getAabb (T, aabbMin, aabbMax);
m_radius = radius;
m_numVoxels = 0;
btVoronoiSimplexSolver simplexSolver;
btSphereShape sphereShape(radius);
btVector3* voxelPositions = (btVector3*)btAlignedAlloc (sizeof(btVector3)*MAX_VOXEL_DIMENSION*MAX_VOXEL_DIMENSION*MAX_VOXEL_DIMENSION,16);
for (int i = 0; i < MAX_VOXEL_DIMENSION; i++)
{
for (int j = 0; j < MAX_VOXEL_DIMENSION; j++)
{
for (int k = 0; k < MAX_VOXEL_DIMENSION; k++)
{
btVector3 point;
btTransform sT;
sT.setIdentity ();
point.setX(aabbMin.getX() + (i * btScalar(2.0f) * radius) + (i * gap));
point.setY(aabbMin.getY() + (j * btScalar(2.0f) * radius) + (j * gap));
point.setZ(aabbMin.getZ() + (k * btScalar(2.0f) * radius) + (k * gap));
if (TestPointAgainstAabb2(aabbMin, aabbMax, point))
{
btTransform sT;
sT.setIdentity ();
sT.setOrigin (point);
if (intersect (&simplexSolver, T, sT, m_convexShape, &sphereShape))
{
voxelPositions[m_numVoxels] = point;
m_numVoxels++;
}
}
}
}
}
m_voxelPositions = (btVector3*)btAlignedAlloc (sizeof(btVector3)*m_numVoxels, 16);
for (int i = 0; i < m_numVoxels;i++)
{
m_voxelPositions[i] = voxelPositions[i];
}
btAlignedFree (voxelPositions);
m_volumePerVoxel = btScalar(4.0f)/btScalar(3.0f)*SIMD_PI*radius*radius*radius;
m_totalVolume = m_numVoxels * m_volumePerVoxel;
m_radius = radius;
}
void btCollisionWorld::updateAabbs()
{
BT_PROFILE("updateAabbs");
btTransform predictedTrans;
for ( int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
//only update aabb of active objects
if (colObj->isActive())
{
btVector3 minAabb,maxAabb;
auto shape = colObj->getCollisionShape();
auto xf = colObj->getWorldTransform();
shape->getAabb(xf, minAabb,maxAabb);
//need to increase the aabb for contact thresholds
btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
minAabb -= contactThreshold;
maxAabb += contactThreshold;
btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
//moving objects should be moderately sized, probably something wrong if not
if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
{
bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
} else
{
//something went wrong, investigate
//this assert is unwanted in 3D modelers (danger of loosing work)
colObj->setActivationState(DISABLE_SIMULATION);
static bool reportMe = true;
if (reportMe && m_debugDrawer)
{
reportMe = false;
m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
m_debugDrawer->reportErrorWarning("If you can reproduce this, please email [email protected]\n");
m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
m_debugDrawer->reportErrorWarning("Thanks.\n");
}
}
}
}
}
void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{
//approximation: take the inertia from the aabb for now
btTransform ident;
ident.setIdentity();
btVector3 aabbMin,aabbMax;
getAabb(ident,aabbMin,aabbMax);
btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
btScalar lx=btScalar(2.)*(halfExtents.x());
btScalar ly=btScalar(2.)*(halfExtents.y());
btScalar lz=btScalar(2.)*(halfExtents.z());
inertia[0] = mass/(btScalar(12.0)) * (ly*ly + lz*lz);
inertia[1] = mass/(btScalar(12.0)) * (lx*lx + lz*lz);
inertia[2] = mass/(btScalar(12.0)) * (lx*lx + ly*ly);
}
void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{
//not yet, return box inertia
btScalar margin = getMargin();
btTransform ident;
ident.setIdentity();
btVector3 aabbMin,aabbMax;
getAabb(ident,aabbMin,aabbMax);
btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5);
btScalar lx=btScalar(2.)*(halfExtents.x()+margin);
btScalar ly=btScalar(2.)*(halfExtents.y()+margin);
btScalar lz=btScalar(2.)*(halfExtents.z()+margin);
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));
}
void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const
{
//start with static aabb
getAabb(curTrans,temporalAabbMin,temporalAabbMax);
btScalar temporalAabbMaxx = temporalAabbMax.getX();
btScalar temporalAabbMaxy = temporalAabbMax.getY();
btScalar temporalAabbMaxz = temporalAabbMax.getZ();
btScalar temporalAabbMinx = temporalAabbMin.getX();
btScalar temporalAabbMiny = temporalAabbMin.getY();
btScalar temporalAabbMinz = temporalAabbMin.getZ();
// add linear motion
btVector3 linMotion = linvel*timeStep;
///@todo: simd would have a vector max/min operation, instead of per-element access
if (linMotion.x() > btScalar(0.))
temporalAabbMaxx += linMotion.x();
else
temporalAabbMinx += linMotion.x();
if (linMotion.y() > btScalar(0.))
temporalAabbMaxy += linMotion.y();
else
temporalAabbMiny += linMotion.y();
if (linMotion.z() > btScalar(0.))
temporalAabbMaxz += linMotion.z();
else
temporalAabbMinz += linMotion.z();
//add conservative angular motion
btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
temporalAabbMin -= angularMotion3d;
temporalAabbMax += angularMotion3d;
}
const AabbBox AabbBox::getAabb(const Scene::PointLight& light)
{
return getAabb(light.get());
}
void btFluidHfBuoyantConvexShape::generateShape(btScalar collisionRadius, btScalar volumeEstimationRadius)
{
m_voxelPositions.resize(0);
btVoronoiSimplexSolver simplexSolver;
btTransform voxelTransform = btTransform::getIdentity();
btVector3 aabbMin, aabbMax;
getAabb( btTransform::getIdentity(), aabbMin, aabbMax ); //AABB of the convex shape
//Generate voxels for collision
{
btSphereShape collisionShape(collisionRadius);
const btScalar collisionDiameter = btScalar(2.0) * collisionRadius;
btVector3 numVoxels = (aabbMax - aabbMin) / collisionDiameter;
for(int i = 0; i < ceil( numVoxels.x() ); i++)
{
for(int j = 0; j < ceil( numVoxels.y() ); j++)
{
for(int k = 0; k < ceil( numVoxels.z() ); k++)
{
btVector3 voxelPosition = aabbMin + btVector3(i * collisionDiameter, j * collisionDiameter, k * collisionDiameter);
voxelTransform.setOrigin(voxelPosition);
if( intersect(&simplexSolver, btTransform::getIdentity(), voxelTransform, m_convexShape, &collisionShape) )
m_voxelPositions.push_back(voxelPosition);
}
}
}
const bool CENTER_VOXELS_AABB_ON_ORIGIN = true;
if(CENTER_VOXELS_AABB_ON_ORIGIN)
{
btVector3 diameter(collisionDiameter, collisionDiameter, collisionDiameter);
btVector3 voxelAabbMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
btVector3 voxelAabbMax(-BT_LARGE_FLOAT, -BT_LARGE_FLOAT, -BT_LARGE_FLOAT);
for(int i = 0; i < m_voxelPositions.size(); ++i)
{
voxelAabbMin.setMin(m_voxelPositions[i] - diameter);
voxelAabbMax.setMax(m_voxelPositions[i] + diameter);
}
btVector3 offset = (voxelAabbMax - voxelAabbMin)*btScalar(0.5) - voxelAabbMax;
for(int i = 0; i < m_voxelPositions.size(); ++i) m_voxelPositions[i] += offset;
}
}
//Estimate volume with smaller spheres
btScalar estimatedVolume;
{
btSphereShape volumeEstimationShape(volumeEstimationRadius);
int numCollidingVoxels = 0;
const btScalar estimationDiameter = btScalar(2.0) * volumeEstimationRadius;
btVector3 numEstimationVoxels = (aabbMax - aabbMin) / estimationDiameter;
for(int i = 0; i < ceil( numEstimationVoxels.x() ); i++)
{
for(int j = 0; j < ceil( numEstimationVoxels.y() ); j++)
{
for(int k = 0; k < ceil( numEstimationVoxels.z() ); k++)
{
btVector3 voxelPosition = aabbMin + btVector3(i * estimationDiameter, j * estimationDiameter, k * estimationDiameter);
voxelTransform.setOrigin(voxelPosition);
if( intersect(&simplexSolver, btTransform::getIdentity(), voxelTransform, m_convexShape, &volumeEstimationShape) )
++numCollidingVoxels;
}
}
}
//Although the voxels are spherical, it is better to use the volume of a cube
//for volume estimation. Since convex shapes are completely solid and the voxels
//are generated by moving along a cubic lattice, using the volume of a sphere
//would result in gaps. Comparing the volume of a cube with edge length 2(8 m^3)
//and the volume of a sphere with diameter 2(~4 m^3), the estimated volume would
//be off by about 1/2.
btScalar volumePerEstimationVoxel = btPow( estimationDiameter, btScalar(3.0) );
//btScalar volumePerEstimationVoxel = btScalar(4.0/3.0) * SIMD_PI * btPow( volumeEstimationRadius, btScalar(3.0) );
estimatedVolume = static_cast<btScalar>(numCollidingVoxels) * volumePerEstimationVoxel;
}
m_volumePerVoxel = estimatedVolume / static_cast<btScalar>( getNumVoxels() );
m_totalVolume = estimatedVolume;
m_collisionRadius = collisionRadius;
}
