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