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++; }