Eigen::Quaternionf rollcorrect(Eigen::Quaternionf rotation){
Vector3f local_z_axis = rotation.inverse() * -Vector3f::UnitZ();
Vector3f local_x_axis = rotation.inverse() * Vector3f::UnitX();
Vector3f local_y_axis = rotation.inverse() * Vector3f::UnitY();
Vector3f z_proj_zx = local_z_axis; z_proj_zx(1) = 0; z_proj_zx.normalize();
Vector3f x_proj_zx = local_x_axis; x_proj_zx(1) = 0; x_proj_zx.normalize();
Vector3f y_proj_zx = local_y_axis; y_proj_zx(1) = 0; y_proj_zx.normalize();
double pitch = angle(local_z_axis, z_proj_zx);
double roll = angle(local_x_axis, x_proj_zx);
int proj_side = local_x_axis.cross(x_proj_zx).dot(local_z_axis) > 0 ? -1 : 1;
int side_up = local_y_axis.dot(Vector3f::UnitY()) > 0 ? 1 : -1;
if(side_up == -1){
roll = M_PI - roll;
}
roll = roll * proj_side * side_up;
double norm_pitch = normalizeRad(pitch);
double correction_factor = (M_PI_2 - fabs(norm_pitch)) / M_PI_2;
correction_factor = pow(correction_factor - 0.5, 1);
if(pitch > 0.7){
correction_factor = 0;
}
AngleAxis<float> roll_correction(correction_factor*-roll, Vector3f::UnitZ());
rotation = roll_correction * rotation;
return rotation;
}
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));
}
}
}
bool Geometry::GetMVBB(const Geometry::VertexSet &vertices, Box &box) {
ApproxMVBB::Vector3List v;
for (Geometry::VertexSet::const_iterator it = vertices.begin(); it!=vertices.end();++it){
pcl::PointXYZ p;
Vertex2Point(*it,p);
v.emplace_back(p.x,p.y,p.z);
}
ApproxMVBB::Matrix3Dyn points(3,v.size());
for(int i=0;i<v.size();i++)
points.col(i)=v[i];
//NOTE parameters see:https://github.com/gabyx/ApproxMVBB
ApproxMVBB::OOBB oobb = ApproxMVBB::approximateMVBB(points,0.001,8,5,0,5);
Eigen::Vector3f centroid(
(const float &) ((oobb.m_minPoint.x()+oobb.m_maxPoint.x())/2),
(const float &) ((oobb.m_minPoint.y()+oobb.m_maxPoint.y())/2),
(const float &) ((oobb.m_minPoint.z()+oobb.m_maxPoint.z())/2));
//upper/lower point in OOBB frame
double width = oobb.m_maxPoint.x()-oobb.m_minPoint.x();
double height = oobb.m_maxPoint.y()-oobb.m_minPoint.y();
double depth = oobb.m_maxPoint.z()-oobb.m_minPoint.z();
if (width<=0 || height<=0 ||depth<=0)
return false;
// coordinate transformation A_IK matrix from OOBB frame K to world frame I
Eigen::Quaternionf q;
q.x()= (float) oobb.m_q_KI.x();
q.y()= (float) oobb.m_q_KI.y();
q.z()= (float) oobb.m_q_KI.z();
q.w()= (float) oobb.m_q_KI.w();
centroid=q.matrix()*centroid;
// a translation to apply to the cube from 0,0,0
box.centroid=centroid;
// a quaternion-based rotation to apply to the cube
box.quanternion=q.inverse();
box.depth=depth;
box.height=height;
box.width=width;
return true;
}
