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