Merge pull request #986 from PhilipRosedale/master

Faceshift rotates body, slow near others for easier positioning

interface/src/Application.cpp

@@ -1752,6 +1752,11 @@ void Application::update(float deltaTime) {
 
     //  Update faceshift
     _faceshift.update();
+    
+    //  Copy angular velocity if measured by faceshift, to the head
+    if (_faceshift.isActive()) {
+        _myAvatar.getHead().setAngularVelocity(_faceshift.getHeadAngularVelocity());
+    }
 
     // if we have faceshift, use that to compute the lookat direction
     glm::vec3 lookAtRayOrigin = mouseRayOrigin, lookAtRayDirection = mouseRayDirection;
@@ -1934,7 +1939,7 @@ void Application::update(float deltaTime) {
             if (!avatar->isInitialized()) {
                 avatar->init();
             }
-            avatar->simulate(deltaTime, NULL, 0.f);
+            avatar->simulate(deltaTime, NULL);
             avatar->setMouseRay(mouseRayOrigin, mouseRayDirection);
         }
         node->unlock();
@@ -1949,9 +1954,9 @@ void Application::update(float deltaTime) {
     }
 
     if (Menu::getInstance()->isOptionChecked(MenuOption::TransmitterDrive) && _myTransmitter.isConnected()) {
-        _myAvatar.simulate(deltaTime, &_myTransmitter, Menu::getInstance()->getGyroCameraSensitivity());
+        _myAvatar.simulate(deltaTime, &_myTransmitter);
     } else {
-        _myAvatar.simulate(deltaTime, NULL, Menu::getInstance()->getGyroCameraSensitivity());
+        _myAvatar.simulate(deltaTime, NULL);
     }
     
     // no transmitter drive implies transmitter pick

interface/src/Menu.cpp

@@ -488,7 +488,6 @@ void Menu::loadSettings(QSettings* settings) {
         settings = Application::getInstance()->getSettings();
     }
     
-    _gyroCameraSensitivity = loadSetting(settings, "gyroCameraSensitivity", 0.5f);
     _audioJitterBufferSamples = loadSetting(settings, "audioJitterBufferSamples", 0);
     _fieldOfView = loadSetting(settings, "fieldOfView", DEFAULT_FIELD_OF_VIEW_DEGREES);
     _maxVoxels = loadSetting(settings, "maxVoxels", DEFAULT_MAX_VOXELS_PER_SYSTEM);
@@ -512,7 +511,6 @@ void Menu::saveSettings(QSettings* settings) {
         settings = Application::getInstance()->getSettings();
     }
     
-    settings->setValue("gyroCameraSensitivity", _gyroCameraSensitivity);
     settings->setValue("audioJitterBufferSamples", _audioJitterBufferSamples);
     settings->setValue("fieldOfView", _fieldOfView);
     settings->setValue("maxVoxels", _maxVoxels);
@@ -781,10 +779,6 @@ void Menu::editPreferences() {
     fieldOfView->setValue(_fieldOfView);
     form->addRow("Vertical Field of View (Degrees):", fieldOfView);
     
-    QDoubleSpinBox* gyroCameraSensitivity = new QDoubleSpinBox();
-    gyroCameraSensitivity->setValue(_gyroCameraSensitivity);
-    form->addRow("Gyro Camera Sensitivity (0 - 1):", gyroCameraSensitivity);
-    
     QDoubleSpinBox* leanScale = new QDoubleSpinBox();
     leanScale->setValue(applicationInstance->getAvatar()->getLeanScale());
     form->addRow("Lean Scale:", leanScale);
@@ -826,7 +820,6 @@ void Menu::editPreferences() {
     
     applicationInstance->getAvatar()->getHead().setPupilDilation(pupilDilation->value() / (float)pupilDilation->maximum());
     
-    _gyroCameraSensitivity = gyroCameraSensitivity->value();
     _maxVoxels = maxVoxels->value();
     applicationInstance->getVoxels()->setMaxVoxels(_maxVoxels);
     

interface/src/Menu.h

@@ -49,7 +49,6 @@ public:
     
     float getAudioJitterBufferSamples() const { return _audioJitterBufferSamples; }
     float getFieldOfView() const { return _fieldOfView; }
-    float getGyroCameraSensitivity() const { return _gyroCameraSensitivity; }
     BandwidthDialog* getBandwidthDialog() const { return _bandwidthDialog; }
     FrustumDrawMode getFrustumDrawMode() const { return _frustumDrawMode; }
     ViewFrustumOffset getViewFrustumOffset() const { return _viewFrustumOffset; }
@@ -114,7 +113,6 @@ private:
     BandwidthDialog* _bandwidthDialog;
     float _fieldOfView; /// in Degrees, doesn't apply to HMD like Oculus
     FrustumDrawMode _frustumDrawMode;
-    float _gyroCameraSensitivity;
     ViewFrustumOffset _viewFrustumOffset;
     QActionGroup* _voxelModeActionsGroup;
     VoxelStatsDialog* _voxelStatsDialog;

interface/src/avatar/Avatar.cpp

@@ -28,7 +28,6 @@
 using namespace std;
 
 const bool BALLS_ON = false;
-const bool USING_AVATAR_GRAVITY = true;
 const glm::vec3 DEFAULT_UP_DIRECTION(0.0f, 1.0f, 0.0f);
 const float YAW_MAG = 500.0;
 const float MY_HAND_HOLDING_PULL = 0.2;
@@ -289,7 +288,7 @@ void Avatar::follow(Avatar* leadingAvatar) {
     }
 }
 
-void Avatar::simulate(float deltaTime, Transmitter* transmitter, float gyroCameraSensitivity) {
+void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
 
     glm::quat orientation = getOrientation();
     glm::vec3 front = orientation * IDENTITY_FRONT;
@@ -403,7 +402,7 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter, float gyroCamer
     _head.setBodyRotation(glm::vec3(_bodyPitch, _bodyYaw, _bodyRoll));
     _head.setPosition(_bodyBall[ BODY_BALL_HEAD_BASE ].position);
     _head.setSkinColor(glm::vec3(SKIN_COLOR[0], SKIN_COLOR[1], SKIN_COLOR[2]));
-    _head.simulate(deltaTime, false, gyroCameraSensitivity);
+    _head.simulate(deltaTime, false);
     _hand.simulate(deltaTime, false);
 
     // use speed and angular velocity to determine walking vs. standing

interface/src/avatar/Avatar.h

@@ -135,7 +135,7 @@ public:
     ~Avatar();
     
     void init();
-    void simulate(float deltaTime, Transmitter* transmitter, float gyroCameraSensitivity);
+    void simulate(float deltaTime, Transmitter* transmitter);
     void follow(Avatar* leadingAvatar);
     void render(bool lookingInMirror, bool renderAvatarBalls);
 

interface/src/avatar/Head.cpp

@@ -71,6 +71,7 @@ Head::Head(Avatar* owningAvatar) :
     _audioAttack(0.0f),
     _returnSpringScale(1.0f),
     _bodyRotation(0.0f, 0.0f, 0.0f),
+    _angularVelocity(0,0,0),
     _renderLookatVectors(false),
     _mohawkInitialized(false),
     _saccade(0.0f, 0.0f, 0.0f),
@@ -132,7 +133,7 @@ void Head::resetHairPhysics() {
 }
 
 
-void Head::simulate(float deltaTime, bool isMine, float gyroCameraSensitivity) {
+void Head::simulate(float deltaTime, bool isMine) {
     
     //  Update audio trailing average for rendering facial animations
     Faceshift* faceshift = Application::getInstance()->getFaceshift();
@@ -230,44 +231,6 @@ void Head::simulate(float deltaTime, bool isMine, float gyroCameraSensitivity) {
     // based on the nature of the lookat position, determine if the eyes can look / are looking at it.      
     if (USING_PHYSICAL_MOHAWK) {
         updateHairPhysics(deltaTime);
-        
-    }
-    
-    // Update camera pitch and yaw independently from motion of head (for gyro-based interface)
-    if (isMine && _cameraFollowsHead && (gyroCameraSensitivity > 0.f)) {
-        //  If we are using gyros and using gyroLook, have the camera follow head but with a null region
-        //  to create stable rendering view with small head movements.
-        const float CAMERA_FOLLOW_HEAD_RATE_START = 0.1f;
-        const float CAMERA_FOLLOW_HEAD_RATE_MAX = 1.0f;
-        const float CAMERA_FOLLOW_HEAD_RATE_RAMP_RATE = 1.05f;
-        const float CAMERA_STOP_TOLERANCE_DEGREES = 0.5f;
-        const float PITCH_START_RANGE = 20.f;
-        const float YAW_START_RANGE = 10.f;
-        float pitchStartTolerance = PITCH_START_RANGE
-                                    * (1.f - gyroCameraSensitivity)
-                                    + (2.f * CAMERA_STOP_TOLERANCE_DEGREES);
-        float yawStartTolerance = YAW_START_RANGE
-                                    * (1.f - gyroCameraSensitivity)
-                                    + (2.f * CAMERA_STOP_TOLERANCE_DEGREES);
-
-        float cameraHeadAngleDifference = glm::length(glm::vec2(_pitch - _cameraPitch, _yaw - _cameraYaw));
-        if (_isCameraMoving) {
-            _cameraFollowHeadRate = glm::clamp(_cameraFollowHeadRate * CAMERA_FOLLOW_HEAD_RATE_RAMP_RATE,
-                                               0.f,
-                                               CAMERA_FOLLOW_HEAD_RATE_MAX);
-                                               
-            _cameraPitch += (_pitch - _cameraPitch) * _cameraFollowHeadRate;
-            _cameraYaw += (_yaw - _cameraYaw) * _cameraFollowHeadRate;
-            if (cameraHeadAngleDifference < CAMERA_STOP_TOLERANCE_DEGREES) {
-                _isCameraMoving = false;
-            }
-        } else {
-            if ((fabs(_pitch - _cameraPitch) > pitchStartTolerance) ||
-                (fabs(_yaw - _cameraYaw) > yawStartTolerance)) {
-                _isCameraMoving = true;
-                _cameraFollowHeadRate = CAMERA_FOLLOW_HEAD_RATE_START;
-            }
-        }
     }
 }
 

interface/src/avatar/Head.h

@@ -44,7 +44,7 @@ public:
     
     void init();
     void reset();
-    void simulate(float deltaTime, bool isMine, float gyroCameraSensitivity);
+    void simulate(float deltaTime, bool isMine);
     void render(float alpha, bool isMine);
     void renderMohawk();
 
@@ -64,6 +64,8 @@ public:
 
     glm::quat getOrientation() const;
     glm::quat getCameraOrientation () const;
+    const glm::vec3& getAngularVelocity() const { return _angularVelocity; }
+    void setAngularVelocity(glm::vec3 angularVelocity) { _angularVelocity = angularVelocity; }
     
     float getScale() const { return _scale; }
     glm::vec3 getPosition() const { return _position; }
@@ -116,6 +118,7 @@ private:
     float _audioAttack;
     float _returnSpringScale; //strength of return springs
     glm::vec3 _bodyRotation;
+    glm::vec3 _angularVelocity;
     bool _renderLookatVectors;
     BendyLine _hairTuft[NUM_HAIR_TUFTS];
     bool _mohawkInitialized;

interface/src/avatar/MyAvatar.cpp

@@ -23,7 +23,6 @@
 
 using namespace std;
 
-const bool USING_AVATAR_GRAVITY = true;
 const glm::vec3 DEFAULT_UP_DIRECTION(0.0f, 1.0f, 0.0f);
 const float YAW_MAG = 500.0;
 const float COLLISION_RADIUS_SCALAR = 1.2; // pertains to avatar-to-avatar collisions
@@ -65,7 +64,7 @@ void MyAvatar::reset() {
     _hand.reset();
 }
 
-void MyAvatar::simulate(float deltaTime, Transmitter* transmitter, float gyroCameraSensitivity) {
+void MyAvatar::simulate(float deltaTime, Transmitter* transmitter) {
 
     glm::quat orientation = getOrientation();
     glm::vec3 front = orientation * IDENTITY_FRONT;
@@ -160,13 +159,11 @@ void MyAvatar::simulate(float deltaTime, Transmitter* transmitter, float gyroCam
     _avatarTouch.simulate(deltaTime);
 
     // apply gravity
-    if (USING_AVATAR_GRAVITY) {
+    // For gravity, always move the avatar by the amount driven by gravity, so that the collision
-        // For gravity, always move the avatar by the amount driven by gravity, so that the collision
+    // routines will detect it and collide every frame when pulled by gravity to a surface
-        // routines will detect it and collide every frame when pulled by gravity to a surface
+    const float MIN_DISTANCE_AFTER_COLLISION_FOR_GRAVITY = 0.02f;
-        const float MIN_DISTANCE_AFTER_COLLISION_FOR_GRAVITY = 0.02f;
+    if (glm::length(_position - _lastCollisionPosition) > MIN_DISTANCE_AFTER_COLLISION_FOR_GRAVITY) {
-        if (glm::length(_position - _lastCollisionPosition) > MIN_DISTANCE_AFTER_COLLISION_FOR_GRAVITY) {
+        _velocity += _scale * _gravity * (GRAVITY_EARTH * deltaTime);
-            _velocity += _scale * _gravity * (GRAVITY_EARTH * deltaTime);
-        }
     }
 
     if (_isCollisionsOn) {
@@ -211,14 +208,20 @@ void MyAvatar::simulate(float deltaTime, Transmitter* transmitter, float gyroCam
     const float STATIC_FRICTION_STRENGTH = _scale * 20.f;
     applyStaticFriction(deltaTime, _velocity, MAX_STATIC_FRICTION_VELOCITY, STATIC_FRICTION_STRENGTH);
     
+    // Damp avatar velocity
     const float LINEAR_DAMPING_STRENGTH = 0.5f;
     const float SPEED_BRAKE_POWER = _scale * 10.0f;
-    // Note: PER changed squared damping strength to zero 
     const float SQUARED_DAMPING_STRENGTH = 0.0f;
+    
+    float linearDamping = LINEAR_DAMPING_STRENGTH;
+    const float AVATAR_DAMPING_FACTOR = 120.f;
+    if (_distanceToNearestAvatar < _scale * PERIPERSONAL_RADIUS) {
+        linearDamping *= 1.f + AVATAR_DAMPING_FACTOR * (PERIPERSONAL_RADIUS - _distanceToNearestAvatar);
+    }
     if (_speedBrakes) {
-        applyDamping(deltaTime, _velocity, LINEAR_DAMPING_STRENGTH * SPEED_BRAKE_POWER, SQUARED_DAMPING_STRENGTH * SPEED_BRAKE_POWER);
+        applyDamping(deltaTime, _velocity,  linearDamping * SPEED_BRAKE_POWER, SQUARED_DAMPING_STRENGTH * SPEED_BRAKE_POWER);
     } else {
-        applyDamping(deltaTime, _velocity, LINEAR_DAMPING_STRENGTH, SQUARED_DAMPING_STRENGTH);            
+        applyDamping(deltaTime, _velocity, linearDamping, SQUARED_DAMPING_STRENGTH);            
     }
     
     // pitch and roll the body as a function of forward speed and turning delta
@@ -303,7 +306,7 @@ void MyAvatar::simulate(float deltaTime, Transmitter* transmitter, float gyroCam
     _head.setPosition(_bodyBall[ BODY_BALL_HEAD_BASE ].position);
     _head.setScale(_scale);
     _head.setSkinColor(glm::vec3(SKIN_COLOR[0], SKIN_COLOR[1], SKIN_COLOR[2]));
-    _head.simulate(deltaTime, true, gyroCameraSensitivity);
+    _head.simulate(deltaTime, true);
     _hand.simulate(deltaTime, true);
 
     const float WALKING_SPEED_THRESHOLD = 0.2f;
@@ -337,7 +340,13 @@ void MyAvatar::updateFromGyrosAndOrWebcam(bool gyroLook,
     if (faceshift->isActive()) {
         estimatedPosition = faceshift->getHeadTranslation();
         estimatedRotation = safeEulerAngles(faceshift->getHeadRotation());
-    
+        //  Rotate the body if the head is turned quickly
+        glm::vec3 headAngularVelocity = faceshift->getHeadAngularVelocity();
+        const float FACESHIFT_YAW_VIEW_SENSITIVITY = 20.f;
+        const float FACESHIFT_MIN_YAW_VELOCITY = 1.0f;
+        if (fabs(headAngularVelocity.y) > FACESHIFT_MIN_YAW_VELOCITY) {
+            _bodyYawDelta += headAngularVelocity.y * FACESHIFT_YAW_VIEW_SENSITIVITY;
+        }
     } else if (gyros->isActive()) {
         estimatedRotation = gyros->getEstimatedRotation();
     
@@ -647,7 +656,9 @@ void MyAvatar::updateThrust(float deltaTime, Transmitter * transmitter) {
     
     //  Add one time jumping force if requested
     if (_shouldJump) {
-        _thrust += _scale * THRUST_JUMP * up;
+        if (glm::length(_gravity) > EPSILON) {
+            _thrust += _scale * THRUST_JUMP * up;
+        }
         _shouldJump = false;
     }
 

interface/src/avatar/MyAvatar.h

@@ -16,7 +16,7 @@ public:
 	MyAvatar(Node* owningNode = NULL);
 
     void reset();
-    void simulate(float deltaTime, Transmitter* transmitter, float gyroCameraSensitivity);
+    void simulate(float deltaTime, Transmitter* transmitter);
     void updateFromGyrosAndOrWebcam(bool gyroLook, float pitchFromTouch);
     void render(bool lookingInMirror, bool renderAvatarBalls);
     void renderScreenTint(ScreenTintLayer layer, Camera& whichCamera);

interface/src/devices/Faceshift.cpp

@@ -142,8 +142,20 @@ void Faceshift::receive(const QByteArray& buffer) {
             case fsMsg::MSG_OUT_TRACKING_STATE: {
                 const fsTrackingData& data = static_cast<fsMsgTrackingState*>(msg.get())->tracking_data();
                 if ((_tracking = data.m_trackingSuccessful)) {
-                    _headRotation = glm::quat(data.m_headRotation.w, -data.m_headRotation.x,
+                    glm::quat newRotation = glm::quat(data.m_headRotation.w, -data.m_headRotation.x,
-                        data.m_headRotation.y, -data.m_headRotation.z);
+                                                      data.m_headRotation.y, -data.m_headRotation.z);
+                    // Compute angular velocity of the head 
+                    glm::quat r = newRotation * glm::inverse(_headRotation);
+                    float theta = 2 * acos(r.w);
+                    if (theta > EPSILON) {
+                        float rMag = glm::length(glm::vec3(r.x, r.y, r.z));
+                        float AVERAGE_FACESHIFT_FRAME_TIME = 0.033f;
+                        _headAngularVelocity = theta / AVERAGE_FACESHIFT_FRAME_TIME * glm::vec3(r.x, r.y, r.z) / rMag;
+                    } else {
+                        _headAngularVelocity = glm::vec3(0,0,0);
+                    }
+                    _headRotation = newRotation;
+                    
                     const float TRANSLATION_SCALE = 0.02f;
                     _headTranslation = glm::vec3(data.m_headTranslation.x, data.m_headTranslation.y,
                         -data.m_headTranslation.z) * TRANSLATION_SCALE;

interface/src/devices/Faceshift.h

@@ -30,6 +30,7 @@ public:
     bool isActive() const;
 
     const glm::quat& getHeadRotation() const { return _headRotation; }
+    const glm::vec3& getHeadAngularVelocity() const { return _headAngularVelocity; }
     const glm::vec3& getHeadTranslation() const { return _headTranslation; }
 
     float getEyeGazeLeftPitch() const { return _eyeGazeLeftPitch; }
@@ -88,6 +89,7 @@ private:
     uint64_t _lastTrackingStateReceived;
     
     glm::quat _headRotation;
+    glm::vec3 _headAngularVelocity;
     glm::vec3 _headTranslation;
     
     float _eyeGazeLeftPitch;