Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-20 11:45:36 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge branch 'master' of github.com:worklist/hifi into synthesis

Browse source
This commit is contained in:
Stephen Birarda 2013-06-05 17:03:53 -07:00
commit e5a1fa4dec
11 changed files with 221 additions and 128 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

11
avatar-mixer/src/main.cpp
View file

@ -105,6 +105,17 @@ int main(int argc, const char* argv[]) {
agentList->getAgentSocket()->send(agentAddress, broadcastPacket, currentBufferPosition - broadcastPacket);
break;
case PACKET_HEADER_AVATAR_VOXEL_URL:
// grab the agent ID from the packet
unpackAgentId(packetData + 1, &agentID);
// let everyone else know about the update
for (AgentList::iterator agent = agentList->begin(); agent != agentList->end(); agent++) {
if (agent->getActiveSocket() && agent->getAgentID() != agentID) {
agentList->getAgentSocket()->send(agent->getActiveSocket(), packetData, receivedBytes);
}
}
break;
case PACKET_HEADER_DOMAIN:
// ignore the DS packet, for now agents are added only when they communicate directly with us

118
interface/src/Application.cpp
View file

@ -753,8 +753,8 @@ void Application::timer() {
gettimeofday(&_timerStart, NULL);
// if we haven't detected gyros, check for them now
if (!_serialPort.active) {
_serialPort.pair();
if (!_serialHeadSensor.active) {
_serialHeadSensor.pair();
}
}
@ -892,8 +892,8 @@ void Application::idle() {
}
// Read serial port interface devices
if (_serialPort.active) {
_serialPort.readData(deltaTime);
if (_serialHeadSensor.active) {
_serialHeadSensor.readData(deltaTime);
}
// Update transmitter
@ -983,6 +983,44 @@ void Application::terminate() {
}
}
static void sendAvatarVoxelURLMessage(const QUrl& url) {
uint16_t ownerID = AgentList::getInstance()->getOwnerID();
if (ownerID == UNKNOWN_AGENT_ID) {
return; // we don't yet know who we are
}
QByteArray message;
message.append(PACKET_HEADER_AVATAR_VOXEL_URL);
message.append((const char*)&ownerID, sizeof(ownerID));
message.append(url.toEncoded());
AgentList::getInstance()->broadcastToAgents((unsigned char*)message.data(), message.size(), &AGENT_TYPE_AVATAR_MIXER, 1);
}
static void processAvatarVoxelURLMessage(unsigned char *packetData, size_t dataBytes) {
// skip the header
packetData++;
dataBytes--;
// read the agent id
uint16_t agentID = *(uint16_t*)packetData;
packetData += sizeof(agentID);
dataBytes -= sizeof(agentID);
// make sure the agent exists
Agent* agent = AgentList::getInstance()->agentWithID(agentID);
if (!agent || !agent->getLinkedData()) {
return;
}
Avatar* avatar = static_cast<Avatar*>(agent->getLinkedData());
if (!avatar->isInitialized()) {
return; // wait until initialized
}
QUrl url = QUrl::fromEncoded(QByteArray((char*)packetData, dataBytes));
// invoke the set URL function on the simulate/render thread
QMetaObject::invokeMethod(avatar->getVoxels(), "setVoxelURL", Q_ARG(QUrl, url));
}
void Application::editPreferences() {
QDialog dialog(_glWidget);
dialog.setWindowTitle("Interface Preferences");
@ -1006,7 +1044,8 @@ void Application::editPreferences() {
}
QUrl url(avatarURL->text());
_settings->setValue("avatarURL", url);
_myAvatar.getVoxels()->loadVoxelsFromURL(url);
_myAvatar.getVoxels()->setVoxelURL(url);
sendAvatarVoxelURLMessage(url);
}
void Application::pair() {
@ -1507,7 +1546,9 @@ void Application::init() {
_myCamera.setModeShiftRate(1.0f);
_myAvatar.setDisplayingLookatVectors(false);
_myAvatar.getVoxels()->loadVoxelsFromURL(_settings->value("avatarURL").toUrl());
QUrl avatarURL = _settings->value("avatarURL").toUrl();
_myAvatar.getVoxels()->setVoxelURL(avatarURL);
sendAvatarVoxelURLMessage(avatarURL);
QCursor::setPos(_headMouseX, _headMouseY);
@ -1523,26 +1564,40 @@ void Application::init() {
}
void Application::updateAvatar(float deltaTime) {
// Update my avatar's head position from gyros
_myAvatar.updateHeadFromGyros(deltaTime, &_serialPort);
// Grab latest readings from the gyros
float measuredPitchRate = _serialPort.getLastPitchRate();
float measuredYawRate = _serialPort.getLastYawRate();
// Update gyro-based mouse (X,Y on screen)
const float MIN_MOUSE_RATE = 3.0;
const float HORIZONTAL_PIXELS_PER_DEGREE = 2880.f / 45.f;
const float VERTICAL_PIXELS_PER_DEGREE = 1800.f / 30.f;
if (powf(measuredYawRate * measuredYawRate +
measuredPitchRate * measuredPitchRate, 0.5) > MIN_MOUSE_RATE) {
_headMouseX -= measuredYawRate * HORIZONTAL_PIXELS_PER_DEGREE * deltaTime;
_headMouseY -= measuredPitchRate * VERTICAL_PIXELS_PER_DEGREE * deltaTime;
if (_serialHeadSensor.active) {
// Update avatar head translation
if (_gyroLook->isChecked()) {
glm::vec3 headPosition = _serialHeadSensor.getEstimatedPosition();
const float HEAD_OFFSET_SCALING = 3.f;
headPosition *= HEAD_OFFSET_SCALING;
_myCamera.setEyeOffsetPosition(headPosition);
}
// Update my avatar's head position from gyros
_myAvatar.updateHeadFromGyros(deltaTime, &_serialHeadSensor);
// Grab latest readings from the gyros
float measuredPitchRate = _serialHeadSensor.getLastPitchRate();
float measuredYawRate = _serialHeadSensor.getLastYawRate();
// Update gyro-based mouse (X,Y on screen)
const float MIN_MOUSE_RATE = 3.0;
const float HORIZONTAL_PIXELS_PER_DEGREE = 2880.f / 45.f;
const float VERTICAL_PIXELS_PER_DEGREE = 1800.f / 30.f;
if (powf(measuredYawRate * measuredYawRate +
measuredPitchRate * measuredPitchRate, 0.5) > MIN_MOUSE_RATE) {
_headMouseX -= measuredYawRate * HORIZONTAL_PIXELS_PER_DEGREE * deltaTime;
_headMouseY -= measuredPitchRate * VERTICAL_PIXELS_PER_DEGREE * deltaTime;
}
_headMouseX = max(_headMouseX, 0);
_headMouseX = min(_headMouseX, _glWidget->width());
_headMouseY = max(_headMouseY, 0);
_headMouseY = min(_headMouseY, _glWidget->height());
}
_headMouseX = max(_headMouseX, 0);
_headMouseX = min(_headMouseX, _glWidget->width());
_headMouseY = max(_headMouseY, 0);
_headMouseY = min(_headMouseY, _glWidget->height());
if (OculusManager::isConnected()) {
float yaw, pitch, roll;
@ -1585,6 +1640,12 @@ void Application::updateAvatar(float deltaTime) {
const char broadcastReceivers[2] = {AGENT_TYPE_VOXEL, AGENT_TYPE_AVATAR_MIXER};
AgentList::getInstance()->broadcastToAgents(broadcastString, endOfBroadcastStringWrite - broadcastString, broadcastReceivers, sizeof(broadcastReceivers));
// once in a while, send my voxel url
const float AVATAR_VOXEL_URL_SEND_INTERVAL = 1.0f; // seconds
if (shouldDo(AVATAR_VOXEL_URL_SEND_INTERVAL, deltaTime)) {
sendAvatarVoxelURLMessage(_myAvatar.getVoxels()->getVoxelURL());
}
}
// If I'm in paint mode, send a voxel out to VOXEL server agents.
@ -1954,7 +2015,7 @@ void Application::displayOverlay() {
}
// Show detected levels from the serial I/O ADC channel sensors
if (_displayLevels) _serialPort.renderLevels(_glWidget->width(), _glWidget->height());
if (_displayLevels) _serialHeadSensor.renderLevels(_glWidget->width(), _glWidget->height());
// Show hand transmitter data if detected
if (_myTransmitter.isConnected()) {
@ -2332,8 +2393,8 @@ void Application::resetSensors() {
_headMouseX = _mouseX = _glWidget->width() / 2;
_headMouseY = _mouseY = _glWidget->height() / 2;
if (_serialPort.active) {
_serialPort.resetAverages();
if (_serialHeadSensor.active) {
_serialHeadSensor.resetAverages();
}
QCursor::setPos(_headMouseX, _headMouseY);
_myAvatar.reset();
@ -2421,6 +2482,9 @@ void* Application::networkReceive(void* args) {
app->_incomingPacket,
bytesReceived);
break;
case PACKET_HEADER_AVATAR_VOXEL_URL:
processAvatarVoxelURLMessage(app->_incomingPacket, bytesReceived);
break;
default:
AgentList::getInstance()->processAgentData(&senderAddress, app->_incomingPacket, bytesReceived);
break;

5
interface/src/Application.h
View file

@ -199,9 +199,9 @@ private:
QAction* _frustumRenderModeAction;
QAction* _settingsAutosave; // Whether settings are saved automatically
SerialInterface _serialHeadSensor;
QNetworkAccessManager* _networkAccessManager;
SerialInterface _serialPort;
QSettings* _settings;
bool _displayLevels;
glm::vec3 _gravity;
@ -292,7 +292,6 @@ private:
int _bytesPerSecond;
int _bytesCount;
QSettings* _settings; // Contain Menu settings and Avatar data
bool _autosave; // True if the autosave is on.
};

8
interface/src/Avatar.cpp
View file

@ -353,7 +353,7 @@ void Avatar::updateFromMouse(int mouseX, int mouseY, int screenWidth, int scree
if (fabs(mouseLocationY) > MOUSE_MOVE_RADIUS) {
float mousePitchAdd = (fabs(mouseLocationY) - MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_PITCH_SPEED;
bool downPitching = (mouseLocationY > 0.f);
_head.setPitch(_head.getPitch() + (downPitching ? mousePitchAdd : -mousePitchAdd));
_head.setPitch(_head.getPitch() + (downPitching ? -mousePitchAdd : mousePitchAdd));
}
}
@ -1082,7 +1082,9 @@ void Avatar::updateBodyBalls(float deltaTime) {
if (_skeleton.joint[b].parent == AVATAR_JOINT_NULL || length < SMALL_SPRING_LENGTH) {
_bodyBall[b].rotation = orientation * _skeleton.joint[_bodyBall[b].parentJoint].absoluteBindPoseRotation;
} else {
_bodyBall[b].rotation = rotationBetween(jointDirection, springVector) * orientation;
glm::vec3 parentDirection = _bodyBall[ _skeleton.joint[b].parent ].rotation * JOINT_DIRECTION;
_bodyBall[b].rotation = rotationBetween(parentDirection, springVector) *
_bodyBall[ _skeleton.joint[b].parent ].rotation;
}
}
}
@ -1331,4 +1333,4 @@ void Avatar::renderJointConnectingCone(glm::vec3 position1, glm::vec3 position2,
}
glEnd();
}
}

26
interface/src/AvatarVoxelSystem.cpp
View file

@ -8,7 +8,6 @@
#include <cstring>
#include <QNetworkReply>
#include <QUrl>
#include <GeometryUtil.h>
@ -24,6 +23,9 @@ const int BONE_ELEMENTS_PER_VOXEL = BONE_ELEMENTS_PER_VERTEX * VERTICES_PER_VOXE
AvatarVoxelSystem::AvatarVoxelSystem(Avatar* avatar) :
VoxelSystem(AVATAR_TREE_SCALE, MAX_VOXELS_PER_AVATAR),
_avatar(avatar), _voxelReply(0) {
// we may have been created in the network thread, but we live in the main thread
moveToThread(Application::getInstance()->thread());
}
AvatarVoxelSystem::~AvatarVoxelSystem() {
@ -75,14 +77,23 @@ void AvatarVoxelSystem::removeOutOfView() {
// no-op for now
}
void AvatarVoxelSystem::loadVoxelsFromURL(const QUrl& url) {
void AvatarVoxelSystem::setVoxelURL(const QUrl& url) {
// don't restart the download if it's the same URL
if (_voxelURL == url) {
return;
}
// cancel any current download
if (_voxelReply != 0) {
delete _voxelReply;
_voxelReply = 0;
}
killLocalVoxels();
// remember the URL
_voxelURL = url;
// handle "file://" urls...
if (url.isLocalFile()) {
QString pathString = url.path();
@ -196,17 +207,11 @@ void AvatarVoxelSystem::handleVoxelDownloadProgress(qint64 bytesReceived, qint64
return;
}
// XXXBHG - I don't know why this can happen, but when I was testing this, I used a dropbox URL
// and it resulted in a case where the bytesTotal == bytesReceived, but the _voxelReply was NULL
// which needless to say caused crashes below. I've added this quick guard to protect against
// this case, but it probably should be investigated.
if (!_voxelReply) {
return;
}
QByteArray entirety = _voxelReply->readAll();
_voxelReply->disconnect(this);
_voxelReply->deleteLater();
_voxelReply = 0;
_tree->readBitstreamToTree((unsigned char*)entirety.data(), entirety.size(), WANT_COLOR, NO_EXISTS_BITS);
setupNewVoxelsForDrawing();
}
@ -214,6 +219,7 @@ void AvatarVoxelSystem::handleVoxelDownloadProgress(qint64 bytesReceived, qint64
void AvatarVoxelSystem::handleVoxelReplyError() {
printLog("%s\n", _voxelReply->errorString().toAscii().constData());
_voxelReply->disconnect(this);
_voxelReply->deleteLater();
_voxelReply = 0;
}

7
interface/src/AvatarVoxelSystem.h
View file

@ -10,6 +10,7 @@
#define __interface__AvatarVoxelSystem__
#include <QObject>
#include <QUrl>
#include "VoxelSystem.h"
@ -17,7 +18,6 @@ const int BONE_ELEMENTS_PER_VERTEX = 4;
typedef GLubyte BoneIndices[BONE_ELEMENTS_PER_VERTEX];
class QNetworkReply;
class QUrl;
class Avatar;
@ -33,7 +33,8 @@ public:
virtual void removeOutOfView();
void loadVoxelsFromURL(const QUrl& url);
Q_INVOKABLE void setVoxelURL(const QUrl& url);
const QUrl& getVoxelURL() const { return _voxelURL; }
protected:
@ -55,6 +56,8 @@ private:
Avatar* _avatar;
QUrl _voxelURL;
GLubyte* _readBoneIndicesArray;
GLfloat* _readBoneWeightsArray;
GLubyte* _writeBoneIndicesArray;

111
interface/src/SerialInterface.cpp
View file

@ -111,52 +111,61 @@ void SerialInterface::renderLevels(int width, int height) {
const int LEVEL_CORNER_Y = 200;
// Draw the numeric degree/sec values from the gyros
sprintf(val, "Yaw %4.1f", getLastYawRate());
sprintf(val, "Yaw %4.1f", _estimatedRotation.y);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "Pitch %4.1f", getLastPitchRate());
sprintf(val, "Pitch %4.1f", _estimatedRotation.x);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 15, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "Roll %4.1f", getLastRollRate());
sprintf(val, "Roll %4.1f", _estimatedRotation.z);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 30, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "X %4.3f", _lastAccelX);
sprintf(val, "X %4.3f", _lastAcceleration.x - _gravity.x);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 45, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "Y %4.3f", _lastAccelY);
sprintf(val, "Y %4.3f", _lastAcceleration.y - _gravity.y);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 60, 0.10, 0, 1.0, 1, val, 0, 1, 0);
sprintf(val, "Z %4.3f", _lastAccelZ);
sprintf(val, "Z %4.3f", _lastAcceleration.z - _gravity.z);
drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 75, 0.10, 0, 1.0, 1, val, 0, 1, 0);
// Draw the levels as horizontal lines
const int LEVEL_CENTER = 150;
const float ACCEL_VIEW_SCALING = 50.f;
const float ACCEL_VIEW_SCALING = 10.f;
const float POSITION_SCALING = 400.f;
glLineWidth(2.0);
glColor4f(1, 1, 1, 1);
glBegin(GL_LINES);
// Gyro rates
// Rotation rates
glColor4f(1, 1, 1, 1);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 3);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + getLastYawRate(), LEVEL_CORNER_Y - 3);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 12);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + getLastPitchRate(), LEVEL_CORNER_Y + 12);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 27);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + getLastRollRate(), LEVEL_CORNER_Y + 27);
// Gyro Estimated Rotation
// Estimated Rotation
glColor4f(0, 1, 1, 1);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 1);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.y, LEVEL_CORNER_Y - 1);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 14);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.z, LEVEL_CORNER_Y + 14);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.x, LEVEL_CORNER_Y + 14);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 29);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.x, LEVEL_CORNER_Y + 29);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _estimatedRotation.z, LEVEL_CORNER_Y + 29);
// Acceleration
// Acceleration rates
glColor4f(1, 1, 1, 1);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 42);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelX - _gravity.x)* ACCEL_VIEW_SCALING),
LEVEL_CORNER_Y + 42);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.x - _gravity.x) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 42);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 57);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelY - _gravity.y) * ACCEL_VIEW_SCALING),
LEVEL_CORNER_Y + 57);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.y - _gravity.y) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 57);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 72);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelZ - _gravity.z) * ACCEL_VIEW_SCALING),
LEVEL_CORNER_Y + 72);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.z - _gravity.z) * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 72);
// Estimated Position
glColor4f(0, 1, 1, 1);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 44);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedPosition.x * POSITION_SCALING), LEVEL_CORNER_Y + 44);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 59);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedPosition.y * POSITION_SCALING), LEVEL_CORNER_Y + 59);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 74);
glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedPosition.z * POSITION_SCALING), LEVEL_CORNER_Y + 74);
glEnd();
// Draw green vertical centerline
@ -202,10 +211,8 @@ void SerialInterface::readData(float deltaTime) {
// From MPU-9150 register map, with setting on
// highest resolution = +/- 2G
_lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND2;
_lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND2;
_lastAccelZ = ((float) -accelZRate) * LSB_TO_METERS_PER_SECOND2;
_lastAcceleration = glm::vec3(accelXRate, accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
int rollRate, yawRate, pitchRate;
convertHexToInt(sensorBuffer + 22, rollRate);
@ -213,38 +220,41 @@ void SerialInterface::readData(float deltaTime) {
convertHexToInt(sensorBuffer + 30, pitchRate);
// Convert the integer rates to floats
const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec.
_lastRollRate = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
_lastYawRate = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
_lastPitchRate = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec.
_lastRotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
_lastRotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
_lastRotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
// Update raw rotation estimates
_estimatedRotation += deltaTime * glm::vec3(_lastRollRate - _averageGyroRates[0],
_lastYawRate - _averageGyroRates[1],
_lastPitchRate - _averageGyroRates[2]);
_estimatedRotation += deltaTime * (_lastRotationRates - _averageRotationRates);
// Update estimated position and velocity
float const DECAY_VELOCITY = 0.95f;
float const DECAY_POSITION = 0.95f;
_estimatedVelocity += deltaTime * (_lastAcceleration - _averageAcceleration);
_estimatedPosition += deltaTime * _estimatedVelocity;
_estimatedVelocity *= DECAY_VELOCITY;
_estimatedPosition *= DECAY_POSITION;
//glm::vec3 baseline = glm::normalize(_gravity);
//glm::vec3 current = glm::normalize(_lastAcceleration);
// Accumulate a set of initial baseline readings for setting gravity
if (totalSamples == 0) {
_averageGyroRates[0] = _lastRollRate;
_averageGyroRates[1] = _lastYawRate;
_averageGyroRates[2] = _lastPitchRate;
_gravity.x = _lastAccelX;
_gravity.y = _lastAccelY;
_gravity.z = _lastAccelZ;
_averageRotationRates = _lastRotationRates;
_averageAcceleration = _lastAcceleration;
_gravity = _lastAcceleration;
}
else {
// Cumulate long term average to (hopefully) take DC bias out of rotation rates
_averageGyroRates[0] = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[0] +
1.f/(float)LONG_TERM_RATE_SAMPLES * _lastRollRate;
_averageGyroRates[1] = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[1] +
1.f/(float)LONG_TERM_RATE_SAMPLES * _lastYawRate;
_averageGyroRates[2] = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageGyroRates[2] +
1.f/(float)LONG_TERM_RATE_SAMPLES * _lastPitchRate;
_averageRotationRates = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates
+ 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastRotationRates;
_averageAcceleration = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration
+ 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastAcceleration;
if (totalSamples < GRAVITY_SAMPLES) {
_gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity +
1.f/(float)GRAVITY_SAMPLES * glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ);
1.f/(float)GRAVITY_SAMPLES * _lastAcceleration;
}
}
@ -268,7 +278,12 @@ void SerialInterface::readData(float deltaTime) {
void SerialInterface::resetAverages() {
totalSamples = 0;
_gravity = glm::vec3(0, 0, 0);
_averageGyroRates = glm::vec3(0, 0, 0);
_averageRotationRates = glm::vec3(0, 0, 0);
_averageAcceleration = glm::vec3(0, 0, 0);
_lastRotationRates = glm::vec3(0, 0, 0);
_estimatedRotation = glm::vec3(0, 0, 0);
_estimatedPosition = glm::vec3(0, 0, 0);
_estimatedVelocity = glm::vec3(0, 0, 0);
}
void SerialInterface::resetSerial() {

56
interface/src/SerialInterface.h
View file

@ -20,41 +20,32 @@
#include <dirent.h>
#endif
#define NUM_CHANNELS 6
// Acceleration sensors, in screen/world coord system (X = left/right, Y = Up/Down, Z = fwd/back)
#define ACCEL_X 3
#define ACCEL_Y 4
#define ACCEL_Z 5
// Gyro sensors, in coodinate system of head/airplane
#define HEAD_PITCH_RATE 1
#define HEAD_YAW_RATE 0
#define HEAD_ROLL_RATE 2
extern const bool USING_INVENSENSE_MPU9150;
class SerialInterface {
public:
SerialInterface() : active(false),
_gravity(0,0,0),
_averageGyroRates(0, 0, 0),
_gravity(0, 0, 0),
_averageRotationRates(0, 0, 0),
_averageAcceleration(0, 0, 0),
_estimatedRotation(0, 0, 0),
_lastAccelX(0),
_lastAccelY(0),
_lastAccelZ(0),
_lastYawRate(0),
_lastPitchRate(0),
_lastRollRate(0) {}
_estimatedPosition(0, 0, 0),
_estimatedVelocity(0, 0, 0),
_lastAcceleration(0, 0, 0),
_lastRotationRates(0, 0, 0)
{}
void pair();
void readData(float deltaTime);
float getLastYawRate() const { return _lastYawRate - _averageGyroRates[1]; }
float getLastPitchRate() const { return _lastPitchRate - _averageGyroRates[2]; }
float getLastRollRate() const { return _lastRollRate - _averageGyroRates[0]; }
glm::vec3 getLastAcceleration() { return glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ); };
glm::vec3 getGravity() {return _gravity;};
const float getLastPitchRate() const { return _lastRotationRates[0] - _averageRotationRates[0]; }
const float getLastYawRate() const { return _lastRotationRates[1] - _averageRotationRates[1]; }
const float getLastRollRate() const { return _lastRotationRates[2] - _averageRotationRates[2]; }
const glm::vec3& getLastRotationRates() const { return _lastRotationRates; };
const glm::vec3& getEstimatedRotation() const { return _estimatedRotation; };
const glm::vec3& getEstimatedPosition() const { return _estimatedPosition; };
const glm::vec3& getEstimatedVelocity() const { return _estimatedVelocity; };
const glm::vec3& getLastAcceleration() const { return _lastAcceleration; };
const glm::vec3& getGravity() const { return _gravity; };
void renderLevels(int width, int height);
void resetAverages();
@ -68,14 +59,13 @@ private:
int totalSamples;
timeval lastGoodRead;
glm::vec3 _gravity;
glm::vec3 _averageGyroRates;
glm::vec3 _averageRotationRates;
glm::vec3 _averageAcceleration;
glm::vec3 _estimatedRotation;
float _lastAccelX;
float _lastAccelY;
float _lastAccelZ;
float _lastYawRate; // Rates are in degrees per second.
float _lastPitchRate;
float _lastRollRate;
glm::vec3 _estimatedPosition;
glm::vec3 _estimatedVelocity;
glm::vec3 _lastAcceleration;
glm::vec3 _lastRotationRates;
};
#endif

4
interface/src/Skeleton.cpp
View file

@ -118,7 +118,9 @@ void Skeleton::initialize() {
} else {
joint[b].absoluteBindPosePosition = joint[ joint[b].parent ].absoluteBindPosePosition +
joint[b].bindPosePosition;
joint[b].absoluteBindPoseRotation = rotationBetween(JOINT_DIRECTION, joint[b].bindPosePosition);
glm::vec3 parentDirection = joint[ joint[b].parent ].absoluteBindPoseRotation * JOINT_DIRECTION;
joint[b].absoluteBindPoseRotation = rotationBetween(parentDirection, joint[b].bindPosePosition) *
joint[ joint[b].parent ].absoluteBindPoseRotation;
}
}
}

2
interface/src/Skeleton.h
View file

@ -42,7 +42,7 @@ enum AvatarJointID
NUM_AVATAR_JOINTS
};
const glm::vec3 JOINT_DIRECTION = glm::vec3(1.0f, 0.0f, 0.0f);
const glm::vec3 JOINT_DIRECTION = glm::vec3(0.0f, 1.0f, 0.0f);
class Skeleton {
public:

1
libraries/shared/src/PacketHeaders.h
View file

@ -27,6 +27,7 @@ const PACKET_HEADER PACKET_HEADER_ERASE_VOXEL = 'E';
const PACKET_HEADER PACKET_HEADER_VOXEL_DATA = 'V';
const PACKET_HEADER PACKET_HEADER_VOXEL_DATA_MONOCHROME = 'v';
const PACKET_HEADER PACKET_HEADER_BULK_AVATAR_DATA = 'X';
const PACKET_HEADER PACKET_HEADER_AVATAR_VOXEL_URL = 'U';
const PACKET_HEADER PACKET_HEADER_TRANSMITTER_DATA_V2 = 'T';
const PACKET_HEADER PACKET_HEADER_ENVIRONMENT_DATA = 'e';
const PACKET_HEADER PACKET_HEADER_DOMAIN_LIST_REQUEST = 'L';