btTransform ToBullet(aiMatrix4x4t<float> Transform) {
aiQuaterniont<float> OrientationPart;
aiVector3D TranslationPart(0,0,0);
aiVector3D ScalePart(0,0,0);
Transform.Decompose(ScalePart,OrientationPart,TranslationPart);
return btTransform(ToBullet(OrientationPart),ToBullet(TranslationPart));
}
void TriangleShape::UpdateShape()
{
if (mCollisionShape) {
delete mCollisionShape;
}
mCollisionShape = new btTriangleShape(ToBullet(mTriangle[0]), ToBullet(mTriangle[1]), ToBullet(mTriangle[2]));
}
void BulletWrapper::updateObjectHolding(const SkeletonHand& skeletonHand, BulletHandRepresentation& handRepresentation, float deltaTime)
{
const float strengthThreshold = 1.8f;
if (skeletonHand.grabStrength >= strengthThreshold && handRepresentation.m_HeldBody == NULL)
{
// Find body to pick
EigenTypes::Vector3f underHandDirection = -skeletonHand.rotationButNotReally * EigenTypes::Vector3f::UnitY();
btRigidBody* closestBody = utilFindClosestBody(skeletonHand.getManipulationPoint(), underHandDirection);
handRepresentation.m_HeldBody = closestBody;
}
if (skeletonHand.grabStrength < strengthThreshold)
{
// Let body go
handRepresentation.m_HeldBody = NULL;
}
if (handRepresentation.m_HeldBody != NULL)
{
btRigidBody* body = handRepresentation.m_HeldBody;
// Control held body
s_LastHeldBody = body;
m_FramesTilLastHeldBodyReset = 12;
//handRepresentation.m_HeldBody->setCollisionFlags(0);
// apply velocities or apply positions
btVector3 target = ToBullet(skeletonHand.getManipulationPoint());// - skeletonHand.rotationButNotReally * EigenTypes::Vector3f(0.1f, 0.1f, 0.1f));
btVector3 current = body->getWorldTransform().getOrigin();
btVector3 targetVelocity = 0.5f * (target - current) / deltaTime;
body->setLinearVelocity(targetVelocity);
// convert from-to quaternions to angular velocity in world space
{
Eigen::Quaternionf currentRotation = FromBullet(body->getWorldTransform().getRotation());
Eigen::Quaternionf targetRotation = Eigen::Quaternionf(skeletonHand.arbitraryRelatedRotation()); // breaks for left hand ???
Eigen::Quaternionf delta = currentRotation.inverse() * targetRotation;
Eigen::AngleAxis<float> angleAxis(delta);
EigenTypes::Vector3f axis = angleAxis.axis();
float angle = angleAxis.angle();
const float pi = 3.1415926536f;
if (angle > pi) angle -= 2.0f * pi;
if (angle < -pi) angle += 2.0f * pi;
EigenTypes::Vector3f angularVelocity = currentRotation * (axis * angle * 0.5f / deltaTime);
body->setAngularVelocity(ToBullet(angularVelocity));
}
}
}
btRigidBody* BulletWrapper::utilFindClosestBody(const EigenTypes::Vector3f& point, const EigenTypes::Vector3f& acceptedHalfSpaceDirection )
{
btRigidBody* closest = NULL;
float minDist2 = 10000.0f;
for (size_t i = 0; i < m_BodyDatas.size(); i++)
{
if (m_BodyDatas[i].m_Visible) // skip the hand bodies.
{
btRigidBody* body = m_BodyDatas[i].m_Body;
btVector3 position = body->getWorldTransform().getOrigin();
btVector3 handToBody = position - ToBullet(point);
float dot = handToBody.dot(ToBullet(acceptedHalfSpaceDirection));
float dist2 = handToBody.length2();
const bool hardCutOff = true;
if (hardCutOff)
{
if (0.0f < dot)
{
if (dist2 < minDist2)
{
minDist2 = dist2;
closest = body;
}
}
}
else
{
// Alternative
if (0.0f < dot)
{
float sin2 = std::sqrt(1 - dot * dot);
float sin8 = sin2 * sin2 * sin2 * sin2;
float sin32 = sin8 * sin8 *sin8 *sin8;
dist2 *= 1.0f / (sin32 + 0.000001f);
}
if (dist2 < minDist2)
{
minDist2 = dist2;
closest = body;
}
}
}
}
return closest;
}
void BulletWrapper::utilSyncHeadRepresentation(const EigenTypes::Vector3f& headPosition, float deltaTime)
{
// apply velocities or apply positions
btVector3 target = ToBullet(headPosition);
btVector3 current = m_HeadRepresentation->getWorldTransform().getOrigin();
btVector3 targetVelocity = (target - current) / deltaTime;
m_HeadRepresentation->setLinearVelocity(targetVelocity);
m_HeadRepresentation->setAngularVelocity(btVector3(0, 0, 0));
}
void BulletWrapper::updateHandRepresentation(const SkeletonHand& skeletonHand, BulletWrapper::BulletHandRepresentation& handRepresentation, float deltaTime)
{
for (unsigned int i = 0; i < handRepresentation.m_Bodies.size(); i++)
{
EigenTypes::Vector3f jointPos = skeletonHand.joints[i];
btRigidBody* body = handRepresentation.m_Bodies[i];
// apply velocities or apply positions
btVector3 target = ToBullet(jointPos);
btVector3 current = body->getWorldTransform().getOrigin();
btVector3 targetVelocity = (target - current) / deltaTime;
body->setLinearVelocity(targetVelocity);
body->setAngularVelocity(btVector3(0,0,0));
}
}
void BulletWrapper::utilAddCube(const EigenTypes::Vector3f& position, const EigenTypes::Vector3f& halfExtents)
{
m_BodyDatas.resize(m_BodyDatas.size() + 1);
BodyData& bodyData = m_BodyDatas.back();
bodyData.m_SharedShape.reset(new btBoxShape(ToBullet(halfExtents)));
btDefaultMotionState* fallMotionState = new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), btVector3(position.x(), position.y(), position.z())));
btScalar mass = 1;
btVector3 fallInertia(0, 0, 0);
bodyData.m_SharedShape->calculateLocalInertia(mass, fallInertia);
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass, fallMotionState, bodyData.m_SharedShape.get(), fallInertia);
bodyData.m_Body = new btRigidBody(fallRigidBodyCI);
bodyData.m_ShapeType = SHAPE_TYPE_BOX;
bodyData.m_Visible = true;
bodyData.m_Body->setActivationState(DISABLE_DEACTIVATION);
m_DynamicsWorld->addRigidBody(bodyData.m_Body, COLLISION_GROUP_DYNAMIC, COLLISION_GROUP_DYNAMIC | COLLISION_GROUP_HAND);
}