void BulletRigidBodyObject::updateObjectFromBullet(const btTransform& worldTrans) { assert(mFixed == false); LocationInfo& locinfo = mParent->info(mID); btVector3 pos = worldTrans.getOrigin(); btVector3 vel = mObjRigidBody->getLinearVelocity(); TimedMotionVector3f newLocation(mParent->context()->simTime(), MotionVector3f(Vector3f(pos.x(), pos.y(), pos.z()), Vector3f(vel.x(), vel.y(), vel.z()))); mParent->setLocation(mID, newLocation); BULLETLOG(insane, "Updating " << mID << " to velocity " << vel.x() << " " << vel.y() << " " << vel.z()); btQuaternion rot = worldTrans.getRotation(); btVector3 angvel = mObjRigidBody->getAngularVelocity(); Vector3f angvel_siri(angvel.x(), angvel.y(), angvel.z()); float angvel_angle = angvel_siri.normalizeThis(); TimedMotionQuaternion newOrientation( mParent->context()->simTime(), MotionQuaternion( Quaternion(rot.x(), rot.y(), rot.z(), rot.w()), Quaternion(angvel_siri, angvel_angle) ) ); mParent->setOrientation(mID, newOrientation); mParent->addUpdate(mID); }
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~ bool ZeroTorqueBehavior::handlePhysicsForce(Zen::Engine::Core::I_GameObject& _gameObject, Zen::Engine::Physics::I_PhysicsShape::I_ApplyForcesEventData& _eventData) { Zen::Math::Vector3 torque; _eventData.getShape().getTorque(torque); // Negate the torque torque *= -1.0f; _eventData.applyTorque(torque); Zen::Math::Matrix4 orientation; _eventData.getShape().getOrientation(orientation); Zen::Math::Matrix4 newOrientation(Zen::Math::Matrix4::INIT_IDENTITY); Zen::Math::Vector3 position; orientation.getPosition(position); newOrientation.setPosition(position); _eventData.getShape().setOrientation(newOrientation); Zen::Math::Vector3 stopped(0.0f, 0.0f, 0.0f); _eventData.getShape().setAngularMomentum(stopped); return true; }
bool OrientationAdapter::_hasChanges() { WFMath::Quaternion originalOrientation(mOriginalValue); WFMath::Quaternion newOrientation(getValue()); return originalOrientation != newOrientation; }
void BulletCharacterObject::postTick(const Time& t) { LocationInfo& locinfo = mParent->info(mID); btTransform worldTrans = mGhostObject->getWorldTransform(); btVector3 pos = worldTrans.getOrigin(); TimedMotionVector3f newLocation(t, MotionVector3f(Vector3f(pos.x(), pos.y(), pos.z()), locinfo.props.location().velocity())); btQuaternion rot = worldTrans.getRotation(); TimedMotionQuaternion newOrientation( t, MotionQuaternion( Quaternion(rot.x(), rot.y(), rot.z(), rot.w()), locinfo.props.orientation().velocity() ) ); // Only update and report a change if it's big enough. This allows us to // stop sending updates if the object ends up essentially still. float32 pos_diff = (mParent->location(mID).position(t)-newLocation.position(t)).lengthSquared(); // This test is easy, but also conservative... float32 angle1, angle2; Vector3f axis1, axis2; mParent->orientation(mID).position(t).toAngleAxis(angle1, axis1); newOrientation.position(t).toAngleAxis(angle2, axis2); // FIXME what should this really be? This approach doesn't seem to really // work because we sometimes get 0 vectors and the constants seem odd... bool orient_changed = (axis1.dot(axis2) < 0.9) || (fabs(angle1-angle2) > 3.14159/180); if (pos_diff > 0.001 || orient_changed) { mParent->setLocation(mID, newLocation); mParent->setOrientation(mID, newOrientation); mParent->addUpdate(mID); } }
void PhysicsObject::setRotation(glm::vec3 axis, float angle) { btTransform bulletTransform = this->rigidBody->getWorldTransform(); axis = glm::normalize(axis); btQuaternion newOrientation(btVector3(axis.x,axis.y,axis.z),angle); bulletTransform.setRotation(newOrientation); this->rigidBody->setWorldTransform(bulletTransform); this->transformationMatrix = PhysicsUtils::convertBulletTransformToGLM(bulletTransform); }
void PhysicsObject::roll(float angle) { btTransform bulletTransform = this->rigidBody->getWorldTransform(); btQuaternion oldOrientation = bulletTransform.getRotation(); btQuaternion newOrientation(btVector3(0,0,1),angle); newOrientation = oldOrientation + newOrientation; bulletTransform.setRotation(newOrientation); this->rigidBody->setWorldTransform(bulletTransform); this->transformationMatrix = PhysicsUtils::convertBulletTransformToGLM(bulletTransform); }
void PhysicsObject::pitch(float angle) { btTransform bulletTransform = this->collisionObject->getWorldTransform(); btQuaternion oldOrientation = bulletTransform.getRotation(); btQuaternion newOrientation(btVector3(1,0,0),angle); newOrientation = oldOrientation + newOrientation; bulletTransform.setRotation(newOrientation); this->collisionObject->setWorldTransform(bulletTransform); this->transformationMatrix = Utils::convertBulletTransformToGLM(bulletTransform); }
void PhysicsObject::rotate(glm::vec3 axis, float angle) { btTransform bulletTransform = this->collisionObject->getWorldTransform(); btQuaternion oldOrientation = bulletTransform.getRotation(); axis = glm::normalize(axis); btQuaternion newOrientation(btVector3(axis.x,axis.y,axis.z),angle); newOrientation = oldOrientation + newOrientation; bulletTransform.setRotation(newOrientation); this->collisionObject->setWorldTransform(bulletTransform); this->transformationMatrix = Utils::convertBulletTransformToGLM(bulletTransform); }
void GVRInterface::idle() { #if defined(ANDROID) && defined(HAVE_LIBOVR) if (!_inVRMode && ovr_IsHeadsetDocked()) { qDebug() << "The headset just got docked - enter VR mode."; enterVRMode(); } else if (_inVRMode) { if (ovr_IsHeadsetDocked()) { static int counter = 0; // Get the latest head tracking state, predicted ahead to the midpoint of the time // it will be displayed. It will always be corrected to the real values by // time warp, but the closer we get, the less black will be pulled in at the edges. const double now = ovr_GetTimeInSeconds(); static double prev; const double rawDelta = now - prev; prev = now; const double clampedPrediction = std::min( 0.1, rawDelta * 2); ovrSensorState sensor = ovrHmd_GetSensorState(OvrHmd, now + clampedPrediction, true ); auto ovrOrientation = sensor.Predicted.Pose.Orientation; glm::quat newOrientation(ovrOrientation.w, ovrOrientation.x, ovrOrientation.y, ovrOrientation.z); _client->setOrientation(newOrientation); if (counter++ % 100000 == 0) { qDebug() << "GetSensorState in frame" << counter << "-" << ovrOrientation.x << ovrOrientation.y << ovrOrientation.z << ovrOrientation.w; } } else { qDebug() << "The headset was undocked - leaving VR mode."; leaveVRMode(); } } OVR::KeyState& backKeyState = _mainWindow->getBackKeyState(); auto backEvent = backKeyState.Update(ovr_GetTimeInSeconds()); if (backEvent == OVR::KeyState::KEY_EVENT_LONG_PRESS) { qDebug() << "Attemping to start the Platform UI Activity."; ovr_StartPackageActivity(_ovr, PUI_CLASS_NAME, PUI_GLOBAL_MENU); } else if (backEvent == OVR::KeyState::KEY_EVENT_DOUBLE_TAP || backEvent == OVR::KeyState::KEY_EVENT_SHORT_PRESS) { qDebug() << "Got an event we should cancel for!"; } else if (backEvent == OVR::KeyState::KEY_EVENT_DOUBLE_TAP) { qDebug() << "The button is down!"; } #endif }
void DeclinationFilter::correct(unsigned, const CompassData* data) { CompassData newOrientation(*data); if(newOrientation.timestamp_ - lastUpdate_ > updateInterval_) { loadSettings(); lastUpdate_ = newOrientation.timestamp_; } newOrientation.correctedDegrees_ = newOrientation.degrees_; if(declinationCorrection_) { newOrientation.correctedDegrees_ += declinationCorrection_; newOrientation.correctedDegrees_ %= 360; sensordLogT() << "DeclinationFilter corrected degree " << newOrientation.degrees_ << " => " << newOrientation.correctedDegrees_ << ". Level: " << newOrientation.level_; } orientation_ = newOrientation; source_.propagate(1, &orientation_); }
void DeclinationFilter::correct(unsigned, const CompassData* data) { CompassData newOrientation(*data); if (newOrientation.timestamp_ - lastUpdate_ > updateInterval_) { loadSettings(); lastUpdate_ = newOrientation.timestamp_; } newOrientation.correctedDegrees_ = newOrientation.degrees_; #if QT_VERSION < QT_VERSION_CHECK(5, 0, 0) if (declinationCorrection_) { newOrientation.correctedDegrees_ += declinationCorrection_; #else if (declinationCorrection_.loadAcquire() != 0) { newOrientation.correctedDegrees_ += declinationCorrection_.loadAcquire(); #endif newOrientation.correctedDegrees_ %= 360; // sensordLogT() << "DeclinationFilter corrected degree " << newOrientation.degrees_ << " => " << newOrientation.correctedDegrees_ << ". Level: " << newOrientation.level_; } orientation_ = newOrientation; source_.propagate(1, &orientation_); } void DeclinationFilter::loadSettings() { QSettings confFile("/etc/xdg/sensorfw/location.conf", QSettings::IniFormat); confFile.beginGroup("location"); double declination = confFile.value("declination",0).toDouble(); if (declination != 0) { declinationCorrection_ = declination; } #if QT_VERSION < QT_VERSION_CHECK(5, 0, 0) sensordLogD() << "Fetched declination correction from GConf: " << declinationCorrection_; #else sensordLogD() << "Fetched declination correction from GConf: " << declinationCorrection_.loadAcquire(); #endif }
void Director1::rotateDevice(CCObject* pSender) { newOrientation(); restartCallback(NULL); }
unsigned int CObjTreePlugin::insertBBox(unsigned int id, const geometry_msgs::Pose &pose, const geometry_msgs::Vector3 &scale, CObjTreePlugin::Operation op) { printf("insertBoundingBox called, mode %d\n", op); if(op == INSERT && m_octree.removeObject(id)) { //Updating existing box removePrimitiveMarker(id); } objtree::Point newPosition(pose.position.x, pose.position.y, pose.position.z); objtree::Vector4f newOrientation(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); objtree::Point newScale(scale.x, scale.y, scale.z); //Compute minimal aligned bounding box Eigen::Vector3f min, max; Eigen::Vector3f vec[4]; vec[0] = Eigen::Vector3f(-scale.x/2.0f, -scale.y/2.0f, -scale.z/2.0f); vec[1] = Eigen::Vector3f(-scale.x/2.0f, scale.y/2.0f, -scale.z/2.0f); vec[2] = Eigen::Vector3f( scale.x/2.0f, -scale.y/2.0f, -scale.z/2.0f); vec[3] = Eigen::Vector3f( scale.x/2.0f, scale.y/2.0f, -scale.z/2.0f); Eigen::Quaternionf q(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); min = max = q*vec[0]; for(int i = 1; i < 4; i++) { vec[i] = q*vec[i]; for(int j = 0; j < 3; j++) { if(min[j] > vec[i][j]) min[j] = vec[i][j]; if(max[j] < vec[i][j]) max[j] = vec[i][j]; } } for(int i = 0; i < 4; i++) { vec[i] = -vec[i]; for(int j = 0; j < 3; j++) { if(min[j] > vec[i][j]) min[j] = vec[i][j]; if(max[j] < vec[i][j]) max[j] = vec[i][j]; } } Eigen::Vector3f alignedScale(max-min); objtree::Box alignedBox(min[0]+newPosition.x, min[1]+newPosition.y, min[2]+newPosition.z, alignedScale[0], alignedScale[1], alignedScale[2]); objtree::GBBox *newBox = new objtree::GBBox(newPosition, newOrientation, newScale, alignedBox); newBox->setId(id); switch(op) { case INSERT: return m_octree.insert(newBox); case UPDATE: return m_octree.insertUpdate(newBox); case GET_SIMILAR: { unsigned int id = -1; const objtree::Object *object = m_octree.getSimilarObject(newBox); if(object) { id = object->id(); } delete newBox; return id; } } return -1; }