diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 84596c8f7e..327291e8be 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -114,7 +114,7 @@ void GLCanvas::wheelEvent(QWheelEvent* event) { Application::getInstance()->wheelEvent(event); } -Application::Application(int& argc, char** argv) : +Application::Application(int& argc, char** argv, timeval &startup_time) : QApplication(argc, argv), _window(new QMainWindow(desktop())), _glWidget(new GLCanvas()), @@ -152,7 +152,7 @@ Application::Application(int& argc, char** argv) : _bytesPerSecond(0), _bytesCount(0) { - gettimeofday(&_applicationStartupTime, NULL); + _applicationStartupTime = startup_time; _window->setWindowTitle("Interface"); printLog("Interface Startup:\n"); @@ -268,6 +268,15 @@ void Application::initializeGL() { QTimer* idleTimer = new QTimer(this); connect(idleTimer, SIGNAL(timeout()), SLOT(idle())); idleTimer->start(0); + + if (_justStarted) { + float startupTime = (usecTimestampNow() - usecTimestamp(&_applicationStartupTime))/1000000.0; + _justStarted = false; + char title[50]; + sprintf(title, "Interface: %4.2f seconds\n", startupTime); + printLog("%s", title); + _window->setWindowTitle(title); + } } void Application::paintGL() { @@ -404,16 +413,6 @@ void Application::paintGL() { _frameCount++; - - // If application has just started, report time from startup to now (first frame display) - if (_justStarted) { - float startupTime = (usecTimestampNow() - usecTimestamp(&_applicationStartupTime))/1000000.0; - _justStarted = false; - char title[50]; - sprintf(title, "Interface: %4.2f seconds\n", startupTime); - printLog("%s", title); - _window->setWindowTitle(title); - } } void Application::resizeGL(int width, int height) { diff --git a/interface/src/Application.h b/interface/src/Application.h index 01b6f9c4b9..1ac46eef52 100644 --- a/interface/src/Application.h +++ b/interface/src/Application.h @@ -46,7 +46,7 @@ class Application : public QApplication { public: static Application* getInstance() { return static_cast(QCoreApplication::instance()); } - Application(int& argc, char** argv); + Application(int& argc, char** argv, timeval &startup_time); void initializeGL(); void paintGL(); diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index 9e72e1ca90..b1f7926973 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -276,18 +276,29 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) { if (transmitter) { glm::vec3 rotation = transmitter->getEstimatedRotation(); const float TRANSMITTER_MIN_RATE = 1.f; + const float TRANSMITTER_MIN_YAW_RATE = 4.f; const float TRANSMITTER_LATERAL_FORCE_SCALE = 25.f; - const float TRANSMITTER_FWD_FORCE_SCALE = 50.f; - const float TRANSMITTER_YAW_SCALE = 7.0f; + const float TRANSMITTER_FWD_FORCE_SCALE = 25.f; + const float TRANSMITTER_YAW_SCALE = 10.0f; + const float TRANSMITTER_LIFT_SCALE = 3.f; + const float TOUCH_POSITION_RANGE_HALF = 32767.f; if (fabs(rotation.z) > TRANSMITTER_MIN_RATE) { _thrust += rotation.z * TRANSMITTER_LATERAL_FORCE_SCALE * deltaTime * _orientation.getRight(); } if (fabs(rotation.x) > TRANSMITTER_MIN_RATE) { _thrust += -rotation.x * TRANSMITTER_FWD_FORCE_SCALE * deltaTime * _orientation.getFront(); } - if (fabs(rotation.y) > TRANSMITTER_MIN_RATE) { + if (fabs(rotation.y) > TRANSMITTER_MIN_YAW_RATE) { _bodyYawDelta += rotation.y * TRANSMITTER_YAW_SCALE * deltaTime; } + if (transmitter->getTouchState()->state == 'D') { + _thrust += THRUST_MAG * + (float)(transmitter->getTouchState()->y - TOUCH_POSITION_RANGE_HALF) / TOUCH_POSITION_RANGE_HALF * + TRANSMITTER_LIFT_SCALE * + deltaTime * + _orientation.getUp(); + } + } } diff --git a/interface/src/Head.cpp b/interface/src/Head.cpp index f4aa5c9785..67355bda50 100644 --- a/interface/src/Head.cpp +++ b/interface/src/Head.cpp @@ -18,7 +18,6 @@ const float EYE_FRONT_OFFSET = 0.8f; const float EAR_RIGHT_OFFSET = 1.0; const float MOUTH_FRONT_OFFSET = 1.0f; const float MOUTH_UP_OFFSET = -0.3f; -const float HEAD_MOTION_DECAY = 0.1; const float MINIMUM_EYE_ROTATION = 0.7f; // based on a dot product: 1.0 is straight ahead, 0.0 is 90 degrees off const float EYEBALL_RADIUS = 0.02; const float EYEBALL_COLOR[3] = { 0.9f, 0.9f, 0.8f }; @@ -56,13 +55,15 @@ void Head::reset() { void Head::simulate(float deltaTime, bool isMine) { + const float HEAD_MOTION_DECAY = 0.00; + // Decay head back to center if turned on if (isMine && _returnHeadToCenter) { // Decay rotation back toward center - _pitch *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * 2 * deltaTime); - _yaw *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * 2 * deltaTime); - _roll *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * 2 * deltaTime); + _pitch *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * deltaTime); + _yaw *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * deltaTime); + _roll *= (1.0f - HEAD_MOTION_DECAY * _returnSpringScale * deltaTime); } // For invensense gyro, decay only slightly when roughly centered diff --git a/interface/src/Transmitter.cpp b/interface/src/Transmitter.cpp index 917c98847f..5f6def92f7 100644 --- a/interface/src/Transmitter.cpp +++ b/interface/src/Transmitter.cpp @@ -11,6 +11,7 @@ #include "Util.h" #include #include +#include "Log.h" const float DELTA_TIME = 1.f / 60.f; const float DECAY_RATE = 0.15f; @@ -30,24 +31,50 @@ void Transmitter::resetLevels() { } void Transmitter::processIncomingData(unsigned char* packetData, int numBytes) { - if (numBytes == 3 + sizeof(_lastRotationRate) + - sizeof(_lastAcceleration)) { - memcpy(&_lastRotationRate, packetData + 2, sizeof(_lastRotationRate)); - memcpy(&_lastAcceleration, packetData + 3 + sizeof(_lastAcceleration), sizeof(_lastAcceleration)); - + const int PACKET_HEADER_SIZE = 1; // Packet's first byte is 'T' + const int ROTATION_MARKER_SIZE = 1; // 'R' = Rotation (clockwise about x,y,z) + const int ACCELERATION_MARKER_SIZE = 1; // 'A' = Acceleration (x,y,z) + if (numBytes == PACKET_HEADER_SIZE + ROTATION_MARKER_SIZE + ACCELERATION_MARKER_SIZE + + sizeof(_lastRotationRate) + sizeof(_lastAcceleration) + + sizeof(_touchState.x) + sizeof(_touchState.y) + sizeof(_touchState.state)) { + unsigned char* packetDataPosition = &packetData[PACKET_HEADER_SIZE + ROTATION_MARKER_SIZE]; + memcpy(&_lastRotationRate, packetDataPosition, sizeof(_lastRotationRate)); + packetDataPosition += sizeof(_lastRotationRate) + ACCELERATION_MARKER_SIZE; + memcpy(&_lastAcceleration, packetDataPosition, sizeof(_lastAcceleration)); + packetDataPosition += sizeof(_lastAcceleration); + memcpy(&_touchState.state, packetDataPosition, sizeof(_touchState.state)); + packetDataPosition += sizeof(_touchState.state); + memcpy(&_touchState.x, packetDataPosition, sizeof(_touchState.x)); + packetDataPosition += sizeof(_touchState.x); + memcpy(&_touchState.y, packetDataPosition, sizeof(_touchState.y)); + packetDataPosition += sizeof(_touchState.y); + // Update estimated absolute position from rotation rates _estimatedRotation += _lastRotationRate * DELTA_TIME; - - // Decay estimated absolute position to slowly return to zero regardless - _estimatedRotation *= (1.f - DECAY_RATE * DELTA_TIME); + // Sensor Fusion! Slowly adjust estimated rotation to be relative to gravity (average acceleration) + const float GRAVITY_FOLLOW_RATE = 1.f; + float rollAngle = angleBetween(glm::vec3(_lastAcceleration.x, _lastAcceleration.y, 0.f), glm::vec3(0,-1,0)) * + ((_lastAcceleration.x < 0.f) ? -1.f : 1.f); + float pitchAngle = angleBetween(glm::vec3(0.f, _lastAcceleration.y, _lastAcceleration.z), glm::vec3(0,-1,0)) * + ((_lastAcceleration.z < 0.f) ? 1.f : -1.f); + + _estimatedRotation.x = (1.f - GRAVITY_FOLLOW_RATE * DELTA_TIME) * _estimatedRotation.x + + GRAVITY_FOLLOW_RATE * DELTA_TIME * pitchAngle; + _estimatedRotation.z = (1.f - GRAVITY_FOLLOW_RATE * DELTA_TIME) * _estimatedRotation.z + + GRAVITY_FOLLOW_RATE * DELTA_TIME * rollAngle; + + // Can't apply gravity fusion to Yaw, so decay estimated yaw to zero, + // presuming that the average yaw direction is toward screen + _estimatedRotation.y *= (1.f - DECAY_RATE * DELTA_TIME); + if (!_isConnected) { printf("Transmitter V2 Connected.\n"); _isConnected = true; _estimatedRotation *= 0.0; } } else { - printf("Transmitter V2 packet read error.\n"); + printf("Transmitter V2 packet read error, %d bytes.\n", numBytes); } } diff --git a/interface/src/Transmitter.h b/interface/src/Transmitter.h index 6d54b5cbcf..326d0a1182 100644 --- a/interface/src/Transmitter.h +++ b/interface/src/Transmitter.h @@ -15,6 +15,11 @@ #include #include "world.h" +struct TouchState { + uint16_t x, y; + char state; +}; + class Transmitter { public: @@ -26,6 +31,7 @@ public: const glm::vec3 getLastRotationRate() const { return _lastRotationRate; }; const glm::vec3 getLastAcceleration() const { return _lastRotationRate; }; const glm::vec3 getEstimatedRotation() const { return _estimatedRotation; }; + const TouchState* getTouchState() const { return &_touchState; }; void processIncomingData(unsigned char* packetData, int numBytes); private: @@ -33,6 +39,7 @@ private: glm::vec3 _lastRotationRate; glm::vec3 _lastAcceleration; glm::vec3 _estimatedRotation; + TouchState _touchState; #endif /* defined(__hifi__Transmitter__) */ }; diff --git a/interface/src/main.cpp b/interface/src/main.cpp index 58bad2db2f..ea915658af 100644 --- a/interface/src/main.cpp +++ b/interface/src/main.cpp @@ -1,28 +1,31 @@ -// -// Interface -// -// Allows you to connect to and see/hear the shared 3D space. -// Optionally uses serialUSB connection to get gyro data for head movement. -// Optionally gets UDP stream from transmitter to animate controller/hand. -// -// Usage: The interface client first attempts to contact a domain server to -// discover the appropriate audio, voxel, and avatar servers to contact. -// Right now, the default domain server is "highfidelity.below92.com" -// You can change the domain server to use your own by editing the -// DOMAIN_HOSTNAME or DOMAIN_IP strings in the file AgentList.cpp -// -// -// Welcome Aboard! -// - -#include "Application.h" -#include "Log.h" - -int main(int argc, const char * argv[]) { - - Application app(argc, const_cast(argv)); - printLog( "Created QT Application.\n" ); - int exitCode = app.exec(); - printLog("Normal exit.\n"); - return exitCode; -} +// +// Interface +// +// Allows you to connect to and see/hear the shared 3D space. +// Optionally uses serialUSB connection to get gyro data for head movement. +// Optionally gets UDP stream from transmitter to animate controller/hand. +// +// Usage: The interface client first attempts to contact a domain server to +// discover the appropriate audio, voxel, and avatar servers to contact. +// Right now, the default domain server is "highfidelity.below92.com" +// You can change the domain server to use your own by editing the +// DOMAIN_HOSTNAME or DOMAIN_IP strings in the file AgentList.cpp +// +// +// Welcome Aboard! +// + +#include "Application.h" +#include "Log.h" + +int main(int argc, const char * argv[]) { + + timeval startup_time; + gettimeofday(&startup_time, NULL); + + Application app(argc, const_cast(argv), startup_time); + printLog( "Created QT Application.\n" ); + int exitCode = app.exec(); + printLog("Normal exit.\n"); + return exitCode; +}