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));
}
static void draw_axes(const btRigidBody& rb, const btTransform& frame)
{
glBegin(GL_LINES);
// draw world transform
btVector3 from = rb.getWorldTransform().getOrigin();
btVector3 to = from + rb.getWorldTransform().getBasis() * btVector3(5,0,0);
// X in red
glColor3f(255.0F, 0.0F, 0.0F);
glVertex3d(from.getX(), from.getY(), from.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
to = from + rb.getWorldTransform().getBasis() * btVector3(0,5,0);
// Y in green
glColor3f(0.0F, 255.0F, 0.0F);
glVertex3d(from.getX(), from.getY(), from.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
to = from + rb.getWorldTransform().getBasis() * btVector3(0,0,5);
// Z in blue
glColor3f(0.0F, 0.0F, 255.0F);
glVertex3d(from.getX(), from.getY(), from.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
// draw slider frame
btTransform calc_frame = rb.getWorldTransform() * frame;
from = calc_frame.getOrigin();
to = from + calc_frame.getBasis() * btVector3(10,0,0);
// X in red
glColor3f(255.0F, 0.0F, 0.0F);
glVertex3d(from.getX(), from.getY(), from.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
to = from + calc_frame.getBasis() * btVector3(0,10,0);
// Y in green
glColor3f(0.0F, 255.0F, 0.0F);
glVertex3d(from.getX(), from.getY(), from.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
to = from + calc_frame.getBasis() * btVector3(0,0,10);
// Z in blue
glColor3f(0.0F, 0.0F, 255.0F);
glVertex3d(from.getX(), from.getY(), from.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
} // draw_axes()
void set_trans(cl_float4 clpos, quaternion m)
{
mat3f mat_diff = base_diff.get_rotation_matrix();
mat3f current_hand = m.get_rotation_matrix();
mat3f my_rot = current_hand * mat_diff;
quaternion n;
n.load_from_matrix(my_rot);
vec3f absolute_pos = {clpos.x, clpos.y, clpos.z};
///current hand does not take into account the rotation offset when grabbing
///ie we'll double rotate
vec3f offset_rot = current_hand * offset;
vec3f pos = absolute_pos + offset_rot;
btTransform newTrans;
//rigid_body->getMotionState()->getWorldTransform(newTrans);
newTrans.setOrigin(btVector3(pos.v[0], pos.v[1], pos.v[2]));
newTrans.setRotation(btQuaternion(n.x(), n.y(), n.z(), n.w()));
rigid_body->getMotionState()->setWorldTransform(newTrans);
//rigid_body->setInterpolationWorldTransform(newTrans);
//if(ctr)
// ctr->set_pos(conv_implicit<cl_float4>(pos));
slide_parent_init = true;
slide_saved_parent = absolute_pos;
remote_pos = pos;
remote_rot = n;
kinematic_old = kinematic_current;
kinematic_current = xyzf_to_vec(rigid_body->getWorldTransform().getOrigin());
}
void BulletWrapper::utilBounceBodiesAt2mAway()
{
btVector3 headPosition = m_HeadRepresentation->getWorldTransform().getOrigin();
for (size_t i = 0; i < m_BodyDatas.size(); i++)
{
btRigidBody* body = m_BodyDatas[i].m_Body;
btVector3 position = body->getWorldTransform().getOrigin();
if ((position - headPosition).length2() > 2.0f * 2.0f)
{
// ensure velocity is not away from center
btVector3 velocity = body->getLinearVelocity();
btVector3 toCenter = (headPosition - position).normalized();
float dot = velocity.dot(toCenter);
if (dot < 0.0f)
{
velocity -= 2.0f * dot * toCenter;
body->setLinearVelocity(velocity);
}
}
}
}
