Matrix3x3 BoxShape::calcInertia(const float4& extent, float mass)
{
Matrix3x3 inertia = mtZero();
(inertia.m_row[0].x) = 1.f/12.f*(pow(extent.y,2.f)+pow(extent.z,2.f))*mass;
(inertia.m_row[1].y) = 1.f/12.f*(pow(extent.z,2.f)+pow(extent.x,2.f))*mass;
(inertia.m_row[2].z) = 1.f/12.f*(pow(extent.x,2.f)+pow(extent.y,2.f))*mass;
return inertia;
}
int btGpuNarrowphaseAndSolver::registerRigidBody(int shapeIndex, float mass, const float* position, const float* orientation , bool writeToGpu)
{
assert(m_internalData->m_numAcceleratedRigidBodies< (MAX_CONVEX_BODIES_CL-1));
m_internalData->m_bodyBufferGPU->resize(m_internalData->m_numAcceleratedRigidBodies+1);
RigidBodyBase::Body& body = m_internalData->m_bodyBufferCPU->at(m_internalData->m_numAcceleratedRigidBodies);
float friction = 1.f;
float restitution = 0.f;
body.m_frictionCoeff = friction;
body.m_restituitionCoeff = restitution;
body.m_angVel = make_float4(0.f);
body.m_linVel = make_float4(0.f);
body.m_pos = make_float4(position[0],position[1],position[2],0.f);
body.m_quat = make_float4(orientation[0],orientation[1],orientation[2],orientation[3]);
body.m_shapeIdx = shapeIndex;
if (shapeIndex<0)
{
body.m_shapeType = CollisionShape::SHAPE_PLANE;
m_planeBodyIndex = m_internalData->m_numAcceleratedRigidBodies;
} else
{
body.m_shapeType = CollisionShape::SHAPE_CONVEX_HEIGHT_FIELD;
}
body.m_invMass = mass? 1.f/mass : 0.f;
if (writeToGpu)
{
m_internalData->m_bodyBufferGPU->copyFromHostPointer(&body,1,m_internalData->m_numAcceleratedRigidBodies);
}
RigidBodyBase::Inertia& shapeInfo = m_internalData->m_inertiaBufferCPU->at(m_internalData->m_numAcceleratedRigidBodies);
if (mass==0.f)
{
shapeInfo.m_initInvInertia = mtZero();
shapeInfo.m_invInertia = mtZero();
} else
{
assert(body.m_shapeIdx>=0);
//approximate using the aabb of the shape
Aabb aabb = (*m_internalData->m_shapePointers)[shapeIndex]->m_aabb;
float4 halfExtents = (aabb.m_max - aabb.m_min);
float4 localInertia;
float lx=2.f*halfExtents.x;
float ly=2.f*halfExtents.y;
float lz=2.f*halfExtents.z;
localInertia = make_float4( (mass/12.0f) * (ly*ly + lz*lz),
(mass/12.0f) * (lx*lx + lz*lz),
(mass/12.0f) * (lx*lx + ly*ly));
float4 invLocalInertia;
invLocalInertia.x = 1.f/localInertia.x;
invLocalInertia.y = 1.f/localInertia.y;
invLocalInertia.z = 1.f/localInertia.z;
invLocalInertia.w = 0.f;
shapeInfo.m_initInvInertia = mtZero();
shapeInfo.m_initInvInertia.m_row[0].x = invLocalInertia.x;
shapeInfo.m_initInvInertia.m_row[1].y = invLocalInertia.y;
shapeInfo.m_initInvInertia.m_row[2].z = invLocalInertia.z;
Matrix3x3 m = qtGetRotationMatrix( body.m_quat);
Matrix3x3 mT = mtTranspose( m );
shapeInfo.m_invInertia = mtMul( mtMul( m, shapeInfo.m_initInvInertia ), mT );
}
if (writeToGpu)
m_internalData->m_inertiaBufferGPU->copyFromHostPointer(&shapeInfo,1,m_internalData->m_numAcceleratedRigidBodies);
return m_internalData->m_numAcceleratedRigidBodies++;
}