From 8e06602e9adfab3d788fd5dc2ffd3010fa9feca5 Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Tue, 30 Apr 2013 13:40:16 -0700 Subject: [PATCH 1/3] Added Avatar method to allow received UDP transmitter data to drive the avatar head rotation, cleaned up gyro code. --- interface/src/Avatar.cpp | 281 +++++++++++++++++++++++---------------- interface/src/Avatar.h | 9 ++ interface/src/Util.cpp | 1 - interface/src/main.cpp | 1 + 4 files changed, 176 insertions(+), 116 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 24af81dfdf..a7d88ff893 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -50,73 +50,75 @@ Avatar::Avatar(bool isMine) { _orientation.setToIdentity(); - _velocity = glm::vec3( 0.0, 0.0, 0.0 ); - _thrust = glm::vec3( 0.0, 0.0, 0.0 ); - _rotation = glm::quat( 0.0f, 0.0f, 0.0f, 0.0f ); - _bodyYaw = -90.0; - _bodyPitch = 0.0; - _bodyRoll = 0.0; - _bodyYawDelta = 0.0; - _mousePressed = false; - _mode = AVATAR_MODE_STANDING; - _isMine = isMine; - _maxArmLength = 0.0; - _transmitterHz = 0.0; - _transmitterPackets = 0; - _speed = 0.0; - _pelvisStandingHeight = 0.0f; - _displayingHead = true; - _TEST_bigSphereRadius = 0.3f; - _TEST_bigSpherePosition = glm::vec3( 0.0f, _TEST_bigSphereRadius, 2.0f ); + _velocity = glm::vec3( 0.0, 0.0, 0.0 ); + _thrust = glm::vec3( 0.0, 0.0, 0.0 ); + _rotation = glm::quat( 0.0f, 0.0f, 0.0f, 0.0f ); + _bodyYaw = -90.0; + _bodyPitch = 0.0; + _bodyRoll = 0.0; + _bodyYawDelta = 0.0; + _mousePressed = false; + _mode = AVATAR_MODE_STANDING; + _isMine = isMine; + _maxArmLength = 0.0; + _transmitterHz = 0.0; + _transmitterPackets = 0; + _transmitterIsFirstData = true; + _transmitterInitialReading = glm::vec3( 0.f, 0.f, 0.f ); + _speed = 0.0; + _pelvisStandingHeight = 0.0f; + _displayingHead = true; + _TEST_bigSphereRadius = 0.3f; + _TEST_bigSpherePosition = glm::vec3( 0.0f, _TEST_bigSphereRadius, 2.0f ); for (int i = 0; i < MAX_DRIVE_KEYS; i++) _driveKeys[i] = false; - _head.pupilSize = 0.10; - _head.interPupilDistance = 0.6; - _head.interBrowDistance = 0.75; - _head.nominalPupilSize = 0.10; - _head.pitchRate = 0.0; - _head.yawRate = 0.0; - _head.rollRate = 0.0; - _head.eyebrowPitch[0] = -30; - _head.eyebrowPitch[1] = -30; - _head.eyebrowRoll [0] = 20; - _head.eyebrowRoll [1] = -20; - _head.mouthPitch = 0; - _head.mouthYaw = 0; - _head.mouthWidth = 1.0; - _head.mouthHeight = 0.2; - _head.eyeballPitch[0] = 0; - _head.eyeballPitch[1] = 0; - _head.eyeballScaleX = 1.2; - _head.eyeballScaleY = 1.5; - _head.eyeballScaleZ = 1.0; - _head.eyeballYaw[0] = 0; - _head.eyeballYaw[1] = 0; - _head.pitchTarget = 0; - _head.yawTarget = 0; - _head.noiseEnvelope = 1.0; - _head.pupilConverge = 10.0; - _head.leanForward = 0.0; - _head.leanSideways = 0.0; - _head.eyeContact = 1; - _head.eyeContactTarget = LEFT_EYE; - _head.scale = 1.0; - _head.audioAttack = 0.0; - _head.averageLoudness = 0.0; - _head.lastLoudness = 0.0; - _head.browAudioLift = 0.0; - _head.noise = 0; - _movedHandOffset = glm::vec3( 0.0, 0.0, 0.0 ); - _usingBodySprings = true; - _springForce = 6.0f; - _springVelocityDecay = 16.0f; - _renderYaw = 0.0; - _renderPitch = 0.0; - _sphere = NULL; - _interactingOther = NULL; - _interactingOtherIsNearby = false; - _handHoldingPosition = glm::vec3( 0.0, 0.0, 0.0 ); + _head.pupilSize = 0.10; + _head.interPupilDistance = 0.6; + _head.interBrowDistance = 0.75; + _head.nominalPupilSize = 0.10; + _head.pitchRate = 0.0; + _head.yawRate = 0.0; + _head.rollRate = 0.0; + _head.eyebrowPitch[0] = -30; + _head.eyebrowPitch[1] = -30; + _head.eyebrowRoll [0] = 20; + _head.eyebrowRoll [1] = -20; + _head.mouthPitch = 0; + _head.mouthYaw = 0; + _head.mouthWidth = 1.0; + _head.mouthHeight = 0.2; + _head.eyeballPitch[0] = 0; + _head.eyeballPitch[1] = 0; + _head.eyeballScaleX = 1.2; + _head.eyeballScaleY = 1.5; + _head.eyeballScaleZ = 1.0; + _head.eyeballYaw[0] = 0; + _head.eyeballYaw[1] = 0; + _head.pitchTarget = 0; + _head.yawTarget = 0; + _head.noiseEnvelope = 1.0; + _head.pupilConverge = 10.0; + _head.leanForward = 0.0; + _head.leanSideways = 0.0; + _head.eyeContact = 1; + _head.eyeContactTarget = LEFT_EYE; + _head.scale = 1.0; + _head.audioAttack = 0.0; + _head.averageLoudness = 0.0; + _head.lastLoudness = 0.0; + _head.browAudioLift = 0.0; + _head.noise = 0; + _movedHandOffset = glm::vec3( 0.0, 0.0, 0.0 ); + _usingBodySprings = true; + _springForce = 6.0f; + _springVelocityDecay = 16.0f; + _renderYaw = 0.0; + _renderPitch = 0.0; + _sphere = NULL; + _interactingOther = NULL; + _interactingOtherIsNearby = false; + _handHoldingPosition = glm::vec3( 0.0, 0.0, 0.0 ); initializeSkeleton(); @@ -135,29 +137,32 @@ Avatar::Avatar(bool isMine) { Avatar::Avatar(const Avatar &otherAvatar) { - _velocity = otherAvatar._velocity; - _thrust = otherAvatar._thrust; - _rotation = otherAvatar._rotation; - _interactingOtherIsNearby = otherAvatar._interactingOtherIsNearby; - _bodyYaw = otherAvatar._bodyYaw; - _bodyPitch = otherAvatar._bodyPitch; - _bodyRoll = otherAvatar._bodyRoll; - _bodyYawDelta = otherAvatar._bodyYawDelta; - _mousePressed = otherAvatar._mousePressed; - _mode = otherAvatar._mode; - _isMine = otherAvatar._isMine; - _renderYaw = otherAvatar._renderYaw; - _renderPitch = otherAvatar._renderPitch; - _maxArmLength = otherAvatar._maxArmLength; - _transmitterTimer = otherAvatar._transmitterTimer; - _transmitterHz = otherAvatar._transmitterHz; - _transmitterPackets = otherAvatar._transmitterPackets; - _TEST_bigSphereRadius = otherAvatar._TEST_bigSphereRadius; - _TEST_bigSpherePosition = otherAvatar._TEST_bigSpherePosition; - _movedHandOffset = otherAvatar._movedHandOffset; - _usingBodySprings = otherAvatar._usingBodySprings; - _springForce = otherAvatar._springForce; - _springVelocityDecay = otherAvatar._springVelocityDecay; + _velocity = otherAvatar._velocity; + _thrust = otherAvatar._thrust; + _rotation = otherAvatar._rotation; + _interactingOtherIsNearby = otherAvatar._interactingOtherIsNearby; + _bodyYaw = otherAvatar._bodyYaw; + _bodyPitch = otherAvatar._bodyPitch; + _bodyRoll = otherAvatar._bodyRoll; + _bodyYawDelta = otherAvatar._bodyYawDelta; + _mousePressed = otherAvatar._mousePressed; + _mode = otherAvatar._mode; + _isMine = otherAvatar._isMine; + _renderYaw = otherAvatar._renderYaw; + _renderPitch = otherAvatar._renderPitch; + _maxArmLength = otherAvatar._maxArmLength; + _transmitterTimer = otherAvatar._transmitterTimer; + _transmitterIsFirstData = otherAvatar._transmitterIsFirstData; + _transmitterTimeLastReceived = otherAvatar._transmitterTimeLastReceived; + _transmitterHz = otherAvatar._transmitterHz; + _transmitterInitialReading = otherAvatar._transmitterInitialReading; + _transmitterPackets = otherAvatar._transmitterPackets; + _TEST_bigSphereRadius = otherAvatar._TEST_bigSphereRadius; + _TEST_bigSpherePosition = otherAvatar._TEST_bigSpherePosition; + _movedHandOffset = otherAvatar._movedHandOffset; + _usingBodySprings = otherAvatar._usingBodySprings; + _springForce = otherAvatar._springForce; + _springVelocityDecay = otherAvatar._springVelocityDecay; _orientation.set( otherAvatar._orientation ); _sphere = NULL; @@ -240,9 +245,9 @@ void Avatar::UpdateGyros(float frametime, SerialInterface * serialInterface, glm float measured_pitch_rate = serialInterface->getRelativeValue(HEAD_PITCH_RATE); _head.yawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE); float measured_lateral_accel = serialInterface->getRelativeValue(ACCEL_X) - - ROLL_ACCEL_COUPLING*serialInterface->getRelativeValue(HEAD_ROLL_RATE); + ROLL_ACCEL_COUPLING * serialInterface->getRelativeValue(HEAD_ROLL_RATE); float measured_fwd_accel = serialInterface->getRelativeValue(ACCEL_Z) - - PITCH_ACCEL_COUPLING*serialInterface->getRelativeValue(HEAD_PITCH_RATE); + PITCH_ACCEL_COUPLING * serialInterface->getRelativeValue(HEAD_PITCH_RATE); float measured_roll_rate = serialInterface->getRelativeValue(HEAD_ROLL_RATE); //printLog("Pitch Rate: %d ACCEL_Z: %d\n", serialInterface->getRelativeValue(PITCH_RATE), @@ -423,19 +428,23 @@ void Avatar::simulate(float deltaTime) { // Update Head information // - if (!_head.noise) { + + // Decay head back to center if turned on + if (_returnHeadToCenter) { // Decay back toward center _headPitch *= (1.0f - DECAY * 2 * deltaTime); _headYaw *= (1.0f - DECAY * 2 * deltaTime); _headRoll *= (1.0f - DECAY * 2 * deltaTime); } - else { + + if (_head.noise) { // Move toward new target _headPitch += (_head.pitchTarget - _headPitch) * 10 * deltaTime; // (1.f - DECAY*deltaTime)*Pitch + ; _headYaw += (_head.yawTarget - _headYaw ) * 10 * deltaTime; // (1.f - DECAY*deltaTime); _headRoll *= 1.f - (DECAY * deltaTime); } + _head.leanForward *= (1.f - DECAY * 30 * deltaTime); _head.leanSideways *= (1.f - DECAY * 30 * deltaTime); @@ -1231,14 +1240,19 @@ void Avatar::SetNewHeadTarget(float pitch, float yaw) { _head.yawTarget = yaw; } -// getting data from Android transmitte app +// getting data from Android transmitter void Avatar::processTransmitterData(unsigned char* packetData, int numBytes) { // Read a packet from a transmitter app, process the data - float accX, accY, accZ, - graX, graY, graZ, - gyrX, gyrY, gyrZ, - linX, linY, linZ, - rot1, rot2, rot3, rot4; + float + accX, accY, accZ, // Measured acceleration + graX, graY, graZ, // Gravity + gyrX, gyrY, gyrZ, // Gyro velocity in radians/sec as (pitch, roll, yaw) + linX, linY, linZ, // Linear Acceleration (less gravity) + rot1, rot2, rot3, rot4; // Rotation of device: + // rot1 = roll, ranges from -1 to 1, 0 = flat on table + // rot2 = pitch, ranges from -1 to 1, 0 = flat on table + // rot3 = yaw, ranges from -1 to 1 + sscanf((char *)packetData, "tacc %f %f %f gra %f %f %f gyr %f %f %f lin %f %f %f rot %f %f %f %f", &accX, &accY, &accZ, &graX, &graY, &graZ, @@ -1247,7 +1261,13 @@ void Avatar::processTransmitterData(unsigned char* packetData, int numBytes) { &rot1, &rot2, &rot3, &rot4); if (_transmitterPackets++ == 0) { + // If first packet received, note time, turn head spring return OFF, get start rotation gettimeofday(&_transmitterTimer, NULL); + setHeadReturnToCenter(false); + _transmitterInitialReading = glm::vec3( rot3, + rot2, + rot1 ); + printLog("Using Transmitter to drive head, springs OFF.\n"); } const int TRANSMITTER_COUNT = 100; if (_transmitterPackets % TRANSMITTER_COUNT == 0) { @@ -1257,25 +1277,56 @@ void Avatar::processTransmitterData(unsigned char* packetData, int numBytes) { double msecsElapsed = diffclock(&_transmitterTimer, &now); _transmitterHz = static_cast( (double)TRANSMITTER_COUNT/(msecsElapsed/1000.0) ); _transmitterTimer = now; + printLog("Transmitter Hz: %3.1f\n", _transmitterHz); + } + // Update the head with the transmitter data + glm::vec3 eulerAngles((rot3 - _transmitterInitialReading.x) * 180.f, + -(rot2 - _transmitterInitialReading.y) * 180.f, + (rot1 - _transmitterInitialReading.z) * 180.f); + if (eulerAngles.x > 180.f) { eulerAngles.x -= 360.f; } + if (eulerAngles.x < -180.f) { eulerAngles.x += 360.f; } + + glm::vec3 angularVelocity(glm::degrees(gyrZ), glm::degrees(-gyrX), glm::degrees(gyrY)); + setHeadFromGyros( &eulerAngles, &angularVelocity, + (_transmitterHz == 0.f) ? 0.f : 1.f/_transmitterHz ); +} + +void Avatar::setHeadFromGyros(glm::vec3* eulerAngles, glm::vec3* angularVelocity, float deltaTime) { + // + // Given absolute position and angular velocity information, update the avatar's head angles + // with the goal of fast instantaneous updates that gradually follow the absolute data. + // + // Euler Angle format is (Yaw, Pitch, Roll) in degrees + // + // Angular Velocity is (Yaw, Pitch, Roll) in degrees per second + // + // SMOOTHING_TIME is the time is seconds over which the head should average to the + // absolute eulerAngles passed. + // + // + float const MAX_YAW = 90.f; + float const MIN_YAW = -90.f; + float const MAX_PITCH = 85.f; + float const MIN_PITCH = -85.f; + float const MAX_ROLL = 90.f; + float const MIN_ROLL = -90.f; + + if (deltaTime == 0.f) { + // On first sample, set head to absolute position + setHeadYaw(eulerAngles->x); + setHeadPitch(eulerAngles->y); + setHeadRoll(eulerAngles->z); + } else { + glm::vec3 angles(getHeadYaw(), getHeadPitch(), getHeadRoll()); + // Increment by detected velocity + angles += (*angularVelocity) * deltaTime; + // Smooth to slowly follow absolute values + const float SMOOTHING_TIME = 1.0; + angles = ((1.f - deltaTime/SMOOTHING_TIME) * angles) + (deltaTime/SMOOTHING_TIME) * (*eulerAngles); + setHeadYaw(fmin(fmax(angles.x, MIN_YAW), MAX_YAW)); + setHeadPitch(fmin(fmax(angles.y, MIN_PITCH), MAX_PITCH)); + setHeadRoll(fmin(fmax(angles.z, MIN_ROLL), MAX_ROLL)); } - /* NOTE: PR: Will add back in when ready to animate avatar hand - - // Add rotational forces to the hand - const float ANG_VEL_SENSITIVITY = 4.0; - const float ANG_VEL_THRESHOLD = 0.0; - float angVelScale = ANG_VEL_SENSITIVITY*(1.0f/getTransmitterHz()); - - addAngularVelocity(fabs(gyrX*angVelScale)>ANG_VEL_THRESHOLD?gyrX*angVelScale:0, - fabs(gyrZ*angVelScale)>ANG_VEL_THRESHOLD?gyrZ*angVelScale:0, - fabs(-gyrY*angVelScale)>ANG_VEL_THRESHOLD?-gyrY*angVelScale:0); - - // Add linear forces to the hand - //const float LINEAR_VEL_SENSITIVITY = 50.0; - const float LINEAR_VEL_SENSITIVITY = 5.0; - float linVelScale = LINEAR_VEL_SENSITIVITY*(1.0f/getTransmitterHz()); - glm::vec3 linVel(linX*linVelScale, linZ*linVelScale, -linY*linVelScale); - addVelocity(linVel); - */ } // Find and return the gravity vector at my location diff --git a/interface/src/Avatar.h b/interface/src/Avatar.h index 8f5a7fe41d..234be78c66 100644 --- a/interface/src/Avatar.h +++ b/interface/src/Avatar.h @@ -234,6 +234,10 @@ public: // Find out what the local gravity vector is at this location glm::vec3 getGravity(glm::vec3 pos); + + // Do you want head to try to return to center (depends on interface detected) + void setHeadReturnToCenter(bool r) { _returnHeadToCenter = r; }; + const bool getHeadReturnToCenter() const { return _returnHeadToCenter; }; private: AvatarHead _head; @@ -259,9 +263,12 @@ private: GLUquadric* _sphere; float _renderYaw; float _renderPitch; // Pitch from view frustum when this is own head + bool _transmitterIsFirstData; + timeval _transmitterTimeLastReceived; timeval _transmitterTimer; float _transmitterHz; int _transmitterPackets; + glm::vec3 _transmitterInitialReading; Avatar* _interactingOther; bool _interactingOtherIsNearby; float _pelvisStandingHeight; @@ -269,6 +276,7 @@ private: Balls* _balls; AvatarTouch _avatarTouch; bool _displayingHead; // should be false if in first-person view + bool _returnHeadToCenter; // private methods... void initializeSkeleton(); @@ -279,6 +287,7 @@ private: void readSensors(); void updateCollisionWithSphere( glm::vec3 position, float radius, float deltaTime ); void updateCollisionWithOtherAvatar( Avatar * other, float deltaTime ); + void setHeadFromGyros(glm::vec3 * eulerAngles, glm::vec3 * angularVelocity, float deltaTime); }; #endif diff --git a/interface/src/Util.cpp b/interface/src/Util.cpp index 55b55b12eb..2e25e8cb3b 100644 --- a/interface/src/Util.cpp +++ b/interface/src/Util.cpp @@ -25,7 +25,6 @@ using namespace std; // see http://www.opengl.org/resources/libraries/glut/spec3/node78.html static float MONO_STROKE_WIDTH_GLUT = 104.76; - void eulerToOrthonormals(glm::vec3 * angles, glm::vec3 * front, glm::vec3 * right, glm::vec3 * up) { // // Converts from three euler angles to the associated orthonormal vectors diff --git a/interface/src/main.cpp b/interface/src/main.cpp index 058d7ad2c9..a8834d377e 100644 --- a/interface/src/main.cpp +++ b/interface/src/main.cpp @@ -1421,6 +1421,7 @@ void* networkReceive(void* args) switch (incomingPacket[0]) { case PACKET_HEADER_TRANSMITTER_DATA: + // Process UDP packets that are sent to the client from local sensor devices myAvatar.processTransmitterData(incomingPacket, bytesReceived); break; case PACKET_HEADER_VOXEL_DATA: From 55330cf14fe64ddb5956c70315781303c6d1b22b Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Tue, 30 Apr 2013 13:49:42 -0700 Subject: [PATCH 2/3] Merge repairs. --- interface/src/Avatar.cpp | 23 +++++++++++++++++------ 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 7ddc988b2d..11026ab1b9 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -111,8 +111,6 @@ Avatar::Avatar(bool isMine) { _head.noise = 0; _movedHandOffset = glm::vec3( 0.0, 0.0, 0.0 ); _usingBodySprings = true; - _springForce = 6.0f; - _springVelocityDecay = 16.0f; _renderYaw = 0.0; _renderPitch = 0.0; _sphere = NULL; @@ -161,8 +159,6 @@ Avatar::Avatar(const Avatar &otherAvatar) { _TEST_bigSpherePosition = otherAvatar._TEST_bigSpherePosition; _movedHandOffset = otherAvatar._movedHandOffset; _usingBodySprings = otherAvatar._usingBodySprings; - _springForce = otherAvatar._springForce; - _springVelocityDecay = otherAvatar._springVelocityDecay; _orientation.set( otherAvatar._orientation ); _sphere = NULL; @@ -427,8 +423,23 @@ void Avatar::simulate(float deltaTime) { // update head information updateHead(deltaTime); - - // Decay head back to center if turned on + // calculate speed, and use that to determine walking vs. standing + _speed = glm::length( _velocity ); + float rotationalSpeed = fabs( _bodyYawDelta ); + + if ( _speed + rotationalSpeed > 0.2 ) { + _mode = AVATAR_MODE_WALKING; + } else { + _mode = AVATAR_MODE_INTERACTING; + } +} + + + + +void Avatar::updateHead(float deltaTime) { + + // Decay head back to center if turned on if (_returnHeadToCenter) { // Decay back toward center _headPitch *= (1.0f - DECAY * 2 * deltaTime); From 24a636486e26cc242ec664adc8ac1682ef9adbc0 Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Tue, 30 Apr 2013 16:04:55 -0700 Subject: [PATCH 3/3] Added Google glass support, cleaned up gyro code, ready for Oculus! --- interface/src/Avatar.cpp | 50 +++++++++++++++++++++++++++++----------- interface/src/Avatar.h | 6 ++++- 2 files changed, 42 insertions(+), 14 deletions(-) diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 11026ab1b9..3c60f32d04 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -109,6 +109,7 @@ Avatar::Avatar(bool isMine) { _head.lastLoudness = 0.0; _head.browAudioLift = 0.0; _head.noise = 0; + _head.returnSpringScale = 1.0; _movedHandOffset = glm::vec3( 0.0, 0.0, 0.0 ); _usingBodySprings = true; _renderYaw = 0.0; @@ -442,9 +443,9 @@ void Avatar::updateHead(float deltaTime) { // Decay head back to center if turned on if (_returnHeadToCenter) { // Decay back toward center - _headPitch *= (1.0f - DECAY * 2 * deltaTime); - _headYaw *= (1.0f - DECAY * 2 * deltaTime); - _headRoll *= (1.0f - DECAY * 2 * deltaTime); + _headPitch *= (1.0f - DECAY * _head.returnSpringScale * 2 * deltaTime); + _headYaw *= (1.0f - DECAY * _head.returnSpringScale * 2 * deltaTime); + _headRoll *= (1.0f - DECAY * _head.returnSpringScale * 2 * deltaTime); } if (_head.noise) { @@ -1266,7 +1267,7 @@ void Avatar::SetNewHeadTarget(float pitch, float yaw) { _head.yawTarget = yaw; } -// getting data from Android transmitter +// Process UDP interface data from Android transmitter or Google Glass void Avatar::processTransmitterData(unsigned char* packetData, int numBytes) { // Read a packet from a transmitter app, process the data float @@ -1279,6 +1280,8 @@ void Avatar::processTransmitterData(unsigned char* packetData, int numBytes) { // rot2 = pitch, ranges from -1 to 1, 0 = flat on table // rot3 = yaw, ranges from -1 to 1 + const bool IS_GLASS = false; // Whether to assume this is a Google glass transmitting + sscanf((char *)packetData, "tacc %f %f %f gra %f %f %f gyr %f %f %f lin %f %f %f rot %f %f %f %f", &accX, &accY, &accZ, &graX, &graY, &graZ, @@ -1289,11 +1292,19 @@ void Avatar::processTransmitterData(unsigned char* packetData, int numBytes) { if (_transmitterPackets++ == 0) { // If first packet received, note time, turn head spring return OFF, get start rotation gettimeofday(&_transmitterTimer, NULL); - setHeadReturnToCenter(false); + if (IS_GLASS) { + setHeadReturnToCenter(true); + setHeadSpringScale(10.f); + printLog("Using Google Glass to drive head, springs ON.\n"); + + } else { + setHeadReturnToCenter(false); + printLog("Using Transmitter to drive head, springs OFF.\n"); + + } _transmitterInitialReading = glm::vec3( rot3, rot2, rot1 ); - printLog("Using Transmitter to drive head, springs OFF.\n"); } const int TRANSMITTER_COUNT = 100; if (_transmitterPackets % TRANSMITTER_COUNT == 0) { @@ -1301,10 +1312,13 @@ void Avatar::processTransmitterData(unsigned char* packetData, int numBytes) { timeval now; gettimeofday(&now, NULL); double msecsElapsed = diffclock(&_transmitterTimer, &now); - _transmitterHz = static_cast( (double)TRANSMITTER_COUNT/(msecsElapsed/1000.0) ); + _transmitterHz = static_cast( (double)TRANSMITTER_COUNT / (msecsElapsed / 1000.0) ); _transmitterTimer = now; printLog("Transmitter Hz: %3.1f\n", _transmitterHz); } + //printLog("Gyr: %3.1f, %3.1f, %3.1f\n", glm::degrees(gyrZ), glm::degrees(-gyrX), glm::degrees(gyrY)); + //printLog("Rot: %3.1f, %3.1f, %3.1f, %3.1f\n", rot1, rot2, rot3, rot4); + // Update the head with the transmitter data glm::vec3 eulerAngles((rot3 - _transmitterInitialReading.x) * 180.f, -(rot2 - _transmitterInitialReading.y) * 180.f, @@ -1312,12 +1326,22 @@ void Avatar::processTransmitterData(unsigned char* packetData, int numBytes) { if (eulerAngles.x > 180.f) { eulerAngles.x -= 360.f; } if (eulerAngles.x < -180.f) { eulerAngles.x += 360.f; } - glm::vec3 angularVelocity(glm::degrees(gyrZ), glm::degrees(-gyrX), glm::degrees(gyrY)); - setHeadFromGyros( &eulerAngles, &angularVelocity, - (_transmitterHz == 0.f) ? 0.f : 1.f/_transmitterHz ); + glm::vec3 angularVelocity; + if (!IS_GLASS) { + angularVelocity = glm::vec3(glm::degrees(gyrZ), glm::degrees(-gyrX), glm::degrees(gyrY)); + setHeadFromGyros( &eulerAngles, &angularVelocity, + (_transmitterHz == 0.f) ? 0.f : 1.f / _transmitterHz, 1.0); + + } else { + angularVelocity = glm::vec3(glm::degrees(gyrY), glm::degrees(-gyrX), glm::degrees(-gyrZ)); + setHeadFromGyros( &eulerAngles, &angularVelocity, + (_transmitterHz == 0.f) ? 0.f : 1.f / _transmitterHz, 1000.0); + + } + } -void Avatar::setHeadFromGyros(glm::vec3* eulerAngles, glm::vec3* angularVelocity, float deltaTime) { +void Avatar::setHeadFromGyros(glm::vec3* eulerAngles, glm::vec3* angularVelocity, float deltaTime, float smoothingTime) { // // Given absolute position and angular velocity information, update the avatar's head angles // with the goal of fast instantaneous updates that gradually follow the absolute data. @@ -1347,11 +1371,11 @@ void Avatar::setHeadFromGyros(glm::vec3* eulerAngles, glm::vec3* angularVelocity // Increment by detected velocity angles += (*angularVelocity) * deltaTime; // Smooth to slowly follow absolute values - const float SMOOTHING_TIME = 1.0; - angles = ((1.f - deltaTime/SMOOTHING_TIME) * angles) + (deltaTime/SMOOTHING_TIME) * (*eulerAngles); + angles = ((1.f - deltaTime / smoothingTime) * angles) + (deltaTime / smoothingTime) * (*eulerAngles); setHeadYaw(fmin(fmax(angles.x, MIN_YAW), MAX_YAW)); setHeadPitch(fmin(fmax(angles.y, MIN_PITCH), MAX_PITCH)); setHeadRoll(fmin(fmax(angles.z, MIN_ROLL), MAX_ROLL)); + //printLog("Y/P/R: %3.1f, %3.1f, %3.1f\n", angles.x, angles.y, angles.z); } } diff --git a/interface/src/Avatar.h b/interface/src/Avatar.h index 3dac11367b..7a7f06287a 100644 --- a/interface/src/Avatar.h +++ b/interface/src/Avatar.h @@ -225,6 +225,9 @@ private: float lastLoudness; float averageLoudness; float audioAttack; + + // Strength of return springs + float returnSpringScale; }; AvatarHead _head; @@ -273,7 +276,8 @@ private: void updateHead( float deltaTime ); void updateCollisionWithSphere( glm::vec3 position, float radius, float deltaTime ); void updateCollisionWithOtherAvatar( Avatar * other, float deltaTime ); - void setHeadFromGyros(glm::vec3 * eulerAngles, glm::vec3 * angularVelocity, float deltaTime); + void setHeadFromGyros(glm::vec3 * eulerAngles, glm::vec3 * angularVelocity, float deltaTime, float smoothingTime); + void setHeadSpringScale(float s) { _head.returnSpringScale = s; } }; #endif