diff --git a/assignment-client/src/audio/AudioMixer.cpp b/assignment-client/src/audio/AudioMixer.cpp index 2dc51b44a0..8e4ce04f0b 100644 --- a/assignment-client/src/audio/AudioMixer.cpp +++ b/assignment-client/src/audio/AudioMixer.cpp @@ -54,9 +54,6 @@ #include "AudioMixer.h" -const short JITTER_BUFFER_MSECS = 12; -const short JITTER_BUFFER_SAMPLES = JITTER_BUFFER_MSECS * (SAMPLE_RATE / 1000.0); - const float LOUDNESS_TO_DISTANCE_RATIO = 0.00001f; const QString AUDIO_MIXER_LOGGING_TARGET_NAME = "audio-mixer"; @@ -67,6 +64,8 @@ void attachNewBufferToNode(Node *newNode) { } } +bool AudioMixer::_useDynamicJitterBuffers = false; + AudioMixer::AudioMixer(const QByteArray& packet) : ThreadedAssignment(packet), _trailingSleepRatio(1.0f), @@ -427,12 +426,46 @@ void AudioMixer::sendStatsPacket() { } else { statsObject["average_mixes_per_listener"] = 0.0; } - + ThreadedAssignment::addPacketStatsAndSendStatsPacket(statsObject); - _sumListeners = 0; _sumMixes = 0; _numStatFrames = 0; + + + // NOTE: These stats can be too large to fit in an MTU, so we break it up into multiple packts... + QJsonObject statsObject2; + + // add stats for each listerner + bool somethingToSend = false; + int sizeOfStats = 0; + int TOO_BIG_FOR_MTU = 1200; // some extra space for JSONification + + NodeList* nodeList = NodeList::getInstance(); + int clientNumber = 0; + foreach (const SharedNodePointer& node, nodeList->getNodeHash()) { + clientNumber++; + AudioMixerClientData* clientData = static_cast(node->getLinkedData()); + if (clientData) { + QString property = "jitterStats." + node->getUUID().toString(); + QString value = clientData->getJitterBufferStats(); + statsObject2[qPrintable(property)] = value; + somethingToSend = true; + sizeOfStats += property.size() + value.size(); + } + + // if we're too large, send the packet + if (sizeOfStats > TOO_BIG_FOR_MTU) { + nodeList->sendStatsToDomainServer(statsObject2); + sizeOfStats = 0; + statsObject2 = QJsonObject(); // clear it + somethingToSend = false; + } + } + + if (somethingToSend) { + nodeList->sendStatsToDomainServer(statsObject2); + } } void AudioMixer::run() { @@ -471,6 +504,16 @@ void AudioMixer::run() { << QString("%1, %2, %3").arg(destinationCenter.x).arg(destinationCenter.y).arg(destinationCenter.z); } + // check the payload to see if we have asked for dynamicJitterBuffer support + const QString DYNAMIC_JITTER_BUFFER_REGEX_STRING = "--dynamicJitterBuffer"; + QRegExp dynamicJitterBufferMatch(DYNAMIC_JITTER_BUFFER_REGEX_STRING); + if (dynamicJitterBufferMatch.indexIn(_payload) != -1) { + qDebug() << "Enable dynamic jitter buffers."; + _useDynamicJitterBuffers = true; + } else { + qDebug() << "Dynamic jitter buffers disabled, using old behavior."; + } + int nextFrame = 0; QElapsedTimer timer; timer.start(); @@ -487,8 +530,7 @@ void AudioMixer::run() { foreach (const SharedNodePointer& node, nodeList->getNodeHash()) { if (node->getLinkedData()) { - ((AudioMixerClientData*) node->getLinkedData())->checkBuffersBeforeFrameSend(JITTER_BUFFER_SAMPLES, - _sourceUnattenuatedZone, + ((AudioMixerClientData*) node->getLinkedData())->checkBuffersBeforeFrameSend(_sourceUnattenuatedZone, _listenerUnattenuatedZone); } } diff --git a/assignment-client/src/audio/AudioMixer.h b/assignment-client/src/audio/AudioMixer.h index 39f8cf63ae..8560978689 100644 --- a/assignment-client/src/audio/AudioMixer.h +++ b/assignment-client/src/audio/AudioMixer.h @@ -34,6 +34,9 @@ public slots: void readPendingDatagrams(); void sendStatsPacket(); + + static bool getUseDynamicJitterBuffers() { return _useDynamicJitterBuffers; } + private: /// adds one buffer to the mix for a listening node void addBufferToMixForListeningNodeWithBuffer(PositionalAudioRingBuffer* bufferToAdd, @@ -54,6 +57,7 @@ private: int _sumMixes; AABox* _sourceUnattenuatedZone; AABox* _listenerUnattenuatedZone; + static bool _useDynamicJitterBuffers; }; #endif // hifi_AudioMixer_h diff --git a/assignment-client/src/audio/AudioMixerClientData.cpp b/assignment-client/src/audio/AudioMixerClientData.cpp index 2f78a4ac78..395cebf9c2 100644 --- a/assignment-client/src/audio/AudioMixerClientData.cpp +++ b/assignment-client/src/audio/AudioMixerClientData.cpp @@ -16,6 +16,7 @@ #include "InjectedAudioRingBuffer.h" +#include "AudioMixer.h" #include "AudioMixerClientData.h" AudioMixerClientData::AudioMixerClientData() : @@ -65,7 +66,7 @@ int AudioMixerClientData::parseData(const QByteArray& packet) { if (!avatarRingBuffer) { // we don't have an AvatarAudioRingBuffer yet, so add it - avatarRingBuffer = new AvatarAudioRingBuffer(isStereo); + avatarRingBuffer = new AvatarAudioRingBuffer(isStereo, AudioMixer::getUseDynamicJitterBuffers()); _ringBuffers.push_back(avatarRingBuffer); } @@ -88,7 +89,8 @@ int AudioMixerClientData::parseData(const QByteArray& packet) { if (!matchingInjectedRingBuffer) { // we don't have a matching injected audio ring buffer, so add it - matchingInjectedRingBuffer = new InjectedAudioRingBuffer(streamIdentifier); + matchingInjectedRingBuffer = new InjectedAudioRingBuffer(streamIdentifier, + AudioMixer::getUseDynamicJitterBuffers()); _ringBuffers.push_back(matchingInjectedRingBuffer); } @@ -98,10 +100,9 @@ int AudioMixerClientData::parseData(const QByteArray& packet) { return 0; } -void AudioMixerClientData::checkBuffersBeforeFrameSend(int jitterBufferLengthSamples, - AABox* checkSourceZone, AABox* listenerZone) { +void AudioMixerClientData::checkBuffersBeforeFrameSend(AABox* checkSourceZone, AABox* listenerZone) { for (int i = 0; i < _ringBuffers.size(); i++) { - if (_ringBuffers[i]->shouldBeAddedToMix(jitterBufferLengthSamples)) { + if (_ringBuffers[i]->shouldBeAddedToMix()) { // this is a ring buffer that is ready to go // set its flag so we know to push its buffer when all is said and done _ringBuffers[i]->setWillBeAddedToMix(true); @@ -120,20 +121,62 @@ void AudioMixerClientData::checkBuffersBeforeFrameSend(int jitterBufferLengthSam } void AudioMixerClientData::pushBuffersAfterFrameSend() { - for (int i = 0; i < _ringBuffers.size(); i++) { + + QList::iterator i = _ringBuffers.begin(); + while (i != _ringBuffers.end()) { // this was a used buffer, push the output pointer forwards - PositionalAudioRingBuffer* audioBuffer = _ringBuffers[i]; + PositionalAudioRingBuffer* audioBuffer = *i; if (audioBuffer->willBeAddedToMix()) { - audioBuffer->shiftReadPosition(audioBuffer->isStereo() - ? NETWORK_BUFFER_LENGTH_SAMPLES_STEREO : NETWORK_BUFFER_LENGTH_SAMPLES_PER_CHANNEL); - + audioBuffer->shiftReadPosition(audioBuffer->getSamplesPerFrame()); audioBuffer->setWillBeAddedToMix(false); } else if (audioBuffer->getType() == PositionalAudioRingBuffer::Injector && audioBuffer->hasStarted() && audioBuffer->isStarved()) { // this is an empty audio buffer that has starved, safe to delete delete audioBuffer; - _ringBuffers.erase(_ringBuffers.begin() + i); + i = _ringBuffers.erase(i); + continue; } + i++; } } + +QString AudioMixerClientData::getJitterBufferStats() const { + QString result; + AvatarAudioRingBuffer* avatarRingBuffer = getAvatarAudioRingBuffer(); + if (avatarRingBuffer) { + int desiredJitterBuffer = avatarRingBuffer->getDesiredJitterBufferFrames(); + int calculatedJitterBuffer = avatarRingBuffer->getCalculatedDesiredJitterBufferFrames(); + int currentJitterBuffer = avatarRingBuffer->getCurrentJitterBufferFrames(); + int resetCount = avatarRingBuffer->getResetCount(); + int samplesAvailable = avatarRingBuffer->samplesAvailable(); + int framesAvailable = (samplesAvailable / avatarRingBuffer->getSamplesPerFrame()); + result += "mic.desired:" + QString::number(desiredJitterBuffer) + + " calculated:" + QString::number(calculatedJitterBuffer) + + " current:" + QString::number(currentJitterBuffer) + + " available:" + QString::number(framesAvailable) + + " samples:" + QString::number(samplesAvailable) + + " resets:" + QString::number(resetCount); + } else { + result = "mic unknown"; + } + + for (int i = 0; i < _ringBuffers.size(); i++) { + if (_ringBuffers[i]->getType() == PositionalAudioRingBuffer::Injector) { + int desiredJitterBuffer = _ringBuffers[i]->getDesiredJitterBufferFrames(); + int calculatedJitterBuffer = _ringBuffers[i]->getCalculatedDesiredJitterBufferFrames(); + int currentJitterBuffer = _ringBuffers[i]->getCurrentJitterBufferFrames(); + int resetCount = _ringBuffers[i]->getResetCount(); + int samplesAvailable = _ringBuffers[i]->samplesAvailable(); + int framesAvailable = (samplesAvailable / _ringBuffers[i]->getSamplesPerFrame()); + result += "| injected["+QString::number(i)+"].desired:" + QString::number(desiredJitterBuffer) + + " calculated:" + QString::number(calculatedJitterBuffer) + + " current:" + QString::number(currentJitterBuffer) + + " available:" + QString::number(framesAvailable) + + " samples:" + QString::number(samplesAvailable) + + " resets:" + QString::number(resetCount); + } + } + + return result; +} diff --git a/assignment-client/src/audio/AudioMixerClientData.h b/assignment-client/src/audio/AudioMixerClientData.h index e52b09e134..1760b96f5d 100644 --- a/assignment-client/src/audio/AudioMixerClientData.h +++ b/assignment-client/src/audio/AudioMixerClientData.h @@ -27,9 +27,11 @@ public: AvatarAudioRingBuffer* getAvatarAudioRingBuffer() const; int parseData(const QByteArray& packet); - void checkBuffersBeforeFrameSend(int jitterBufferLengthSamples, - AABox* checkSourceZone = NULL, AABox* listenerZone = NULL); + void checkBuffersBeforeFrameSend(AABox* checkSourceZone = NULL, AABox* listenerZone = NULL); void pushBuffersAfterFrameSend(); + + QString getJitterBufferStats() const; + private: QList _ringBuffers; }; diff --git a/assignment-client/src/audio/AvatarAudioRingBuffer.cpp b/assignment-client/src/audio/AvatarAudioRingBuffer.cpp index 5613a64cc4..9c6cc32f57 100644 --- a/assignment-client/src/audio/AvatarAudioRingBuffer.cpp +++ b/assignment-client/src/audio/AvatarAudioRingBuffer.cpp @@ -13,12 +13,15 @@ #include "AvatarAudioRingBuffer.h" -AvatarAudioRingBuffer::AvatarAudioRingBuffer(bool isStereo) : - PositionalAudioRingBuffer(PositionalAudioRingBuffer::Microphone, isStereo) { +AvatarAudioRingBuffer::AvatarAudioRingBuffer(bool isStereo, bool dynamicJitterBuffer) : + PositionalAudioRingBuffer(PositionalAudioRingBuffer::Microphone, isStereo, dynamicJitterBuffer) { } int AvatarAudioRingBuffer::parseData(const QByteArray& packet) { + _interframeTimeGapStats.frameReceived(); + updateDesiredJitterBufferFrames(); + _shouldLoopbackForNode = (packetTypeForPacket(packet) == PacketTypeMicrophoneAudioWithEcho); return PositionalAudioRingBuffer::parseData(packet); } diff --git a/assignment-client/src/audio/AvatarAudioRingBuffer.h b/assignment-client/src/audio/AvatarAudioRingBuffer.h index f842c2aa33..e227e70958 100644 --- a/assignment-client/src/audio/AvatarAudioRingBuffer.h +++ b/assignment-client/src/audio/AvatarAudioRingBuffer.h @@ -18,7 +18,7 @@ class AvatarAudioRingBuffer : public PositionalAudioRingBuffer { public: - AvatarAudioRingBuffer(bool isStereo = false); + AvatarAudioRingBuffer(bool isStereo = false, bool dynamicJitterBuffer = false); int parseData(const QByteArray& packet); private: diff --git a/examples/editModels.js b/examples/editModels.js index 5959b6eef5..fa000888ca 100644 --- a/examples/editModels.js +++ b/examples/editModels.js @@ -34,6 +34,7 @@ var LASER_COLOR = { red: 255, green: 0, blue: 0 }; var LASER_LENGTH_FACTOR = 500 ; +var MIN_ANGULAR_SIZE = 2; var MAX_ANGULAR_SIZE = 45; var LEFT = 0; @@ -277,8 +278,9 @@ function controller(wichSide) { var X = Vec3.sum(A, Vec3.multiply(B, x)); var d = Vec3.length(Vec3.subtract(P, X)); - if (0 < x && x < LASER_LENGTH_FACTOR) { - if (2 * Math.atan(properties.radius / Vec3.distance(Camera.getPosition(), properties.position)) * 180 / 3.14 > MAX_ANGULAR_SIZE) { + var angularSize = 2 * Math.atan(properties.radius / Vec3.distance(Camera.getPosition(), properties.position)) * 180 / 3.14; + if (0 < x && angularSize > MIN_ANGULAR_SIZE) { + if (angularSize > MAX_ANGULAR_SIZE) { print("Angular size too big: " + 2 * Math.atan(properties.radius / Vec3.distance(Camera.getPosition(), properties.position)) * 180 / 3.14); return { valid: false }; } @@ -326,7 +328,8 @@ function controller(wichSide) { origin: this.palmPosition, direction: this.front }); - if (intersection.accurate && intersection.modelID.isKnownID) { + var angularSize = 2 * Math.atan(intersection.modelProperties.radius / Vec3.distance(Camera.getPosition(), intersection.modelProperties.position)) * 180 / 3.14; + if (intersection.accurate && intersection.modelID.isKnownID && angularSize > MIN_ANGULAR_SIZE && angularSize < MAX_ANGULAR_SIZE) { this.glowedIntersectingModel = intersection.modelID; Models.editModel(this.glowedIntersectingModel, { glowLevel: 0.25 }); } @@ -828,8 +831,9 @@ function mousePressEvent(event) { var X = Vec3.sum(A, Vec3.multiply(B, x)); var d = Vec3.length(Vec3.subtract(P, X)); - if (0 < x && x < LASER_LENGTH_FACTOR) { - if (2 * Math.atan(properties.radius / Vec3.distance(Camera.getPosition(), properties.position)) * 180 / 3.14 < MAX_ANGULAR_SIZE) { + var angularSize = 2 * Math.atan(properties.radius / Vec3.distance(Camera.getPosition(), properties.position)) * 180 / 3.14; + if (0 < x && angularSize > MIN_ANGULAR_SIZE) { + if (angularSize < MAX_ANGULAR_SIZE) { modelSelected = true; selectedModelID = foundModel; selectedModelProperties = properties; @@ -884,7 +888,8 @@ function mouseMoveEvent(event) { glowedModelID.isKnownID = false; } - if (modelIntersection.modelID.isKnownID) { + var angularSize = 2 * Math.atan(modelIntersection.modelProperties.radius / Vec3.distance(Camera.getPosition(), modelIntersection.modelProperties.position)) * 180 / 3.14; + if (modelIntersection.modelID.isKnownID && angularSize > MIN_ANGULAR_SIZE && angularSize < MAX_ANGULAR_SIZE) { Models.editModel(modelIntersection.modelID, { glowLevel: 0.25 }); glowedModelID = modelIntersection.modelID; } @@ -1125,8 +1130,8 @@ function handeMenuEvent(menuItem){ Models.editModel(editModelID, properties); } } - tooltip.show(false); } + tooltip.show(false); } Menu.menuItemEvent.connect(handeMenuEvent); diff --git a/examples/editVoxels.js b/examples/editVoxels.js index cff0d65743..412612fdad 100644 --- a/examples/editVoxels.js +++ b/examples/editVoxels.js @@ -1196,7 +1196,7 @@ function menuItemEvent(menuItem) { print("deleting..."); if (isImporting) { cancelImport(); - } else { + } else if (voxelToolSelected) { Clipboard.deleteVoxel(selectedVoxel.x, selectedVoxel.y, selectedVoxel.z, selectedVoxel.s); } } diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 75c3f442ca..d7e3b7453d 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -3189,7 +3189,7 @@ void Application::updateWindowTitle(){ float creditBalance = accountManager.getAccountInfo().getBalance() / SATOSHIS_PER_CREDIT; QString creditBalanceString; - creditBalanceString.sprintf("%.8f", creditBalance); + creditBalanceString.sprintf("%.8f", floor(creditBalance + 0.5)); title += " - ₵" + creditBalanceString; } diff --git a/interface/src/Audio.cpp b/interface/src/Audio.cpp index 271bcd5279..65912f83e8 100644 --- a/interface/src/Audio.cpp +++ b/interface/src/Audio.cpp @@ -218,9 +218,14 @@ QAudioDeviceInfo defaultAudioDeviceForMode(QAudio::Mode mode) { hr = pPropertyStore->GetValue(PKEY_Device_FriendlyName, &pv); pPropertyStore->Release(); pPropertyStore = NULL; - //QAudio devices seems to only take the 31 first characters of the Friendly Device Name. - const DWORD QT_WIN_MAX_AUDIO_DEVICENAME_LEN = 31; - deviceName = QString::fromWCharArray((wchar_t*)pv.pwszVal).left(QT_WIN_MAX_AUDIO_DEVICENAME_LEN); + deviceName = QString::fromWCharArray((wchar_t*)pv.pwszVal); + const DWORD WINDOWS7_MAJOR_VERSION = 6; + const DWORD WINDOWS7_MINOR_VERSION = 1; + if (osvi.dwMajorVersion <= WINDOWS7_MAJOR_VERSION && osvi.dwMinorVersion <= WINDOWS7_MINOR_VERSION) { + // Windows 7 provides only the 31 first characters of the device name. + const DWORD QT_WIN7_MAX_AUDIO_DEVICENAME_LEN = 31; + deviceName = deviceName.left(QT_WIN7_MAX_AUDIO_DEVICENAME_LEN); + } qDebug() << (mode == QAudio::AudioOutput ? "output" : "input") << " device:" << deviceName; PropVariantClear(&pv); } @@ -461,8 +466,8 @@ void Audio::handleAudioInput() { int16_t* inputAudioSamples = new int16_t[inputSamplesRequired]; _inputRingBuffer.readSamples(inputAudioSamples, inputSamplesRequired); - int numNetworkBytes = _isStereoInput ? NETWORK_BUFFER_LENGTH_BYTES_STEREO : NETWORK_BUFFER_LENGTH_BYTES_PER_CHANNEL; - int numNetworkSamples = _isStereoInput ? NETWORK_BUFFER_LENGTH_SAMPLES_STEREO : NETWORK_BUFFER_LENGTH_SAMPLES_PER_CHANNEL; + const int numNetworkBytes = _isStereoInput ? NETWORK_BUFFER_LENGTH_BYTES_STEREO : NETWORK_BUFFER_LENGTH_BYTES_PER_CHANNEL; + const int numNetworkSamples = _isStereoInput ? NETWORK_BUFFER_LENGTH_SAMPLES_STEREO : NETWORK_BUFFER_LENGTH_SAMPLES_PER_CHANNEL; // zero out the monoAudioSamples array and the locally injected audio memset(networkAudioSamples, 0, numNetworkBytes); @@ -634,12 +639,10 @@ void Audio::handleAudioInput() { packetType = PacketTypeSilentAudioFrame; // we need to indicate how many silent samples this is to the audio mixer - audioDataPacket[0] = _isStereoInput - ? NETWORK_BUFFER_LENGTH_SAMPLES_STEREO - : NETWORK_BUFFER_LENGTH_SAMPLES_PER_CHANNEL; + networkAudioSamples[0] = numNetworkSamples; numAudioBytes = sizeof(int16_t); } else { - numAudioBytes = _isStereoInput ? NETWORK_BUFFER_LENGTH_BYTES_STEREO : NETWORK_BUFFER_LENGTH_BYTES_PER_CHANNEL; + numAudioBytes = numNetworkBytes; if (Menu::getInstance()->isOptionChecked(MenuOption::EchoServerAudio)) { packetType = PacketTypeMicrophoneAudioWithEcho; diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 5c8c2e97aa..7d4c7d109d 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -249,12 +249,21 @@ Menu::Menu() : QMenu* viewMenu = addMenu("View"); +#ifdef Q_OS_MAC addCheckableActionToQMenuAndActionHash(viewMenu, MenuOption::Fullscreen, Qt::CTRL | Qt::META | Qt::Key_F, false, appInstance, SLOT(setFullscreen(bool))); +#else + addCheckableActionToQMenuAndActionHash(viewMenu, + MenuOption::Fullscreen, + Qt::CTRL | Qt::Key_F, + false, + appInstance, + SLOT(setFullscreen(bool))); +#endif addCheckableActionToQMenuAndActionHash(viewMenu, MenuOption::FirstPerson, Qt::Key_P, true, appInstance,SLOT(cameraMenuChanged())); addCheckableActionToQMenuAndActionHash(viewMenu, MenuOption::Mirror, Qt::SHIFT | Qt::Key_H, true); @@ -366,7 +375,7 @@ Menu::Menu() : addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::RenderSkeletonCollisionShapes); addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::RenderHeadCollisionShapes); addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::RenderBoundingCollisionShapes); - addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::CollideAsRagDoll); + addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::CollideAsRagdoll); addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::LookAtVectors, 0, false); addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, @@ -1293,6 +1302,7 @@ void Menu::showChat() { if (_chatWindow->isHidden()) { _chatWindow->show(); } + _chatWindow->activateWindow(); } else { Application::getInstance()->getTrayIcon()->showMessage("Interface", "You need to login to be able to chat with others on this domain."); } diff --git a/interface/src/Menu.h b/interface/src/Menu.h index 4d2174a448..5b479773b6 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -322,7 +322,7 @@ namespace MenuOption { const QString CascadedShadows = "Cascaded"; const QString Chat = "Chat..."; const QString ChatCircling = "Chat Circling"; - const QString CollideAsRagDoll = "Collide As RagDoll"; + const QString CollideAsRagdoll = "Collide As Ragdoll"; const QString CollideWithAvatars = "Collide With Avatars"; const QString CollideWithEnvironment = "Collide With World Boundaries"; const QString CollideWithParticles = "Collide With Particles"; diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp index baf46605fd..57452f687f 100644 --- a/interface/src/avatar/Avatar.cpp +++ b/interface/src/avatar/Avatar.cpp @@ -218,9 +218,6 @@ void Avatar::render(const glm::vec3& cameraPosition, RenderMode renderMode) { bool renderSkeleton = Menu::getInstance()->isOptionChecked(MenuOption::RenderSkeletonCollisionShapes); bool renderHead = Menu::getInstance()->isOptionChecked(MenuOption::RenderHeadCollisionShapes); bool renderBounding = Menu::getInstance()->isOptionChecked(MenuOption::RenderBoundingCollisionShapes); - if (renderSkeleton || renderHead || renderBounding) { - updateShapePositions(); - } if (renderSkeleton) { _skeletonModel.renderJointCollisionShapes(0.7f); @@ -230,7 +227,6 @@ void Avatar::render(const glm::vec3& cameraPosition, RenderMode renderMode) { getHead()->getFaceModel().renderJointCollisionShapes(0.7f); } if (renderBounding && shouldRenderHead(cameraPosition, renderMode)) { - getHead()->getFaceModel().renderBoundingCollisionShapes(0.7f); _skeletonModel.renderBoundingCollisionShapes(0.7f); } @@ -579,10 +575,9 @@ bool Avatar::findRayIntersection(const glm::vec3& origin, const glm::vec3& direc return false; } -bool Avatar::findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius, - CollisionList& collisions, int skeletonSkipIndex) { - return _skeletonModel.findSphereCollisions(penetratorCenter, penetratorRadius, collisions, skeletonSkipIndex); - // Temporarily disabling collisions against the head because most of its collision proxies are bad. +bool Avatar::findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius, CollisionList& collisions) { + return _skeletonModel.findSphereCollisions(penetratorCenter, penetratorRadius, collisions); + // TODO: Andrew to fix: Temporarily disabling collisions against the head //return getHead()->getFaceModel().findSphereCollisions(penetratorCenter, penetratorRadius, collisions); } @@ -591,18 +586,6 @@ bool Avatar::findPlaneCollisions(const glm::vec4& plane, CollisionList& collisio getHead()->getFaceModel().findPlaneCollisions(plane, collisions); } -void Avatar::updateShapePositions() { - _skeletonModel.updateShapePositions(); - Model& headModel = getHead()->getFaceModel(); - headModel.updateShapePositions(); - /* KEEP FOR DEBUG: use this in rather than code above to see shapes - * in their default positions where the bounding shape is computed. - _skeletonModel.resetShapePositions(); - Model& headModel = getHead()->getFaceModel(); - headModel.resetShapePositions(); - */ -} - bool Avatar::findCollisions(const QVector& shapes, CollisionList& collisions) { // TODO: Andrew to fix: also collide against _skeleton //bool collided = _skeletonModel.findCollisions(shapes, collisions); @@ -613,69 +596,6 @@ bool Avatar::findCollisions(const QVector& shapes, CollisionList& return collided; } -bool Avatar::findParticleCollisions(const glm::vec3& particleCenter, float particleRadius, CollisionList& collisions) { - if (_collisionGroups & COLLISION_GROUP_PARTICLES) { - return false; - } - bool collided = false; - // first do the hand collisions - const HandData* handData = getHandData(); - if (handData) { - for (int i = 0; i < NUM_HANDS; i++) { - const PalmData* palm = handData->getPalm(i); - if (palm && palm->hasPaddle()) { - // create a disk collision proxy where the hand is - int jointIndex = -1; - glm::vec3 handPosition; - if (i == 0) { - _skeletonModel.getLeftHandPosition(handPosition); - jointIndex = _skeletonModel.getLeftHandJointIndex(); - } - else { - _skeletonModel.getRightHandPosition(handPosition); - jointIndex = _skeletonModel.getRightHandJointIndex(); - } - - glm::vec3 fingerAxis = palm->getFingerDirection(); - glm::vec3 diskCenter = handPosition + HAND_PADDLE_OFFSET * fingerAxis; - glm::vec3 diskNormal = palm->getNormal(); - const float DISK_THICKNESS = 0.08f; - - // collide against the disk - glm::vec3 penetration; - if (findSphereDiskPenetration(particleCenter, particleRadius, - diskCenter, HAND_PADDLE_RADIUS, DISK_THICKNESS, diskNormal, - penetration)) { - CollisionInfo* collision = collisions.getNewCollision(); - if (collision) { - collision->_type = COLLISION_TYPE_PADDLE_HAND; - collision->_intData = jointIndex; - collision->_penetration = penetration; - collision->_addedVelocity = palm->getVelocity(); - collided = true; - } else { - // collisions are full, so we might as well bail now - return collided; - } - } - } - } - } - // then collide against the models - int preNumCollisions = collisions.size(); - if (_skeletonModel.findSphereCollisions(particleCenter, particleRadius, collisions)) { - // the Model doesn't have velocity info, so we have to set it for each new collision - int postNumCollisions = collisions.size(); - for (int i = preNumCollisions; i < postNumCollisions; ++i) { - CollisionInfo* collision = collisions.getCollision(i); - collision->_penetration /= (float)(TREE_SCALE); - collision->_addedVelocity = getVelocity(); - } - collided = true; - } - return collided; -} - glm::quat Avatar::getJointRotation(int index) const { if (QThread::currentThread() != thread()) { return AvatarData::getJointRotation(index); @@ -909,25 +829,6 @@ float Avatar::getHeadHeight() const { return DEFAULT_HEAD_HEIGHT; } -bool Avatar::collisionWouldMoveAvatar(CollisionInfo& collision) const { - if (!collision._data || collision._type != COLLISION_TYPE_MODEL) { - return false; - } - Model* model = static_cast(collision._data); - int jointIndex = collision._intData; - - if (model == &(_skeletonModel) && jointIndex != -1) { - // collision response of skeleton is temporarily disabled - return false; - //return _skeletonModel.collisionHitsMoveableJoint(collision); - } - if (model == &(getHead()->getFaceModel())) { - // ATM we always handle COLLISION_TYPE_MODEL against the face. - return true; - } - return false; -} - float Avatar::getBoundingRadius() const { // TODO: also use head model when computing the avatar's bounding radius return _skeletonModel.getBoundingRadius(); diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h index f928881068..a0e41d7122 100755 --- a/interface/src/avatar/Avatar.h +++ b/interface/src/avatar/Avatar.h @@ -101,14 +101,12 @@ public: /// \return true if at least one shape collided with avatar bool findCollisions(const QVector& shapes, CollisionList& collisions); - /// Checks for penetration between the described sphere and the avatar. + /// Checks for penetration between the a sphere and the avatar's models. /// \param penetratorCenter the center of the penetration test sphere /// \param penetratorRadius the radius of the penetration test sphere /// \param collisions[out] a list to which collisions get appended - /// \param skeletonSkipIndex if not -1, the index of a joint to skip (along with its descendents) in the skeleton model /// \return whether or not the sphere penetrated - bool findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius, - CollisionList& collisions, int skeletonSkipIndex = -1); + bool findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius, CollisionList& collisions); /// Checks for penetration between the described plane and the avatar. /// \param plane the penetration plane @@ -116,13 +114,6 @@ public: /// \return whether or not the plane penetrated bool findPlaneCollisions(const glm::vec4& plane, CollisionList& collisions); - /// Checks for collision between the a spherical particle and the avatar (including paddle hands) - /// \param collisionCenter the center of particle's bounding sphere - /// \param collisionRadius the radius of particle's bounding sphere - /// \param collisions[out] a list to which collisions get appended - /// \return whether or not the particle collided - bool findParticleCollisions(const glm::vec3& particleCenter, float particleRadius, CollisionList& collisions); - virtual bool isMyAvatar() { return false; } virtual glm::quat getJointRotation(int index) const; @@ -141,14 +132,10 @@ public: static void renderJointConnectingCone(glm::vec3 position1, glm::vec3 position2, float radius1, float radius2); - /// \return true if we expect the avatar would move as a result of the collision - bool collisionWouldMoveAvatar(CollisionInfo& collision) const; - virtual void applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration) { } /// \return bounding radius of avatar virtual float getBoundingRadius() const; - void updateShapePositions(); quint32 getCollisionGroups() const { return _collisionGroups; } virtual void setCollisionGroups(quint32 collisionGroups) { _collisionGroups = (collisionGroups & VALID_COLLISION_GROUPS); } diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp index 3aff984893..94f734ba06 100644 --- a/interface/src/avatar/Hand.cpp +++ b/interface/src/avatar/Hand.cpp @@ -94,39 +94,6 @@ void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) { } } -void Hand::collideAgainstOurself() { - if (!Menu::getInstance()->isOptionChecked(MenuOption::HandsCollideWithSelf)) { - return; - } - - int leftPalmIndex, rightPalmIndex; - getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex); - float scaledPalmRadius = PALM_COLLISION_RADIUS * _owningAvatar->getScale(); - - const SkeletonModel& skeletonModel = _owningAvatar->getSkeletonModel(); - for (int i = 0; i < int(getNumPalms()); i++) { - PalmData& palm = getPalms()[i]; - if (!palm.isActive()) { - continue; - } - // ignoring everything below the parent of the parent of the last free joint - int skipIndex = skeletonModel.getParentJointIndex(skeletonModel.getParentJointIndex( - skeletonModel.getLastFreeJointIndex((int(i) == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() : - (int(i) == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1))); - - handCollisions.clear(); - if (_owningAvatar->findSphereCollisions(palm.getPosition(), scaledPalmRadius, handCollisions, skipIndex)) { - glm::vec3 totalPenetration; - for (int j = 0; j < handCollisions.size(); ++j) { - CollisionInfo* collision = handCollisions.getCollision(j); - totalPenetration = addPenetrations(totalPenetration, collision->_penetration); - } - // resolve penetration - palm.addToPenetration(totalPenetration); - } - } -} - void Hand::resolvePenetrations() { for (size_t i = 0; i < getNumPalms(); ++i) { PalmData& palm = getPalms()[i]; diff --git a/interface/src/avatar/Hand.h b/interface/src/avatar/Hand.h index 5d171f2809..ed2fa3e1ab 100755 --- a/interface/src/avatar/Hand.h +++ b/interface/src/avatar/Hand.h @@ -54,7 +54,6 @@ public: void render(bool isMine, Model::RenderMode renderMode = Model::DEFAULT_RENDER_MODE); void collideAgainstAvatar(Avatar* avatar, bool isMyHand); - void collideAgainstOurself(); void resolvePenetrations(); diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 54ed641d72..a90a65b5ed 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -76,14 +76,21 @@ MyAvatar::MyAvatar() : _lastFloorContactPoint(0.0f), _lookAtTargetAvatar(), _shouldRender(true), - _billboardValid(false) + _billboardValid(false), + _physicsSimulation() { for (int i = 0; i < MAX_DRIVE_KEYS; i++) { _driveKeys[i] = 0.0f; } + _skeletonModel.setEnableShapes(true); + // The skeleton is both a PhysicsEntity and Ragdoll, so we add it to the simulation once for each type. + _physicsSimulation.addEntity(&_skeletonModel); + _physicsSimulation.addRagdoll(&_skeletonModel); } MyAvatar::~MyAvatar() { + _physicsSimulation.removeEntity(&_skeletonModel); + _physicsSimulation.removeRagdoll(&_skeletonModel); _lookAtTargetAvatar.clear(); } @@ -154,7 +161,6 @@ void MyAvatar::simulate(float deltaTime) { { PerformanceTimer perfTimer("MyAvatar::simulate/hand Collision,simulate"); // update avatar skeleton and simulate hand and head - getHand()->collideAgainstOurself(); getHand()->simulate(deltaTime, true); } @@ -189,6 +195,18 @@ void MyAvatar::simulate(float deltaTime) { head->simulate(deltaTime, true); } + { + PerformanceTimer perfTimer("MyAvatar::simulate/ragdoll"); + if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) { + const int minError = 0.01f; + const float maxIterations = 10; + const quint64 maxUsec = 2000; + _physicsSimulation.stepForward(deltaTime, minError, maxIterations, maxUsec); + } else { + _skeletonModel.moveShapesTowardJoints(1.0f); + } + } + // now that we're done stepping the avatar forward in time, compute new collisions if (_collisionGroups != 0) { PerformanceTimer perfTimer("MyAvatar::simulate/_collisionGroups"); @@ -199,7 +217,6 @@ void MyAvatar::simulate(float deltaTime) { radius = myCamera->getAspectRatio() * (myCamera->getNearClip() / cos(myCamera->getFieldOfView() / 2.0f)); radius *= COLLISION_RADIUS_SCALAR; } - updateShapePositions(); if (_collisionGroups & COLLISION_GROUP_ENVIRONMENT) { PerformanceTimer perfTimer("MyAvatar::simulate/updateCollisionWithEnvironment"); updateCollisionWithEnvironment(deltaTime, radius); @@ -210,10 +227,12 @@ void MyAvatar::simulate(float deltaTime) { } else { _trapDuration = 0.0f; } + /* TODO: Andrew to make this work if (_collisionGroups & COLLISION_GROUP_AVATARS) { PerformanceTimer perfTimer("MyAvatar::simulate/updateCollisionWithAvatars"); updateCollisionWithAvatars(deltaTime); } + */ } // consider updating our billboard @@ -1233,7 +1252,7 @@ void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) { float capsuleHalfHeight = boundingShape.getHalfHeight(); const float MAX_STEP_HEIGHT = capsuleRadius + capsuleHalfHeight; const float MIN_STEP_HEIGHT = 0.0f; - glm::vec3 footBase = boundingShape.getPosition() - (capsuleRadius + capsuleHalfHeight) * _worldUpDirection; + glm::vec3 footBase = boundingShape.getTranslation() - (capsuleRadius + capsuleHalfHeight) * _worldUpDirection; float highestStep = 0.0f; float lowestStep = MAX_STEP_HEIGHT; glm::vec3 floorPoint; @@ -1250,7 +1269,7 @@ void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) { if (horizontalDepth > capsuleRadius || fabsf(verticalDepth) > MAX_STEP_HEIGHT) { isTrapped = true; if (_trapDuration > MAX_TRAP_PERIOD) { - float distance = glm::dot(boundingShape.getPosition() - cubeCenter, _worldUpDirection); + float distance = glm::dot(boundingShape.getTranslation() - cubeCenter, _worldUpDirection); if (distance < 0.0f) { distance = fabsf(distance) + 0.5f * cubeSide; } @@ -1435,7 +1454,6 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) { // don't collide with ourselves continue; } - avatar->updateShapePositions(); float distance = glm::length(_position - avatar->getPosition()); if (_distanceToNearestAvatar > distance) { _distanceToNearestAvatar = distance; @@ -1461,17 +1479,10 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) { } } - // collide our hands against them - // TODO: make this work when we can figure out when the other avatar won't yeild - // (for example, we're colliding against their chest or leg) - //getHand()->collideAgainstAvatar(avatar, true); - // collide their hands against us avatar->getHand()->collideAgainstAvatar(this, false); } } - // TODO: uncomment this when we handle collisions that won't affect other avatar - //getHand()->resolvePenetrations(); } class SortedAvatar { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 2fbc488feb..e7023b45a1 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -14,6 +14,8 @@ #include +#include + #include "Avatar.h" enum AvatarHandState @@ -173,6 +175,7 @@ private: float _oculusYawOffset; QList _animationHandles; + PhysicsSimulation _physicsSimulation; // private methods float computeDistanceToFloor(const glm::vec3& startPoint); diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index af070a91bd..fdb3ce03d8 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -11,21 +11,37 @@ #include +#include +#include + #include "Application.h" #include "Avatar.h" #include "Hand.h" #include "Menu.h" #include "SkeletonModel.h" -SkeletonModel::SkeletonModel(Avatar* owningAvatar) : - _owningAvatar(owningAvatar) { +SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent) : + Model(parent), + Ragdoll(), + _owningAvatar(owningAvatar), + _boundingShape(), + _boundingShapeLocalOffset(0.0f) { } void SkeletonModel::setJointStates(QVector states) { Model::setJointStates(states); - if (isActive() && _owningAvatar->isMyAvatar()) { - _ragDoll.init(_jointStates); + // the SkeletonModel override of updateJointState() will clear the translation part + // of its root joint and we need that done before we try to build shapes hence we + // recompute all joint transforms at this time. + for (int i = 0; i < _jointStates.size(); i++) { + updateJointState(i); + } + + clearShapes(); + clearRagdollConstraintsAndPoints(); + if (_enableShapes) { + buildShapes(); } } @@ -86,25 +102,13 @@ void SkeletonModel::simulate(float deltaTime, bool fullUpdate) { applyPalmData(geometry.leftHandJointIndex, hand->getPalms()[leftPalmIndex]); applyPalmData(geometry.rightHandJointIndex, hand->getPalms()[rightPalmIndex]); } - - simulateRagDoll(deltaTime); -} -void SkeletonModel::simulateRagDoll(float deltaTime) { - _ragDoll.slaveToSkeleton(_jointStates, 0.1f); // fraction = 0.1f left intentionally low for demo purposes - - float MIN_CONSTRAINT_ERROR = 0.005f; // 5mm - int MAX_ITERATIONS = 4; - int iterations = 0; - float delta = 0.0f; - do { - delta = _ragDoll.enforceConstraints(); - ++iterations; - } while (delta > MIN_CONSTRAINT_ERROR && iterations < MAX_ITERATIONS); + _boundingShape.setTranslation(_translation + _rotation * _boundingShapeLocalOffset); + _boundingShape.setRotation(_rotation); } void SkeletonModel::getHandShapes(int jointIndex, QVector& shapes) const { - if (jointIndex < 0 || jointIndex >= int(_jointShapes.size())) { + if (jointIndex < 0 || jointIndex >= int(_shapes.size())) { return; } if (jointIndex == getLeftHandJointIndex() @@ -114,18 +118,21 @@ void SkeletonModel::getHandShapes(int jointIndex, QVector& shapes) for (int i = 0; i < _jointStates.size(); i++) { const FBXJoint& joint = geometry.joints[i]; int parentIndex = joint.parentIndex; + Shape* shape = _shapes[i]; if (i == jointIndex) { // this shape is the hand - shapes.push_back(_jointShapes[i]); - if (parentIndex != -1) { - // also add the forearm - shapes.push_back(_jointShapes[parentIndex]); + if (shape) { + shapes.push_back(shape); } - } else { + if (parentIndex != -1 && _shapes[parentIndex]) { + // also add the forearm + shapes.push_back(_shapes[parentIndex]); + } + } else if (shape) { while (parentIndex != -1) { if (parentIndex == jointIndex) { // this shape is a child of the hand - shapes.push_back(_jointShapes[i]); + shapes.push_back(shape); break; } parentIndex = geometry.joints[parentIndex].parentIndex; @@ -145,7 +152,7 @@ void SkeletonModel::renderIKConstraints() { renderJointConstraints(getRightHandJointIndex()); renderJointConstraints(getLeftHandJointIndex()); //if (isActive() && _owningAvatar->isMyAvatar()) { - // renderRagDoll(); + // renderRagdoll(); //} } @@ -244,15 +251,6 @@ void SkeletonModel::updateJointState(int index) { } } -void SkeletonModel::updateShapePositions() { - if (isActive() && _owningAvatar->isMyAvatar() && - Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagDoll)) { - _ragDoll.updateShapes(_jointShapes, _rotation, _translation); - } else { - Model::updateShapePositions(); - } -} - void SkeletonModel::maybeUpdateLeanRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) { if (!_owningAvatar->isMyAvatar() || Application::getInstance()->getPrioVR()->isActive()) { return; @@ -478,22 +476,21 @@ bool SkeletonModel::getEyePositions(glm::vec3& firstEyePosition, glm::vec3& seco return false; } -void SkeletonModel::renderRagDoll() { +void SkeletonModel::renderRagdoll() { const int BALL_SUBDIVISIONS = 6; glDisable(GL_DEPTH_TEST); glDisable(GL_LIGHTING); glPushMatrix(); Application::getInstance()->loadTranslatedViewMatrix(_translation); - QVector points = _ragDoll.getPoints(); - int numPoints = points.size(); + int numPoints = _ragdollPoints.size(); float alpha = 0.3f; float radius1 = 0.008f; float radius2 = 0.01f; for (int i = 0; i < numPoints; ++i) { glPushMatrix(); // draw each point as a yellow hexagon with black border - glm::vec3 position = _rotation * points[i]; + glm::vec3 position = _rotation * _ragdollPoints[i]._position; glTranslatef(position.x, position.y, position.z); glColor4f(0.0f, 0.0f, 0.0f, alpha); glutSolidSphere(radius2, BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); @@ -505,3 +502,266 @@ void SkeletonModel::renderRagDoll() { glEnable(GL_DEPTH_TEST); glEnable(GL_LIGHTING); } + +// virtual +void SkeletonModel::initRagdollPoints() { + assert(_ragdollPoints.size() == 0); + assert(_ragdollConstraints.size() == 0); + + // one point for each joint + int numJoints = _jointStates.size(); + _ragdollPoints.fill(VerletPoint(), numJoints); + for (int i = 0; i < numJoints; ++i) { + const JointState& state = _jointStates.at(i); + glm::vec3 position = state.getPosition(); + _ragdollPoints[i]._position = position; + _ragdollPoints[i]._lastPosition = position; + } +} + +void SkeletonModel::buildRagdollConstraints() { + // NOTE: the length of DistanceConstraints is computed and locked in at this time + // so make sure the ragdoll positions are in a normal configuration before here. + const int numPoints = _ragdollPoints.size(); + assert(numPoints == _jointStates.size()); + + for (int i = 0; i < numPoints; ++i) { + const JointState& state = _jointStates.at(i); + const FBXJoint& joint = state.getFBXJoint(); + int parentIndex = joint.parentIndex; + if (parentIndex == -1) { + FixedConstraint* anchor = new FixedConstraint(&(_ragdollPoints[i]), glm::vec3(0.0f)); + _ragdollConstraints.push_back(anchor); + } else { + DistanceConstraint* bone = new DistanceConstraint(&(_ragdollPoints[i]), &(_ragdollPoints[parentIndex])); + _ragdollConstraints.push_back(bone); + } + } +} + +// virtual +void SkeletonModel::stepRagdollForward(float deltaTime) { + const float RAGDOLL_FOLLOWS_JOINTS_TIMESCALE = 0.03f; + float fraction = glm::clamp(deltaTime / RAGDOLL_FOLLOWS_JOINTS_TIMESCALE, 0.0f, 1.0f); + moveShapesTowardJoints(fraction); +} + +float DENSITY_OF_WATER = 1000.0f; // kg/m^3 +float MIN_JOINT_MASS = 1.0f; +float VERY_BIG_MASS = 1.0e6f; + +// virtual +void SkeletonModel::buildShapes() { + if (!_geometry || _rootIndex == -1) { + return; + } + + const FBXGeometry& geometry = _geometry->getFBXGeometry(); + if (geometry.joints.isEmpty()) { + return; + } + + initRagdollPoints(); + + float uniformScale = extractUniformScale(_scale); + const int numStates = _jointStates.size(); + for (int i = 0; i < numStates; i++) { + JointState& state = _jointStates[i]; + const FBXJoint& joint = state.getFBXJoint(); + float radius = uniformScale * joint.boneRadius; + float halfHeight = 0.5f * uniformScale * joint.distanceToParent; + Shape::Type type = joint.shapeType; + if (i == 0 || (type == Shape::CAPSULE_SHAPE && halfHeight < EPSILON)) { + // this shape is forced to be a sphere + type = Shape::SPHERE_SHAPE; + } + Shape* shape = NULL; + int parentIndex = joint.parentIndex; + if (type == Shape::SPHERE_SHAPE) { + shape = new VerletSphereShape(radius, &(_ragdollPoints[i])); + shape->setEntity(this); + _ragdollPoints[i]._mass = glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume()); + } else if (type == Shape::CAPSULE_SHAPE) { + assert(parentIndex != -1); + shape = new VerletCapsuleShape(radius, &(_ragdollPoints[parentIndex]), &(_ragdollPoints[i])); + shape->setEntity(this); + _ragdollPoints[i]._mass = glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume()); + } + if (parentIndex != -1) { + // always disable collisions between joint and its parent + disableCollisions(i, parentIndex); + } else { + // give the base joint a very large mass since it doesn't actually move + // in the local-frame simulation (it defines the origin) + _ragdollPoints[i]._mass = VERY_BIG_MASS; + } + _shapes.push_back(shape); + } + + // This method moves the shapes to their default positions in Model frame. + computeBoundingShape(geometry); + + // While the shapes are in their default position we disable collisions between + // joints that are currently colliding. + disableCurrentSelfCollisions(); + + buildRagdollConstraints(); + + // ... then move shapes back to current joint positions + moveShapesTowardJoints(1.0f); + enforceRagdollConstraints(); +} + +void SkeletonModel::moveShapesTowardJoints(float fraction) { + const int numStates = _jointStates.size(); + assert(_jointStates.size() == _ragdollPoints.size()); + assert(fraction >= 0.0f && fraction <= 1.0f); + if (_ragdollPoints.size() == numStates) { + float oneMinusFraction = 1.0f - fraction; + int numJoints = _jointStates.size(); + for (int i = 0; i < numJoints; ++i) { + _ragdollPoints[i]._lastPosition = _ragdollPoints[i]._position; + _ragdollPoints[i]._position = oneMinusFraction * _ragdollPoints[i]._position + fraction * _jointStates.at(i).getPosition(); + } + } +} + +void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) { + // compute default joint transforms + int numJoints = geometry.joints.size(); + if (numJoints != _ragdollPoints.size()) { + return; + } + QVector transforms; + transforms.fill(glm::mat4(), numJoints); + + // compute the default transforms and slam the ragdoll positions accordingly + // (which puts the shapes where we want them) + transforms[0] = _jointStates[0].getTransform(); + _ragdollPoints[0]._position = extractTranslation(transforms[0]); + _ragdollPoints[0]._lastPosition = _ragdollPoints[0]._position; + for (int i = 1; i < numJoints; i++) { + const FBXJoint& joint = geometry.joints.at(i); + int parentIndex = joint.parentIndex; + assert(parentIndex != -1); + + glm::quat modifiedRotation = joint.preRotation * joint.rotation * joint.postRotation; + transforms[i] = transforms[parentIndex] * glm::translate(joint.translation) + * joint.preTransform * glm::mat4_cast(modifiedRotation) * joint.postTransform; + // setting the ragdollPoints here slams the VerletShapes into their default positions + _ragdollPoints[i]._position = extractTranslation(transforms[i]); + _ragdollPoints[i]._lastPosition = _ragdollPoints[i]._position; + } + + // compute bounding box that encloses all shapes + Extents totalExtents; + totalExtents.reset(); + totalExtents.addPoint(glm::vec3(0.0f)); + for (int i = 0; i < _shapes.size(); i++) { + Shape* shape = _shapes[i]; + if (!shape) { + continue; + } + // TODO: skip hand and arm shapes for bounding box calculation + Extents shapeExtents; + shapeExtents.reset(); + glm::vec3 localPosition = shape->getTranslation(); + int type = shape->getType(); + if (type == Shape::CAPSULE_SHAPE) { + // add the two furthest surface points of the capsule + CapsuleShape* capsule = static_cast(shape); + glm::vec3 axis; + capsule->computeNormalizedAxis(axis); + float radius = capsule->getRadius(); + float halfHeight = capsule->getHalfHeight(); + axis = halfHeight * axis + glm::vec3(radius); + + shapeExtents.addPoint(localPosition + axis); + shapeExtents.addPoint(localPosition - axis); + totalExtents.addExtents(shapeExtents); + } else if (type == Shape::SPHERE_SHAPE) { + float radius = shape->getBoundingRadius(); + glm::vec3 axis = glm::vec3(radius); + shapeExtents.addPoint(localPosition + axis); + shapeExtents.addPoint(localPosition - axis); + totalExtents.addExtents(shapeExtents); + } + } + + // compute bounding shape parameters + // NOTE: we assume that the longest side of totalExtents is the yAxis... + glm::vec3 diagonal = totalExtents.maximum - totalExtents.minimum; + // ... and assume the radius is half the RMS of the X and Z sides: + float capsuleRadius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z)); + _boundingShape.setRadius(capsuleRadius); + _boundingShape.setHalfHeight(0.5f * diagonal.y - capsuleRadius); + _boundingShapeLocalOffset = 0.5f * (totalExtents.maximum + totalExtents.minimum); + _boundingRadius = 0.5f * glm::length(diagonal); +} + +void SkeletonModel::resetShapePositions() { + // DEBUG method. + // Moves shapes to the joint default locations for debug visibility into + // how the bounding shape is computed. + + if (!_geometry || _rootIndex == -1 || _shapes.isEmpty()) { + // geometry or joints have not yet been created + return; + } + + const FBXGeometry& geometry = _geometry->getFBXGeometry(); + if (geometry.joints.isEmpty() || _shapes.size() != geometry.joints.size()) { + return; + } + + // The shapes are moved to their default positions in computeBoundingShape(). + computeBoundingShape(geometry); + + // Then we move them into world frame for rendering at the Model's location. + for (int i = 0; i < _shapes.size(); i++) { + Shape* shape = _shapes[i]; + if (shape) { + shape->setTranslation(_translation + _rotation * shape->getTranslation()); + shape->setRotation(_rotation * shape->getRotation()); + } + } + _boundingShape.setTranslation(_translation + _rotation * _boundingShapeLocalOffset); + _boundingShape.setRotation(_rotation); +} + +void SkeletonModel::renderBoundingCollisionShapes(float alpha) { + const int BALL_SUBDIVISIONS = 10; + if (_shapes.isEmpty()) { + // the bounding shape has not been propery computed + // so no need to render it + return; + } + glPushMatrix(); + + Application::getInstance()->loadTranslatedViewMatrix(_translation); + + // draw a blue sphere at the capsule endpoint + glm::vec3 endPoint; + _boundingShape.getEndPoint(endPoint); + endPoint = endPoint - _translation; + glTranslatef(endPoint.x, endPoint.y, endPoint.z); + glColor4f(0.6f, 0.6f, 0.8f, alpha); + glutSolidSphere(_boundingShape.getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); + + // draw a yellow sphere at the capsule startpoint + glm::vec3 startPoint; + _boundingShape.getStartPoint(startPoint); + startPoint = startPoint - _translation; + glm::vec3 axis = endPoint - startPoint; + glTranslatef(-axis.x, -axis.y, -axis.z); + glColor4f(0.8f, 0.8f, 0.6f, alpha); + glutSolidSphere(_boundingShape.getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); + + // draw a green cylinder between the two points + glm::vec3 origin(0.0f); + glColor4f(0.6f, 0.8f, 0.6f, alpha); + Avatar::renderJointConnectingCone( origin, axis, _boundingShape.getRadius(), _boundingShape.getRadius()); + + glPopMatrix(); +} + diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h index 3b8e67df47..b91c112b6a 100644 --- a/interface/src/avatar/SkeletonModel.h +++ b/interface/src/avatar/SkeletonModel.h @@ -13,23 +13,23 @@ #define hifi_SkeletonModel_h #include "renderer/Model.h" -#include "renderer/RagDoll.h" + +#include +#include class Avatar; /// A skeleton loaded from a model. -class SkeletonModel : public Model { +class SkeletonModel : public Model, public Ragdoll { Q_OBJECT public: - SkeletonModel(Avatar* owningAvatar); + SkeletonModel(Avatar* owningAvatar, QObject* parent = NULL); void setJointStates(QVector states); void simulate(float deltaTime, bool fullUpdate = true); - void simulateRagDoll(float deltaTime); - void updateShapePositions(); /// \param jointIndex index of hand joint /// \param shapes[out] list in which is stored pointers to hand shapes @@ -94,9 +94,27 @@ public: /// \return whether or not both eye meshes were found bool getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const; - void renderRagDoll(); + // virtual overrride from Ragdoll + virtual void stepRagdollForward(float deltaTime); + + void moveShapesTowardJoints(float fraction); + + void computeBoundingShape(const FBXGeometry& geometry); + void renderBoundingCollisionShapes(float alpha); + float getBoundingShapeRadius() const { return _boundingShape.getRadius(); } + const CapsuleShape& getBoundingShape() const { return _boundingShape; } + + void resetShapePositions(); // DEBUG method + + void renderRagdoll(); protected: + // virtual overrrides from Ragdoll + void initRagdollPoints(); + void buildRagdollConstraints(); + + void buildShapes(); + /// \param jointIndex index of joint in model /// \param position position of joint in model-frame void applyHandPosition(int jointIndex, const glm::vec3& position); @@ -120,7 +138,9 @@ private: void setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation); Avatar* _owningAvatar; - RagDoll _ragDoll; + + CapsuleShape _boundingShape; + glm::vec3 _boundingShapeLocalOffset; }; #endif // hifi_SkeletonModel_h diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index aff023c2a0..3b5cda4fd2 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -16,15 +16,15 @@ #include #include +#include #include +#include +#include +#include #include "Application.h" #include "Model.h" -#include -#include -#include - using namespace std; static int modelPointerTypeId = qRegisterMetaType >(); @@ -40,10 +40,7 @@ Model::Model(QObject* parent) : _snapModelToCenter(false), _snappedToCenter(false), _rootIndex(-1), - _shapesAreDirty(true), - _boundingRadius(0.0f), - _boundingShape(), - _boundingShapeLocalOffset(0.0f), + //_enableCollisionShapes(false), _lodDistance(0.0f), _pupilDilation(0.0f), _url("http://invalid.com") { @@ -129,7 +126,10 @@ void Model::setScaleInternal(const glm::vec3& scale) { const float ONE_PERCENT = 0.01f; if (relativeDeltaScale > ONE_PERCENT || scaleLength < EPSILON) { _scale = scale; - rebuildShapes(); + if (_shapes.size() > 0) { + clearShapes(); + buildShapes(); + } } } @@ -174,6 +174,7 @@ QVector Model::createJointStates(const FBXGeometry& geometry) { int parentIndex = joint.parentIndex; if (parentIndex == -1) { _rootIndex = i; + // NOTE: in practice geometry.offset has a non-unity scale (rather than a translation) glm::mat4 parentTransform = glm::scale(_scale) * glm::translate(_offset) * geometry.offset; state.computeTransform(parentTransform); } else { @@ -551,7 +552,6 @@ bool Model::updateGeometry() { model->setURL(attachment.url); _attachments.append(model); } - rebuildShapes(); needFullUpdate = true; } return needFullUpdate; @@ -560,6 +560,18 @@ bool Model::updateGeometry() { // virtual void Model::setJointStates(QVector states) { _jointStates = states; + + // compute an approximate bounding radius for broadphase collision queries + // against PhysicsSimulation boundaries + int numJoints = _jointStates.size(); + float radius = 0.0f; + for (int i = 0; i < numJoints; ++i) { + float distance = glm::length(_jointStates[i].getPosition()); + if (distance > radius) { + radius = distance; + } + } + _boundingRadius = radius; } bool Model::render(float alpha, RenderMode mode, bool receiveShadows) { @@ -774,304 +786,13 @@ AnimationHandlePointer Model::createAnimationHandle() { return handle; } -void Model::clearShapes() { - for (int i = 0; i < _jointShapes.size(); ++i) { - delete _jointShapes[i]; - } - _jointShapes.clear(); -} - -void Model::rebuildShapes() { - clearShapes(); - - if (!_geometry || _rootIndex == -1) { - return; - } - - const FBXGeometry& geometry = _geometry->getFBXGeometry(); - if (geometry.joints.isEmpty()) { - return; - } - - // We create the shapes with proper dimensions, but we set their transforms later. - float uniformScale = extractUniformScale(_scale); - for (int i = 0; i < _jointStates.size(); i++) { - const FBXJoint& joint = geometry.joints[i]; - - float radius = uniformScale * joint.boneRadius; - float halfHeight = 0.5f * uniformScale * joint.distanceToParent; - Shape::Type type = joint.shapeType; - if (type == Shape::CAPSULE_SHAPE && halfHeight < EPSILON) { - // this capsule is effectively a sphere - type = Shape::SPHERE_SHAPE; - } - if (type == Shape::CAPSULE_SHAPE) { - CapsuleShape* capsule = new CapsuleShape(radius, halfHeight); - _jointShapes.push_back(capsule); - } else if (type == Shape::SPHERE_SHAPE) { - SphereShape* sphere = new SphereShape(radius, glm::vec3(0.0f)); - _jointShapes.push_back(sphere); - } else { - // this shape type is not handled and the joint shouldn't collide, - // however we must have a shape for each joint, - // so we make a bogus sphere with zero radius. - // TODO: implement collision groups for more control over what collides with what - SphereShape* sphere = new SphereShape(0.0f, glm::vec3(0.0f)); - _jointShapes.push_back(sphere); - } - } - - // This method moves the shapes to their default positions in Model frame - // which is where we compute the bounding shape's parameters. - computeBoundingShape(geometry); - - // finally sync shapes to joint positions - _shapesAreDirty = true; - updateShapePositions(); -} - -void Model::computeBoundingShape(const FBXGeometry& geometry) { - // compute default joint transforms and rotations - // (in local frame, ignoring Model translation and rotation) - int numJoints = geometry.joints.size(); - QVector transforms; - transforms.fill(glm::mat4(), numJoints); - QVector finalRotations; - finalRotations.fill(glm::quat(), numJoints); - - QVector shapeIsSet; - shapeIsSet.fill(false, numJoints); - int numShapesSet = 0; - int lastNumShapesSet = -1; - while (numShapesSet < numJoints && numShapesSet != lastNumShapesSet) { - lastNumShapesSet = numShapesSet; - for (int i = 0; i < numJoints; i++) { - const FBXJoint& joint = geometry.joints.at(i); - int parentIndex = joint.parentIndex; - - if (parentIndex == -1) { - glm::mat4 baseTransform = glm::scale(_scale) * glm::translate(_offset); - glm::quat combinedRotation = joint.preRotation * joint.rotation * joint.postRotation; - glm::mat4 rootTransform = baseTransform * geometry.offset * glm::translate(joint.translation) - * joint.preTransform * glm::mat4_cast(combinedRotation) * joint.postTransform; - // remove the tranlsation part before we save the root transform - transforms[i] = glm::translate(- extractTranslation(rootTransform)) * rootTransform; - - finalRotations[i] = combinedRotation; - ++numShapesSet; - shapeIsSet[i] = true; - } else if (shapeIsSet[parentIndex]) { - glm::quat combinedRotation = joint.preRotation * joint.rotation * joint.postRotation; - transforms[i] = transforms[parentIndex] * glm::translate(joint.translation) - * joint.preTransform * glm::mat4_cast(combinedRotation) * joint.postTransform; - finalRotations[i] = finalRotations[parentIndex] * combinedRotation; - ++numShapesSet; - shapeIsSet[i] = true; - } - } - } - - // sync shapes to joints - _boundingRadius = 0.0f; - float uniformScale = extractUniformScale(_scale); - for (int i = 0; i < _jointShapes.size(); i++) { - const FBXJoint& joint = geometry.joints[i]; - glm::vec3 jointToShapeOffset = uniformScale * (finalRotations[i] * joint.shapePosition); - glm::vec3 localPosition = extractTranslation(transforms[i]) + jointToShapeOffset; - Shape* shape = _jointShapes[i]; - shape->setPosition(localPosition); - shape->setRotation(finalRotations[i] * joint.shapeRotation); - float distance = glm::length(localPosition) + shape->getBoundingRadius(); - if (distance > _boundingRadius) { - _boundingRadius = distance; - } - } - - // compute bounding box - Extents totalExtents; - totalExtents.reset(); - for (int i = 0; i < _jointShapes.size(); i++) { - Extents shapeExtents; - shapeExtents.reset(); - - Shape* shape = _jointShapes[i]; - glm::vec3 localPosition = shape->getPosition(); - int type = shape->getType(); - if (type == Shape::CAPSULE_SHAPE) { - // add the two furthest surface points of the capsule - CapsuleShape* capsule = static_cast(shape); - glm::vec3 axis; - capsule->computeNormalizedAxis(axis); - float radius = capsule->getRadius(); - float halfHeight = capsule->getHalfHeight(); - axis = halfHeight * axis + glm::vec3(radius); - - shapeExtents.addPoint(localPosition + axis); - shapeExtents.addPoint(localPosition - axis); - totalExtents.addExtents(shapeExtents); - } else if (type == Shape::SPHERE_SHAPE) { - float radius = shape->getBoundingRadius(); - glm::vec3 axis = glm::vec3(radius); - shapeExtents.addPoint(localPosition + axis); - shapeExtents.addPoint(localPosition - axis); - totalExtents.addExtents(shapeExtents); - } - } - - // compute bounding shape parameters - // NOTE: we assume that the longest side of totalExtents is the yAxis... - glm::vec3 diagonal = totalExtents.maximum - totalExtents.minimum; - // ... and assume the radius is half the RMS of the X and Z sides: - float capsuleRadius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z)); - _boundingShape.setRadius(capsuleRadius); - _boundingShape.setHalfHeight(0.5f * diagonal.y - capsuleRadius); - _boundingShapeLocalOffset = 0.5f * (totalExtents.maximum + totalExtents.minimum); -} - -void Model::resetShapePositions() { - // DEBUG method. - // Moves shapes to the joint default locations for debug visibility into - // how the bounding shape is computed. - - if (!_geometry || _rootIndex == -1) { - // geometry or joints have not yet been created - return; - } - - const FBXGeometry& geometry = _geometry->getFBXGeometry(); - if (geometry.joints.isEmpty() || _jointShapes.size() != geometry.joints.size()) { - return; - } - - // The shapes are moved to their default positions in computeBoundingShape(). - computeBoundingShape(geometry); - - // Then we move them into world frame for rendering at the Model's location. - for (int i = 0; i < _jointShapes.size(); i++) { - Shape* shape = _jointShapes[i]; - shape->setPosition(_translation + _rotation * shape->getPosition()); - shape->setRotation(_rotation * shape->getRotation()); - } - _boundingShape.setPosition(_translation + _rotation * _boundingShapeLocalOffset); - _boundingShape.setRotation(_rotation); +// virtual override from PhysicsEntity +void Model::buildShapes() { + // TODO: figure out how to load/build collision shapes for general models } void Model::updateShapePositions() { - if (_shapesAreDirty && _jointShapes.size() == _jointStates.size()) { - glm::vec3 rootPosition(0.0f); - _boundingRadius = 0.0f; - float uniformScale = extractUniformScale(_scale); - for (int i = 0; i < _jointStates.size(); i++) { - const JointState& state = _jointStates[i]; - const FBXJoint& joint = state.getFBXJoint(); - // shape position and rotation need to be in world-frame - glm::quat stateRotation = state.getRotation(); - glm::vec3 shapeOffset = uniformScale * (stateRotation * joint.shapePosition); - glm::vec3 worldPosition = _translation + _rotation * (state.getPosition() + shapeOffset); - Shape* shape = _jointShapes[i]; - shape->setPosition(worldPosition); - shape->setRotation(_rotation * stateRotation * joint.shapeRotation); - float distance = glm::distance(worldPosition, _translation) + shape->getBoundingRadius(); - if (distance > _boundingRadius) { - _boundingRadius = distance; - } - if (joint.parentIndex == -1) { - rootPosition = worldPosition; - } - } - _shapesAreDirty = false; - _boundingShape.setPosition(rootPosition + _rotation * _boundingShapeLocalOffset); - _boundingShape.setRotation(_rotation); - } -} - -bool Model::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const { - const glm::vec3 relativeOrigin = origin - _translation; - const FBXGeometry& geometry = _geometry->getFBXGeometry(); - float minDistance = FLT_MAX; - float radiusScale = extractUniformScale(_scale); - for (int i = 0; i < _jointStates.size(); i++) { - const FBXJoint& joint = geometry.joints[i]; - glm::vec3 end = _translation + _rotation * _jointStates[i].getPosition(); - float endRadius = joint.boneRadius * radiusScale; - glm::vec3 start = end; - float startRadius = joint.boneRadius * radiusScale; - if (joint.parentIndex != -1) { - start = _translation + _rotation * _jointStates[joint.parentIndex].getPosition(); - startRadius = geometry.joints[joint.parentIndex].boneRadius * radiusScale; - } - // for now, use average of start and end radii - float capsuleDistance; - if (findRayCapsuleIntersection(relativeOrigin, direction, start, end, - (startRadius + endRadius) / 2.0f, capsuleDistance)) { - minDistance = qMin(minDistance, capsuleDistance); - } - } - if (minDistance < FLT_MAX) { - distance = minDistance; - return true; - } - return false; -} - -bool Model::findCollisions(const QVector shapes, CollisionList& collisions) { - bool collided = false; - for (int i = 0; i < shapes.size(); ++i) { - const Shape* theirShape = shapes[i]; - for (int j = 0; j < _jointShapes.size(); ++j) { - const Shape* ourShape = _jointShapes[j]; - if (ShapeCollider::collideShapes(theirShape, ourShape, collisions)) { - collided = true; - } - } - } - return collided; -} - -bool Model::findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadius, - CollisionList& collisions, int skipIndex) { - bool collided = false; - SphereShape sphere(sphereRadius, sphereCenter); - const FBXGeometry& geometry = _geometry->getFBXGeometry(); - for (int i = 0; i < _jointShapes.size(); i++) { - const FBXJoint& joint = geometry.joints[i]; - if (joint.parentIndex != -1) { - if (skipIndex != -1) { - int ancestorIndex = joint.parentIndex; - do { - if (ancestorIndex == skipIndex) { - goto outerContinue; - } - ancestorIndex = geometry.joints[ancestorIndex].parentIndex; - - } while (ancestorIndex != -1); - } - } - if (ShapeCollider::collideShapes(&sphere, _jointShapes[i], collisions)) { - CollisionInfo* collision = collisions.getLastCollision(); - collision->_type = COLLISION_TYPE_MODEL; - collision->_data = (void*)(this); - collision->_intData = i; - collided = true; - } - outerContinue: ; - } - return collided; -} - -bool Model::findPlaneCollisions(const glm::vec4& plane, CollisionList& collisions) { - bool collided = false; - PlaneShape planeShape(plane); - for (int i = 0; i < _jointShapes.size(); i++) { - if (ShapeCollider::collideShapes(&planeShape, _jointShapes[i], collisions)) { - CollisionInfo* collision = collisions.getLastCollision(); - collision->_type = COLLISION_TYPE_MODEL; - collision->_data = (void*)(this); - collision->_intData = i; - collided = true; - } - } - return collided; + // TODO: implement this when we know how to build shapes for regular Models } class Blender : public QRunnable { @@ -1197,7 +918,7 @@ void Model::simulateInternal(float deltaTime) { for (int i = 0; i < _jointStates.size(); i++) { updateJointState(i); } - _shapesAreDirty = true; + _shapesAreDirty = ! _shapes.isEmpty(); // update the attachment transforms and simulate them const FBXGeometry& geometry = _geometry->getFBXGeometry(); @@ -1332,7 +1053,7 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, const gl for (int j = freeLineage.size() - 1; j >= 0; j--) { updateJointState(freeLineage.at(j)); } - _shapesAreDirty = true; + _shapesAreDirty = !_shapes.isEmpty(); return true; } @@ -1370,14 +1091,17 @@ const int BALL_SUBDIVISIONS = 10; void Model::renderJointCollisionShapes(float alpha) { glPushMatrix(); Application::getInstance()->loadTranslatedViewMatrix(_translation); - for (int i = 0; i < _jointShapes.size(); i++) { - glPushMatrix(); + for (int i = 0; i < _shapes.size(); i++) { + Shape* shape = _shapes[i]; + if (!shape) { + continue; + } - Shape* shape = _jointShapes[i]; - + glPushMatrix(); + // NOTE: the shapes are in the avatar local-frame if (shape->getType() == Shape::SPHERE_SHAPE) { // shapes are stored in world-frame, so we have to transform into model frame - glm::vec3 position = shape->getPosition() - _translation; + glm::vec3 position = _rotation * shape->getTranslation(); glTranslatef(position.x, position.y, position.z); const glm::quat& rotation = shape->getRotation(); glm::vec3 axis = glm::axis(rotation); @@ -1392,7 +1116,7 @@ void Model::renderJointCollisionShapes(float alpha) { // draw a blue sphere at the capsule endpoint glm::vec3 endPoint; capsule->getEndPoint(endPoint); - endPoint = endPoint - _translation; + endPoint = _rotation * endPoint; glTranslatef(endPoint.x, endPoint.y, endPoint.z); glColor4f(0.6f, 0.6f, 0.8f, alpha); glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); @@ -1400,7 +1124,7 @@ void Model::renderJointCollisionShapes(float alpha) { // draw a yellow sphere at the capsule startpoint glm::vec3 startPoint; capsule->getStartPoint(startPoint); - startPoint = startPoint - _translation; + startPoint = _rotation * startPoint; glm::vec3 axis = endPoint - startPoint; glTranslatef(-axis.x, -axis.y, -axis.z); glColor4f(0.8f, 0.8f, 0.6f, alpha); @@ -1416,85 +1140,6 @@ void Model::renderJointCollisionShapes(float alpha) { glPopMatrix(); } -void Model::renderBoundingCollisionShapes(float alpha) { - glPushMatrix(); - - Application::getInstance()->loadTranslatedViewMatrix(_translation); - - // draw a blue sphere at the capsule endpoint - glm::vec3 endPoint; - _boundingShape.getEndPoint(endPoint); - endPoint = endPoint - _translation; - glTranslatef(endPoint.x, endPoint.y, endPoint.z); - glColor4f(0.6f, 0.6f, 0.8f, alpha); - glutSolidSphere(_boundingShape.getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); - - // draw a yellow sphere at the capsule startpoint - glm::vec3 startPoint; - _boundingShape.getStartPoint(startPoint); - startPoint = startPoint - _translation; - glm::vec3 axis = endPoint - startPoint; - glTranslatef(-axis.x, -axis.y, -axis.z); - glColor4f(0.8f, 0.8f, 0.6f, alpha); - glutSolidSphere(_boundingShape.getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); - - // draw a green cylinder between the two points - glm::vec3 origin(0.0f); - glColor4f(0.6f, 0.8f, 0.6f, alpha); - Avatar::renderJointConnectingCone( origin, axis, _boundingShape.getRadius(), _boundingShape.getRadius()); - - glPopMatrix(); -} - -bool Model::collisionHitsMoveableJoint(CollisionInfo& collision) const { - if (collision._type == COLLISION_TYPE_MODEL) { - // the joint is pokable by a collision if it exists and is free to move - const FBXJoint& joint = _geometry->getFBXGeometry().joints[collision._intData]; - if (joint.parentIndex == -1 || _jointStates.isEmpty()) { - return false; - } - // an empty freeLineage means the joint can't move - const FBXGeometry& geometry = _geometry->getFBXGeometry(); - int jointIndex = collision._intData; - const QVector& freeLineage = geometry.joints.at(jointIndex).freeLineage; - return !freeLineage.isEmpty(); - } - return false; -} - -void Model::applyCollision(CollisionInfo& collision) { - if (collision._type != COLLISION_TYPE_MODEL) { - return; - } - - glm::vec3 jointPosition(0.0f); - int jointIndex = collision._intData; - if (getJointPositionInWorldFrame(jointIndex, jointPosition)) { - const FBXJoint& joint = _geometry->getFBXGeometry().joints[jointIndex]; - if (joint.parentIndex != -1) { - // compute the approximate distance (travel) that the joint needs to move - glm::vec3 start; - getJointPositionInWorldFrame(joint.parentIndex, start); - glm::vec3 contactPoint = collision._contactPoint - start; - glm::vec3 penetrationEnd = contactPoint + collision._penetration; - glm::vec3 axis = glm::cross(contactPoint, penetrationEnd); - float travel = glm::length(axis); - const float MIN_TRAVEL = 1.0e-8f; - if (travel > MIN_TRAVEL) { - // compute the new position of the joint - float angle = asinf(travel / (glm::length(contactPoint) * glm::length(penetrationEnd))); - axis = glm::normalize(axis); - glm::vec3 end; - getJointPositionInWorldFrame(jointIndex, end); - // transform into model-frame - glm::vec3 newEnd = glm::inverse(_rotation) * (start + glm::angleAxis(angle, axis) * (end - start) - _translation); - // try to move it - setJointPosition(jointIndex, newEnd, glm::quat(), false, -1, true); - } - } - } -} - void Model::setBlendedVertices(const QVector& vertices, const QVector& normals) { if (_blendedVertexBuffers.isEmpty()) { return; diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index 11e6861775..2045a0c9b5 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -16,7 +16,7 @@ #include #include -#include +#include #include @@ -33,7 +33,7 @@ typedef QSharedPointer AnimationHandlePointer; typedef QWeakPointer WeakAnimationHandlePointer; /// A generic 3D model displaying geometry loaded from a URL. -class Model : public QObject { +class Model : public QObject, public PhysicsEntity { Q_OBJECT public: @@ -41,12 +41,6 @@ public: Model(QObject* parent = NULL); virtual ~Model(); - void setTranslation(const glm::vec3& translation) { _translation = translation; } - const glm::vec3& getTranslation() const { return _translation; } - - void setRotation(const glm::quat& rotation) { _rotation = rotation; } - const glm::quat& getRotation() const { return _rotation; } - /// enables/disables scale to fit behavior, the model will be automatically scaled to the specified largest dimension void setScaleToFit(bool scaleToFit, float largestDimension = 0.0f); bool getScaleToFit() const { return _scaleToFit; } /// is scale to fit enabled @@ -67,7 +61,7 @@ public: void setBlendshapeCoefficients(const QVector& coefficients) { _blendshapeCoefficients = coefficients; } const QVector& getBlendshapeCoefficients() const { return _blendshapeCoefficients; } - + bool isActive() const { return _geometry && _geometry->isLoaded(); } bool isRenderable() const { return !_meshStates.isEmpty() || (isActive() && _geometry->getMeshes().isEmpty()); } @@ -134,69 +128,34 @@ public: QStringList getJointNames() const; AnimationHandlePointer createAnimationHandle(); - + const QList& getRunningAnimations() const { return _runningAnimations; } - - void clearShapes(); - void rebuildShapes(); - void resetShapePositions(); + + // virtual overrides from PhysicsEntity + virtual void buildShapes(); virtual void updateShapePositions(); + void renderJointCollisionShapes(float alpha); - void renderBoundingCollisionShapes(float alpha); - bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const; - - /// \param shapes list of pointers shapes to test against Model - /// \param collisions list to store collision results - /// \return true if at least one shape collided agains Model - bool findCollisions(const QVector shapes, CollisionList& collisions); - - bool findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius, - CollisionList& collisions, int skipIndex = -1); - - bool findPlaneCollisions(const glm::vec4& plane, CollisionList& collisions); - - /// \param collision details about the collisions - /// \return true if the collision is against a moveable joint - bool collisionHitsMoveableJoint(CollisionInfo& collision) const; - - /// \param collision details about the collision - /// Use the collision to affect the model - void applyCollision(CollisionInfo& collision); - - float getBoundingRadius() const { return _boundingRadius; } - float getBoundingShapeRadius() const { return _boundingShape.getRadius(); } - /// Sets blended vertices computed in a separate thread. void setBlendedVertices(const QVector& vertices, const QVector& normals); - const CapsuleShape& getBoundingShape() const { return _boundingShape; } - protected: - QSharedPointer _geometry; - glm::vec3 _translation; - glm::quat _rotation; glm::vec3 _scale; glm::vec3 _offset; bool _scaleToFit; /// If you set scaleToFit, we will calculate scale based on MeshExtents float _scaleToFitLargestDimension; /// this is the dimension that scale to fit will use bool _scaledToFit; /// have we scaled to fit - + bool _snapModelToCenter; /// is the model's offset automatically adjusted to center around 0,0,0 in model space bool _snappedToCenter; /// are we currently snapped to center int _rootIndex; - bool _shapesAreDirty; QVector _jointStates; - QVector _jointShapes; - - float _boundingRadius; - CapsuleShape _boundingShape; - glm::vec3 _boundingShapeLocalOffset; - + class MeshState { public: QVector clusterMatrices; @@ -240,8 +199,6 @@ protected: /// first free ancestor. float getLimbLength(int jointIndex) const; - void computeBoundingShape(const FBXGeometry& geometry); - private: friend class AnimationHandle; diff --git a/interface/src/renderer/RagDoll.cpp b/interface/src/renderer/RagDoll.cpp deleted file mode 100644 index 305724d6e4..0000000000 --- a/interface/src/renderer/RagDoll.cpp +++ /dev/null @@ -1,167 +0,0 @@ -// -// RagDoll.cpp -// interface/src/avatar -// -// Created by Andrew Meadows 2014.05.30 -// Copyright 2014 High Fidelity, Inc. -// -// Distributed under the Apache License, Version 2.0. -// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html -// - -#include -#include -#include - -#include -#include -#include -#include - -#include "RagDoll.h" - -// ---------------------------------------------------------------------------- -// FixedConstraint -// ---------------------------------------------------------------------------- -FixedConstraint::FixedConstraint(glm::vec3* point, const glm::vec3& anchor) : _point(point), _anchor(anchor) { -} - -float FixedConstraint::enforce() { - assert(_point != NULL); - float distance = glm::distance(_anchor, *_point); - *_point = _anchor; - return distance; -} - -void FixedConstraint::setPoint(glm::vec3* point) { - _point = point; -} - -void FixedConstraint::setAnchor(const glm::vec3& anchor) { - _anchor = anchor; -} - -// ---------------------------------------------------------------------------- -// DistanceConstraint -// ---------------------------------------------------------------------------- -DistanceConstraint::DistanceConstraint(glm::vec3* startPoint, glm::vec3* endPoint) : _distance(-1.0f) { - _points[0] = startPoint; - _points[1] = endPoint; - _distance = glm::distance(*(_points[0]), *(_points[1])); -} - -DistanceConstraint::DistanceConstraint(const DistanceConstraint& other) { - _distance = other._distance; - _points[0] = other._points[0]; - _points[1] = other._points[1]; -} - -void DistanceConstraint::setDistance(float distance) { - _distance = fabsf(distance); -} - -float DistanceConstraint::enforce() { - float newDistance = glm::distance(*(_points[0]), *(_points[1])); - glm::vec3 direction(0.0f, 1.0f, 0.0f); - if (newDistance > EPSILON) { - direction = (*(_points[0]) - *(_points[1])) / newDistance; - } - glm::vec3 center = 0.5f * (*(_points[0]) + *(_points[1])); - *(_points[0]) = center + (0.5f * _distance) * direction; - *(_points[1]) = center - (0.5f * _distance) * direction; - return glm::abs(newDistance - _distance); -} - -void DistanceConstraint::updateProxyShape(Shape* shape, const glm::quat& rotation, const glm::vec3& translation) const { - if (!shape) { - return; - } - switch (shape->getType()) { - case Shape::SPHERE_SHAPE: { - // sphere collides at endPoint - SphereShape* sphere = static_cast(shape); - sphere->setPosition(translation + rotation * (*_points[1])); - } - break; - case Shape::CAPSULE_SHAPE: { - // capsule collides from startPoint to endPoint - CapsuleShape* capsule = static_cast(shape); - capsule->setEndPoints(translation + rotation * (*_points[0]), translation + rotation * (*_points[1])); - } - break; - default: - break; - } -} - -// ---------------------------------------------------------------------------- -// RagDoll -// ---------------------------------------------------------------------------- - -RagDoll::RagDoll() { -} - -RagDoll::~RagDoll() { - clear(); -} - -void RagDoll::init(const QVector& states) { - clear(); - const int numStates = states.size(); - _points.reserve(numStates); - for (int i = 0; i < numStates; ++i) { - const JointState& state = states[i]; - _points.push_back(state.getPosition()); - int parentIndex = state.getFBXJoint().parentIndex; - assert(parentIndex < i); - if (parentIndex == -1) { - FixedConstraint* anchor = new FixedConstraint(&(_points[i]), glm::vec3(0.0f)); - _constraints.push_back(anchor); - } else { - DistanceConstraint* stick = new DistanceConstraint(&(_points[i]), &(_points[parentIndex])); - _constraints.push_back(stick); - } - } -} - -/// Delete all data. -void RagDoll::clear() { - int numConstraints = _constraints.size(); - for (int i = 0; i < numConstraints; ++i) { - delete _constraints[i]; - } - _constraints.clear(); - _points.clear(); -} - -float RagDoll::slaveToSkeleton(const QVector& states, float fraction) { - const int numStates = states.size(); - assert(numStates == _points.size()); - fraction = glm::clamp(fraction, 0.0f, 1.0f); - float maxDistance = 0.0f; - for (int i = 0; i < numStates; ++i) { - glm::vec3 oldPoint = _points[i]; - _points[i] = (1.0f - fraction) * _points[i] + fraction * states[i].getPosition(); - maxDistance = glm::max(maxDistance, glm::distance(oldPoint, _points[i])); - } - return maxDistance; -} - -float RagDoll::enforceConstraints() { - float maxDistance = 0.0f; - const int numConstraints = _constraints.size(); - for (int i = 0; i < numConstraints; ++i) { - DistanceConstraint* c = static_cast(_constraints[i]); - //maxDistance = glm::max(maxDistance, _constraints[i]->enforce()); - maxDistance = glm::max(maxDistance, c->enforce()); - } - return maxDistance; -} - -void RagDoll::updateShapes(const QVector& shapes, const glm::quat& rotation, const glm::vec3& translation) const { - int numShapes = shapes.size(); - int numConstraints = _constraints.size(); - for (int i = 0; i < numShapes && i < numConstraints; ++i) { - _constraints[i]->updateProxyShape(shapes[i], rotation, translation); - } -} diff --git a/interface/src/renderer/RagDoll.h b/interface/src/renderer/RagDoll.h deleted file mode 100644 index 60e242d19b..0000000000 --- a/interface/src/renderer/RagDoll.h +++ /dev/null @@ -1,95 +0,0 @@ -// -// RagDoll.h -// interface/src/avatar -// -// Created by Andrew Meadows 2014.05.30 -// Copyright 2014 High Fidelity, Inc. -// -// Distributed under the Apache License, Version 2.0. -// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html -// - -#ifndef hifi_RagDoll_h -#define hifi_RagDoll_h - -#include "renderer/Model.h" - -class Shape; - -class Constraint { -public: - Constraint() {} - virtual ~Constraint() {} - - /// Enforce contraint by moving relevant points. - /// \return max distance of point movement - virtual float enforce() = 0; - - /// \param shape pointer to shape that will be this Constraint's collision proxy - /// \param rotation rotation into shape's collision frame - /// \param translation translation into shape's collision frame - /// Moves the shape such that it will collide at this constraint's position - virtual void updateProxyShape(Shape* shape, const glm::quat& rotation, const glm::vec3& translation) const {} - -protected: - int _type; -}; - -class FixedConstraint : public Constraint { -public: - FixedConstraint(glm::vec3* point, const glm::vec3& anchor); - float enforce(); - void setPoint(glm::vec3* point); - void setAnchor(const glm::vec3& anchor); -private: - glm::vec3* _point; - glm::vec3 _anchor; -}; - -class DistanceConstraint : public Constraint { -public: - DistanceConstraint(glm::vec3* startPoint, glm::vec3* endPoint); - DistanceConstraint(const DistanceConstraint& other); - float enforce(); - void setDistance(float distance); - void updateProxyShape(Shape* shape, const glm::quat& rotation, const glm::vec3& translation) const; -private: - float _distance; - glm::vec3* _points[2]; -}; - -class RagDoll { -public: - - RagDoll(); - virtual ~RagDoll(); - - /// Create points and constraints based on topology of collection of joints - /// \param joints list of connected joint states - void init(const QVector& states); - - /// Delete all data. - void clear(); - - /// \param states list of joint states - /// \param fraction range from 0.0 (no movement) to 1.0 (use joint locations) - /// \return max distance of point movement - float slaveToSkeleton(const QVector& states, float fraction); - - /// Enforce contraints. - /// \return max distance of point movement - float enforceConstraints(); - - const QVector& getPoints() const { return _points; } - - /// \param shapes list of shapes to be updated with new positions - /// \param rotation rotation into shapes' collision frame - /// \param translation translation into shapes' collision frame - void updateShapes(const QVector& shapes, const glm::quat& rotation, const glm::vec3& translation) const; - -private: - QVector _constraints; - QVector _points; -}; - -#endif // hifi_RagDoll_h diff --git a/interface/src/ui/ChatWindow.cpp b/interface/src/ui/ChatWindow.cpp index fde77334f4..a7d2c212d2 100644 --- a/interface/src/ui/ChatWindow.cpp +++ b/interface/src/ui/ChatWindow.cpp @@ -30,6 +30,9 @@ const int NUM_MESSAGES_TO_TIME_STAMP = 20; +const float OPACITY_ACTIVE = 1.0; +const float OPACITY_INACTIVE = 0.8; + const QRegularExpression regexLinks("((?:(?:ftp)|(?:https?)|(?:hifi))://\\S+)"); const QRegularExpression regexHifiLinks("([#@]\\S+)"); const QString mentionSoundsPath("/mention-sounds/"); @@ -108,7 +111,7 @@ ChatWindow::~ChatWindow() { void ChatWindow::keyPressEvent(QKeyEvent* event) { if (event->key() == Qt::Key_Escape) { - hide(); + Application::getInstance()->getWindow()->activateWindow(); } else { FramelessDialog::keyPressEvent(event); } @@ -383,3 +386,12 @@ void ChatWindow::scrollToBottom() { QScrollBar* verticalScrollBar = ui->messagesScrollArea->verticalScrollBar(); verticalScrollBar->setValue(verticalScrollBar->maximum()); } + +bool ChatWindow::event(QEvent* event) { + if (event->type() == QEvent::WindowActivate) { + setWindowOpacity(OPACITY_ACTIVE); + } else if (event->type() == QEvent::WindowDeactivate) { + setWindowOpacity(OPACITY_INACTIVE); + } + return FramelessDialog::event(event); +} diff --git a/interface/src/ui/ChatWindow.h b/interface/src/ui/ChatWindow.h index 1e0f533e9e..652dcb5b08 100644 --- a/interface/src/ui/ChatWindow.h +++ b/interface/src/ui/ChatWindow.h @@ -50,6 +50,7 @@ protected: virtual void keyPressEvent(QKeyEvent *event); virtual void showEvent(QShowEvent* event); + virtual bool event(QEvent* event); private: #ifdef HAVE_QXMPP diff --git a/libraries/audio/src/AudioRingBuffer.cpp b/libraries/audio/src/AudioRingBuffer.cpp index 2101fcb9cd..ee4027841b 100644 --- a/libraries/audio/src/AudioRingBuffer.cpp +++ b/libraries/audio/src/AudioRingBuffer.cpp @@ -21,6 +21,7 @@ AudioRingBuffer::AudioRingBuffer(int numFrameSamples, bool randomAccessMode) : NodeData(), + _resetCount(0), _sampleCapacity(numFrameSamples * RING_BUFFER_LENGTH_FRAMES), _numFrameSamples(numFrameSamples), _isStarved(true), @@ -122,19 +123,15 @@ qint64 AudioRingBuffer::writeData(const char* data, qint64 maxSize) { int samplesToCopy = std::min((quint64)(maxSize / sizeof(int16_t)), (quint64)_sampleCapacity); - std::less less; - std::less_equal lessEqual; - - if (_hasStarted - && (less(_endOfLastWrite, _nextOutput) - && lessEqual(_nextOutput, shiftedPositionAccomodatingWrap(_endOfLastWrite, samplesToCopy)))) { + if (_hasStarted && samplesToCopy > _sampleCapacity - samplesAvailable()) { // this read will cross the next output, so call us starved and reset the buffer qDebug() << "Filled the ring buffer. Resetting."; _endOfLastWrite = _buffer; _nextOutput = _buffer; _isStarved = true; + _resetCount++; } - + if (_endOfLastWrite + samplesToCopy <= _buffer + _sampleCapacity) { memcpy(_endOfLastWrite, data, samplesToCopy * sizeof(int16_t)); } else { @@ -144,7 +141,7 @@ qint64 AudioRingBuffer::writeData(const char* data, qint64 maxSize) { } _endOfLastWrite = shiftedPositionAccomodatingWrap(_endOfLastWrite, samplesToCopy); - + return samplesToCopy * sizeof(int16_t); } diff --git a/libraries/audio/src/AudioRingBuffer.h b/libraries/audio/src/AudioRingBuffer.h index 04cc67c8ac..33fb0d238a 100644 --- a/libraries/audio/src/AudioRingBuffer.h +++ b/libraries/audio/src/AudioRingBuffer.h @@ -31,7 +31,7 @@ const int NETWORK_BUFFER_LENGTH_SAMPLES_PER_CHANNEL = NETWORK_BUFFER_LENGTH_BYTE const unsigned int BUFFER_SEND_INTERVAL_USECS = floorf((NETWORK_BUFFER_LENGTH_SAMPLES_PER_CHANNEL / (float) SAMPLE_RATE) * 1000 * 1000); -const short RING_BUFFER_LENGTH_FRAMES = 10; +const short RING_BUFFER_LENGTH_FRAMES = 100; const int MAX_SAMPLE_VALUE = std::numeric_limits::max(); const int MIN_SAMPLE_VALUE = std::numeric_limits::min(); @@ -71,6 +71,7 @@ public: bool isStarved() const { return _isStarved; } void setIsStarved(bool isStarved) { _isStarved = isStarved; } + int getResetCount() const { return _resetCount; } /// how many times has the ring buffer written past the end and reset bool hasStarted() const { return _hasStarted; } void addSilentFrame(int numSilentSamples); @@ -80,6 +81,8 @@ protected: AudioRingBuffer& operator= (const AudioRingBuffer&); int16_t* shiftedPositionAccomodatingWrap(int16_t* position, int numSamplesShift) const; + + int _resetCount; /// how many times has the ring buffer written past the end and done a reset int _sampleCapacity; int _numFrameSamples; diff --git a/libraries/audio/src/InjectedAudioRingBuffer.cpp b/libraries/audio/src/InjectedAudioRingBuffer.cpp index 2658b4c336..9b6529b49f 100644 --- a/libraries/audio/src/InjectedAudioRingBuffer.cpp +++ b/libraries/audio/src/InjectedAudioRingBuffer.cpp @@ -19,8 +19,8 @@ #include "InjectedAudioRingBuffer.h" -InjectedAudioRingBuffer::InjectedAudioRingBuffer(const QUuid& streamIdentifier) : - PositionalAudioRingBuffer(PositionalAudioRingBuffer::Injector), +InjectedAudioRingBuffer::InjectedAudioRingBuffer(const QUuid& streamIdentifier, bool dynamicJitterBuffer) : + PositionalAudioRingBuffer(PositionalAudioRingBuffer::Injector, /* isStereo=*/ false , dynamicJitterBuffer), _streamIdentifier(streamIdentifier), _radius(0.0f), _attenuationRatio(0) @@ -31,6 +31,9 @@ InjectedAudioRingBuffer::InjectedAudioRingBuffer(const QUuid& streamIdentifier) const uchar MAX_INJECTOR_VOLUME = 255; int InjectedAudioRingBuffer::parseData(const QByteArray& packet) { + _interframeTimeGapStats.frameReceived(); + updateDesiredJitterBufferFrames(); + // setup a data stream to read from this packet QDataStream packetStream(packet); packetStream.skipRawData(numBytesForPacketHeader(packet)); diff --git a/libraries/audio/src/InjectedAudioRingBuffer.h b/libraries/audio/src/InjectedAudioRingBuffer.h index fd766e2848..4e3fea672b 100644 --- a/libraries/audio/src/InjectedAudioRingBuffer.h +++ b/libraries/audio/src/InjectedAudioRingBuffer.h @@ -18,7 +18,7 @@ class InjectedAudioRingBuffer : public PositionalAudioRingBuffer { public: - InjectedAudioRingBuffer(const QUuid& streamIdentifier = QUuid()); + InjectedAudioRingBuffer(const QUuid& streamIdentifier = QUuid(), bool dynamicJitterBuffer = false); int parseData(const QByteArray& packet); diff --git a/libraries/audio/src/PositionalAudioRingBuffer.cpp b/libraries/audio/src/PositionalAudioRingBuffer.cpp index 1cc4147175..23e258fe87 100644 --- a/libraries/audio/src/PositionalAudioRingBuffer.cpp +++ b/libraries/audio/src/PositionalAudioRingBuffer.cpp @@ -19,8 +19,75 @@ #include #include "PositionalAudioRingBuffer.h" +#include "SharedUtil.h" -PositionalAudioRingBuffer::PositionalAudioRingBuffer(PositionalAudioRingBuffer::Type type, bool isStereo) : +InterframeTimeGapStats::InterframeTimeGapStats() + : _lastFrameReceivedTime(0), + _numSamplesInCurrentInterval(0), + _currentIntervalMaxGap(0), + _newestIntervalMaxGapAt(0), + _windowMaxGap(0), + _newWindowMaxGapAvailable(false) +{ + memset(_intervalMaxGaps, 0, TIME_GAP_NUM_INTERVALS_IN_WINDOW * sizeof(quint64)); +} + +void InterframeTimeGapStats::frameReceived() { + quint64 now = usecTimestampNow(); + + // make sure this isn't the first time frameReceived() is called so can actually calculate a gap. + if (_lastFrameReceivedTime != 0) { + quint64 gap = now - _lastFrameReceivedTime; + + // update the current interval max + if (gap > _currentIntervalMaxGap) { + _currentIntervalMaxGap = gap; + + // keep the window max gap at least as large as the current interval max + // this allows the window max gap to respond immediately to a sudden spike in gap times + // also, this prevents the window max gap from staying at 0 until the first interval of samples filled up + if (_currentIntervalMaxGap > _windowMaxGap) { + _windowMaxGap = _currentIntervalMaxGap; + _newWindowMaxGapAvailable = true; + } + } + _numSamplesInCurrentInterval++; + + // if the current interval of samples is now full, record it in our interval maxes + if (_numSamplesInCurrentInterval == TIME_GAP_NUM_SAMPLES_IN_INTERVAL) { + + // find location to insert this interval's max (increment index cyclically) + _newestIntervalMaxGapAt = _newestIntervalMaxGapAt == TIME_GAP_NUM_INTERVALS_IN_WINDOW - 1 ? 0 : _newestIntervalMaxGapAt + 1; + + // record the current interval's max gap as the newest + _intervalMaxGaps[_newestIntervalMaxGapAt] = _currentIntervalMaxGap; + + // update the window max gap, which is the max out of all the past intervals' max gaps + _windowMaxGap = 0; + for (int i = 0; i < TIME_GAP_NUM_INTERVALS_IN_WINDOW; i++) { + if (_intervalMaxGaps[i] > _windowMaxGap) { + _windowMaxGap = _intervalMaxGaps[i]; + } + } + _newWindowMaxGapAvailable = true; + + // reset the current interval + _numSamplesInCurrentInterval = 0; + _currentIntervalMaxGap = 0; + } + } + _lastFrameReceivedTime = now; +} + +quint64 InterframeTimeGapStats::getWindowMaxGap() { + _newWindowMaxGapAvailable = false; + return _windowMaxGap; +} + + +PositionalAudioRingBuffer::PositionalAudioRingBuffer(PositionalAudioRingBuffer::Type type, + bool isStereo, bool dynamicJitterBuffers) : + AudioRingBuffer(isStereo ? NETWORK_BUFFER_LENGTH_SAMPLES_STEREO : NETWORK_BUFFER_LENGTH_SAMPLES_PER_CHANNEL), _type(type), _position(0.0f, 0.0f, 0.0f), @@ -29,9 +96,11 @@ PositionalAudioRingBuffer::PositionalAudioRingBuffer(PositionalAudioRingBuffer:: _shouldLoopbackForNode(false), _shouldOutputStarveDebug(true), _isStereo(isStereo), - _listenerUnattenuatedZone(NULL) + _listenerUnattenuatedZone(NULL), + _desiredJitterBufferFrames(1), + _currentJitterBufferFrames(0), + _dynamicJitterBuffers(dynamicJitterBuffers) { - } int PositionalAudioRingBuffer::parseData(const QByteArray& packet) { @@ -53,14 +122,35 @@ int PositionalAudioRingBuffer::parseData(const QByteArray& packet) { readBytes += sizeof(int16_t); + // NOTE: fixes a bug in old clients that would send garbage for their number of silentSamples + numSilentSamples = getSamplesPerFrame(); + if (numSilentSamples > 0) { - addSilentFrame(numSilentSamples); + if (_currentJitterBufferFrames > _desiredJitterBufferFrames) { + // our current jitter buffer size exceeds its desired value, so ignore some silent + // frames to get that size as close to desired as possible + int samplesPerFrame = getSamplesPerFrame(); + int numSilentFrames = numSilentSamples / samplesPerFrame; + int numFramesToDropDesired = _currentJitterBufferFrames - _desiredJitterBufferFrames; + + if (numSilentFrames > numFramesToDropDesired) { + // we have more than enough frames to drop to get the jitter buffer to its desired length + int numSilentFramesToAdd = numSilentFrames - numFramesToDropDesired; + addSilentFrame(numSilentFramesToAdd * samplesPerFrame); + _currentJitterBufferFrames = _desiredJitterBufferFrames; + + } else { + // we need to drop all frames to get the jitter buffer close as possible to its desired length + _currentJitterBufferFrames -= numSilentFrames; + } + } else { + addSilentFrame(numSilentSamples); + } } } else { // there is audio data to read readBytes += writeData(packet.data() + readBytes, packet.size() - readBytes); } - return readBytes; } @@ -106,29 +196,72 @@ void PositionalAudioRingBuffer::updateNextOutputTrailingLoudness() { } } -bool PositionalAudioRingBuffer::shouldBeAddedToMix(int numJitterBufferSamples) { - if (!isNotStarvedOrHasMinimumSamples(NETWORK_BUFFER_LENGTH_SAMPLES_PER_CHANNEL + numJitterBufferSamples)) { +bool PositionalAudioRingBuffer::shouldBeAddedToMix() { + int samplesPerFrame = getSamplesPerFrame(); + int desiredJitterBufferSamples = _desiredJitterBufferFrames * samplesPerFrame; + + if (!isNotStarvedOrHasMinimumSamples(samplesPerFrame + desiredJitterBufferSamples)) { + // if the buffer was starved, allow it to accrue at least the desired number of + // jitter buffer frames before we start taking frames from it for mixing + if (_shouldOutputStarveDebug) { _shouldOutputStarveDebug = false; } - - return false; - } else if (samplesAvailable() < NETWORK_BUFFER_LENGTH_SAMPLES_PER_CHANNEL) { + + return false; + } else if (samplesAvailable() < samplesPerFrame) { + // if the buffer doesn't have a full frame of samples to take for mixing, it is starved _isStarved = true; + // set to 0 to indicate the jitter buffer is starved + _currentJitterBufferFrames = 0; + // reset our _shouldOutputStarveDebug to true so the next is printed _shouldOutputStarveDebug = true; - + return false; - } else { - // good buffer, add this to the mix + } + + // good buffer, add this to the mix + if (_isStarved) { + // if this buffer has just finished replenishing after being starved, the number of frames in it now + // minus one (since a frame will be read immediately after this) is the length of the jitter buffer + _currentJitterBufferFrames = samplesAvailable() / samplesPerFrame - 1; _isStarved = false; - - // since we've read data from ring buffer at least once - we've started - _hasStarted = true; - - return true; } - return false; + // since we've read data from ring buffer at least once - we've started + _hasStarted = true; + + return true; +} + +int PositionalAudioRingBuffer::getCalculatedDesiredJitterBufferFrames() const { + int calculatedDesiredJitterBufferFrames = 1; + const float USECS_PER_FRAME = NETWORK_BUFFER_LENGTH_SAMPLES_PER_CHANNEL * USECS_PER_SECOND / (float)SAMPLE_RATE; + + calculatedDesiredJitterBufferFrames = ceilf((float)_interframeTimeGapStats.peekWindowMaxGap() / USECS_PER_FRAME); + if (calculatedDesiredJitterBufferFrames < 1) { + calculatedDesiredJitterBufferFrames = 1; + } + return calculatedDesiredJitterBufferFrames; +} + +void PositionalAudioRingBuffer::updateDesiredJitterBufferFrames() { + if (_interframeTimeGapStats.hasNewWindowMaxGapAvailable()) { + if (!_dynamicJitterBuffers) { + _desiredJitterBufferFrames = 1; // HACK to see if this fixes the audio silence + } else { + const float USECS_PER_FRAME = NETWORK_BUFFER_LENGTH_SAMPLES_PER_CHANNEL * USECS_PER_SECOND / (float)SAMPLE_RATE; + + _desiredJitterBufferFrames = ceilf((float)_interframeTimeGapStats.getWindowMaxGap() / USECS_PER_FRAME); + if (_desiredJitterBufferFrames < 1) { + _desiredJitterBufferFrames = 1; + } + const int maxDesired = RING_BUFFER_LENGTH_FRAMES - 1; + if (_desiredJitterBufferFrames > maxDesired) { + _desiredJitterBufferFrames = maxDesired; + } + } + } } diff --git a/libraries/audio/src/PositionalAudioRingBuffer.h b/libraries/audio/src/PositionalAudioRingBuffer.h index 00362c245a..b204dc766b 100644 --- a/libraries/audio/src/PositionalAudioRingBuffer.h +++ b/libraries/audio/src/PositionalAudioRingBuffer.h @@ -18,6 +18,31 @@ #include "AudioRingBuffer.h" +// this means that every 500 samples, the max for the past 10*500 samples will be calculated +const int TIME_GAP_NUM_SAMPLES_IN_INTERVAL = 500; +const int TIME_GAP_NUM_INTERVALS_IN_WINDOW = 10; + +// class used to track time between incoming frames for the purpose of varying the jitter buffer length +class InterframeTimeGapStats { +public: + InterframeTimeGapStats(); + + void frameReceived(); + bool hasNewWindowMaxGapAvailable() const { return _newWindowMaxGapAvailable; } + quint64 peekWindowMaxGap() const { return _windowMaxGap; } + quint64 getWindowMaxGap(); + +private: + quint64 _lastFrameReceivedTime; + + int _numSamplesInCurrentInterval; + quint64 _currentIntervalMaxGap; + quint64 _intervalMaxGaps[TIME_GAP_NUM_INTERVALS_IN_WINDOW]; + int _newestIntervalMaxGapAt; + quint64 _windowMaxGap; + bool _newWindowMaxGapAvailable; +}; + class PositionalAudioRingBuffer : public AudioRingBuffer { public: enum Type { @@ -25,7 +50,7 @@ public: Injector }; - PositionalAudioRingBuffer(PositionalAudioRingBuffer::Type type, bool isStereo = false); + PositionalAudioRingBuffer(PositionalAudioRingBuffer::Type type, bool isStereo = false, bool dynamicJitterBuffers = false); int parseData(const QByteArray& packet); int parsePositionalData(const QByteArray& positionalByteArray); @@ -34,7 +59,7 @@ public: void updateNextOutputTrailingLoudness(); float getNextOutputTrailingLoudness() const { return _nextOutputTrailingLoudness; } - bool shouldBeAddedToMix(int numJitterBufferSamples); + bool shouldBeAddedToMix(); bool willBeAddedToMix() const { return _willBeAddedToMix; } void setWillBeAddedToMix(bool willBeAddedToMix) { _willBeAddedToMix = willBeAddedToMix; } @@ -50,10 +75,18 @@ public: AABox* getListenerUnattenuatedZone() const { return _listenerUnattenuatedZone; } void setListenerUnattenuatedZone(AABox* listenerUnattenuatedZone) { _listenerUnattenuatedZone = listenerUnattenuatedZone; } + int getSamplesPerFrame() const { return _isStereo ? NETWORK_BUFFER_LENGTH_SAMPLES_STEREO : NETWORK_BUFFER_LENGTH_SAMPLES_PER_CHANNEL; } + + int getCalculatedDesiredJitterBufferFrames() const; /// returns what we would calculate our desired as if asked + int getDesiredJitterBufferFrames() const { return _desiredJitterBufferFrames; } + int getCurrentJitterBufferFrames() const { return _currentJitterBufferFrames; } + protected: // disallow copying of PositionalAudioRingBuffer objects PositionalAudioRingBuffer(const PositionalAudioRingBuffer&); PositionalAudioRingBuffer& operator= (const PositionalAudioRingBuffer&); + + void updateDesiredJitterBufferFrames(); PositionalAudioRingBuffer::Type _type; glm::vec3 _position; @@ -65,6 +98,11 @@ protected: float _nextOutputTrailingLoudness; AABox* _listenerUnattenuatedZone; + + InterframeTimeGapStats _interframeTimeGapStats; + int _desiredJitterBufferFrames; + int _currentJitterBufferFrames; + bool _dynamicJitterBuffers; }; #endif // hifi_PositionalAudioRingBuffer_h diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index 8f658678b5..4c7136fd0a 100755 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -223,7 +223,7 @@ public: virtual const glm::vec3& getVelocity() const { return vec3Zero; } - virtual bool findParticleCollisions(const glm::vec3& particleCenter, float particleRadius, CollisionList& collisions) { + virtual bool findSphereCollisions(const glm::vec3& particleCenter, float particleRadius, CollisionList& collisions) { return false; } diff --git a/libraries/fbx/src/FBXReader.cpp b/libraries/fbx/src/FBXReader.cpp index 9aeb81a2a3..33b7fca0ef 100644 --- a/libraries/fbx/src/FBXReader.cpp +++ b/libraries/fbx/src/FBXReader.cpp @@ -1717,7 +1717,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) glm::vec3 boneEnd = extractTranslation(transformJointToMesh); glm::vec3 boneBegin = boneEnd; glm::vec3 boneDirection; - float boneLength; + float boneLength = 0.0f; if (joint.parentIndex != -1) { boneBegin = extractTranslation(inverseModelTransform * geometry.joints[joint.parentIndex].bindTransform); boneDirection = boneEnd - boneBegin; @@ -1779,7 +1779,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) glm::vec3 boneBegin = boneEnd; glm::vec3 boneDirection; - float boneLength; + float boneLength = 0.0f; if (joint.parentIndex != -1) { boneBegin = extractTranslation(inverseModelTransform * geometry.joints[joint.parentIndex].bindTransform); boneDirection = boneEnd - boneBegin; diff --git a/libraries/metavoxels/src/AttributeRegistry.cpp b/libraries/metavoxels/src/AttributeRegistry.cpp index e7a7f41850..33ce298859 100644 --- a/libraries/metavoxels/src/AttributeRegistry.cpp +++ b/libraries/metavoxels/src/AttributeRegistry.cpp @@ -211,6 +211,11 @@ void Attribute::writeMetavoxelSubdivision(const MetavoxelNode& root, MetavoxelSt root.writeSubdivision(state); } +bool Attribute::metavoxelRootsEqual(const MetavoxelNode& firstRoot, const MetavoxelNode& secondRoot, + const glm::vec3& minimum, float size, const MetavoxelLOD& lod) { + return firstRoot.deepEquals(this, secondRoot, minimum, size, lod); +} + FloatAttribute::FloatAttribute(const QString& name, float defaultValue) : SimpleInlineAttribute(name, defaultValue) { } @@ -449,6 +454,12 @@ void SharedObjectAttribute::write(Bitstream& out, void* value, bool isLeaf) cons } } +bool SharedObjectAttribute::deepEqual(void* first, void* second) const { + SharedObjectPointer firstObject = decodeInline(first); + SharedObjectPointer secondObject = decodeInline(second); + return firstObject ? firstObject->equals(secondObject) : !secondObject; +} + bool SharedObjectAttribute::merge(void*& parent, void* children[], bool postRead) const { SharedObjectPointer firstChild = decodeInline(children[0]); for (int i = 1; i < MERGE_COUNT; i++) { @@ -489,6 +500,35 @@ MetavoxelNode* SharedObjectSetAttribute::createMetavoxelNode( return new MetavoxelNode(value, original); } +static bool setsEqual(const SharedObjectSet& firstSet, const SharedObjectSet& secondSet) { + if (firstSet.size() != secondSet.size()) { + return false; + } + // some hackiness here: we assume that the local ids of the first set correspond to the remote ids of the second, + // so that this will work with the tests + foreach (const SharedObjectPointer& firstObject, firstSet) { + int id = firstObject->getID(); + bool found = false; + foreach (const SharedObjectPointer& secondObject, secondSet) { + if (secondObject->getRemoteID() == id) { + if (!firstObject->equals(secondObject)) { + return false; + } + found = true; + break; + } + } + if (!found) { + return false; + } + } + return true; +} + +bool SharedObjectSetAttribute::deepEqual(void* first, void* second) const { + return setsEqual(decodeInline(first), decodeInline(second)); +} + bool SharedObjectSetAttribute::merge(void*& parent, void* children[], bool postRead) const { for (int i = 0; i < MERGE_COUNT; i++) { if (!decodeInline(children[i]).isEmpty()) { @@ -563,3 +603,12 @@ void SpannerSetAttribute::writeMetavoxelSubdivision(const MetavoxelNode& root, M state.stream << SharedObjectPointer(); } +bool SpannerSetAttribute::metavoxelRootsEqual(const MetavoxelNode& firstRoot, const MetavoxelNode& secondRoot, + const glm::vec3& minimum, float size, const MetavoxelLOD& lod) { + + SharedObjectSet firstSet; + firstRoot.getSpanners(this, minimum, size, lod, firstSet); + SharedObjectSet secondSet; + secondRoot.getSpanners(this, minimum, size, lod, secondSet); + return setsEqual(firstSet, secondSet); +} diff --git a/libraries/metavoxels/src/AttributeRegistry.h b/libraries/metavoxels/src/AttributeRegistry.h index 00d974b8b6..7dc2e110b8 100644 --- a/libraries/metavoxels/src/AttributeRegistry.h +++ b/libraries/metavoxels/src/AttributeRegistry.h @@ -27,6 +27,7 @@ class QScriptValue; class Attribute; class MetavoxelData; +class MetavoxelLOD; class MetavoxelNode; class MetavoxelStreamState; @@ -213,6 +214,11 @@ public: virtual bool equal(void* first, void* second) const = 0; + virtual bool deepEqual(void* first, void* second) const { return equal(first, second); } + + virtual bool metavoxelRootsEqual(const MetavoxelNode& firstRoot, const MetavoxelNode& secondRoot, + const glm::vec3& minimum, float size, const MetavoxelLOD& lod); + /// Merges the value of a parent and its children. /// \param postRead whether or not the merge is happening after a read /// \return whether or not the children and parent values are all equal @@ -406,6 +412,8 @@ public: virtual void read(Bitstream& in, void*& value, bool isLeaf) const; virtual void write(Bitstream& out, void* value, bool isLeaf) const; + virtual bool deepEqual(void* first, void* second) const; + virtual bool merge(void*& parent, void* children[], bool postRead = false) const; virtual void* createFromVariant(const QVariant& value) const; @@ -434,6 +442,8 @@ public: virtual MetavoxelNode* createMetavoxelNode(const AttributeValue& value, const MetavoxelNode* original) const; + virtual bool deepEqual(void* first, void* second) const; + virtual bool merge(void*& parent, void* children[], bool postRead = false) const; virtual AttributeValue inherit(const AttributeValue& parentValue) const; @@ -462,6 +472,9 @@ public: virtual void readMetavoxelSubdivision(MetavoxelData& data, MetavoxelStreamState& state); virtual void writeMetavoxelSubdivision(const MetavoxelNode& root, MetavoxelStreamState& state); + + virtual bool metavoxelRootsEqual(const MetavoxelNode& firstRoot, const MetavoxelNode& secondRoot, + const glm::vec3& minimum, float size, const MetavoxelLOD& lod); }; #endif // hifi_AttributeRegistry_h diff --git a/libraries/metavoxels/src/Bitstream.cpp b/libraries/metavoxels/src/Bitstream.cpp index 44342abe33..0ef86e85c0 100644 --- a/libraries/metavoxels/src/Bitstream.cpp +++ b/libraries/metavoxels/src/Bitstream.cpp @@ -110,6 +110,10 @@ const TypeStreamer* Bitstream::getTypeStreamer(int type) { return getTypeStreamers().value(type); } +const ObjectStreamer* Bitstream::getObjectStreamer(const QMetaObject* metaObject) { + return getObjectStreamers().value(metaObject); +} + const QMetaObject* Bitstream::getMetaObject(const QByteArray& className) { return getMetaObjects().value(className); } @@ -2316,6 +2320,15 @@ QObject* MappedObjectStreamer::putJSONData(JSONReader& reader, const QJsonObject return object; } +bool MappedObjectStreamer::equal(const QObject* first, const QObject* second) const { + foreach (const StreamerPropertyPair& property, _properties) { + if (!property.first->equal(property.second.read(first), property.second.read(second))) { + return false; + } + } + return true; +} + void MappedObjectStreamer::write(Bitstream& out, const QObject* object) const { foreach (const StreamerPropertyPair& property, _properties) { property.first->write(out, property.second.read(object)); @@ -2433,6 +2446,17 @@ QObject* GenericObjectStreamer::putJSONData(JSONReader& reader, const QJsonObjec return object; } +bool GenericObjectStreamer::equal(const QObject* first, const QObject* second) const { + const QVariantList& firstValues = static_cast(first)->getValues(); + const QVariantList& secondValues = static_cast(second)->getValues(); + for (int i = 0; i < _properties.size(); i++) { + if (!_properties.at(i).first->equal(firstValues.at(i), secondValues.at(i))) { + return false; + } + } + return true; +} + void GenericObjectStreamer::write(Bitstream& out, const QObject* object) const { const QVariantList& values = static_cast(object)->getValues(); for (int i = 0; i < _properties.size(); i++) { diff --git a/libraries/metavoxels/src/Bitstream.h b/libraries/metavoxels/src/Bitstream.h index 0d9e516640..e32f93dbe2 100644 --- a/libraries/metavoxels/src/Bitstream.h +++ b/libraries/metavoxels/src/Bitstream.h @@ -278,6 +278,9 @@ public: /// Returns the streamer registered for the supplied type, if any. static const TypeStreamer* getTypeStreamer(int type); + /// Returns the streamer registered for the supplied object, if any. + static const ObjectStreamer* getObjectStreamer(const QMetaObject* metaObject); + /// Returns the meta-object registered under the supplied class name, if any. static const QMetaObject* getMetaObject(const QByteArray& className); @@ -1022,6 +1025,7 @@ public: virtual QJsonObject getJSONData(JSONWriter& writer, const QObject* object) const = 0; virtual QObject* putJSONData(JSONReader& reader, const QJsonObject& jsonObject) const = 0; + virtual bool equal(const QObject* first, const QObject* second) const = 0; virtual void write(Bitstream& out, const QObject* object) const = 0; virtual void writeRawDelta(Bitstream& out, const QObject* object, const QObject* reference) const = 0; virtual QObject* read(Bitstream& in, QObject* object = NULL) const = 0; @@ -1047,6 +1051,7 @@ public: virtual QJsonObject getJSONMetadata(JSONWriter& writer) const; virtual QJsonObject getJSONData(JSONWriter& writer, const QObject* object) const; virtual QObject* putJSONData(JSONReader& reader, const QJsonObject& jsonObject) const; + virtual bool equal(const QObject* first, const QObject* second) const; virtual void write(Bitstream& out, const QObject* object) const; virtual void writeRawDelta(Bitstream& out, const QObject* object, const QObject* reference) const; virtual QObject* read(Bitstream& in, QObject* object = NULL) const; @@ -1070,6 +1075,7 @@ public: virtual QJsonObject getJSONMetadata(JSONWriter& writer) const; virtual QJsonObject getJSONData(JSONWriter& writer, const QObject* object) const; virtual QObject* putJSONData(JSONReader& reader, const QJsonObject& jsonObject) const; + virtual bool equal(const QObject* first, const QObject* second) const; virtual void write(Bitstream& out, const QObject* object) const; virtual void writeRawDelta(Bitstream& out, const QObject* object, const QObject* reference) const; virtual QObject* read(Bitstream& in, QObject* object = NULL) const; @@ -1104,7 +1110,7 @@ private: Q_DECLARE_METATYPE(const QMetaObject*) /// Macro for registering streamable meta-objects. Typically, one would use this macro at the top level of the source file -/// associated with the class. +/// associated with the class. The class should have a no-argument constructor flagged with Q_INVOKABLE. #define REGISTER_META_OBJECT(x) static int x##Registration = Bitstream::registerMetaObject(#x, &x::staticMetaObject); /// Contains a value along with a pointer to its streamer. This is stored in QVariants when using fallback generics and @@ -1563,8 +1569,8 @@ public: Bitstream::registerTypeStreamer(qMetaTypeId(), new CollectionTypeStreamer()); /// Declares the metatype and the streaming operators. Typically, one would use this immediately after the definition of a -/// type flagged as STREAMABLE in its header file. The last lines ensure that the generated file will be included in the link -/// phase. +/// type flagged as STREAMABLE in its header file. The type should have a no-argument constructor. The last lines of this +/// macro ensure that the generated file will be included in the link phase. #ifdef _WIN32 #define DECLARE_STREAMABLE_METATYPE(X) Q_DECLARE_METATYPE(X) \ Bitstream& operator<<(Bitstream& out, const X& obj); \ diff --git a/libraries/metavoxels/src/MetavoxelData.cpp b/libraries/metavoxels/src/MetavoxelData.cpp index 0d52fc5ed6..3e70e0f09b 100644 --- a/libraries/metavoxels/src/MetavoxelData.cpp +++ b/libraries/metavoxels/src/MetavoxelData.cpp @@ -610,6 +610,23 @@ MetavoxelNode* MetavoxelData::createRoot(const AttributePointer& attribute) { return root = new MetavoxelNode(attribute); } +bool MetavoxelData::deepEquals(const MetavoxelData& other, const MetavoxelLOD& lod) const { + if (_size != other._size) { + return false; + } + if (_roots.size() != other._roots.size()) { + return false; + } + glm::vec3 minimum = getMinimum(); + for (QHash::const_iterator it = _roots.constBegin(); it != _roots.constEnd(); it++) { + MetavoxelNode* otherNode = other._roots.value(it.key()); + if (!(otherNode && it.key()->metavoxelRootsEqual(*it.value(), *otherNode, minimum, _size, lod))) { + return false; + } + } + return true; +} + bool MetavoxelData::operator==(const MetavoxelData& other) const { return _size == other._size && _roots == other._roots; } @@ -1006,6 +1023,44 @@ void MetavoxelNode::clearChildren(const AttributePointer& attribute) { } } +bool MetavoxelNode::deepEquals(const AttributePointer& attribute, const MetavoxelNode& other, + const glm::vec3& minimum, float size, const MetavoxelLOD& lod) const { + if (!attribute->deepEqual(_attributeValue, other._attributeValue)) { + return false; + } + if (!lod.shouldSubdivide(minimum, size, attribute->getLODThresholdMultiplier())) { + return true; + } + bool leaf = isLeaf(), otherLeaf = other.isLeaf(); + if (leaf && otherLeaf) { + return true; + } + if (leaf || otherLeaf) { + return false; + } + float nextSize = size * 0.5f; + for (int i = 0; i < CHILD_COUNT; i++) { + glm::vec3 nextMinimum = getNextMinimum(minimum, nextSize, i); + if (!_children[i]->deepEquals(attribute, *(other._children[i]), nextMinimum, nextSize, lod)) { + return false; + } + } + return true; +} + +void MetavoxelNode::getSpanners(const AttributePointer& attribute, const glm::vec3& minimum, + float size, const MetavoxelLOD& lod, SharedObjectSet& results) const { + results.unite(decodeInline(_attributeValue)); + if (isLeaf() || !lod.shouldSubdivide(minimum, size, attribute->getLODThresholdMultiplier())) { + return; + } + float nextSize = size * 0.5f; + for (int i = 0; i < CHILD_COUNT; i++) { + glm::vec3 nextMinimum = getNextMinimum(minimum, nextSize, i); + _children[i]->getSpanners(attribute, nextMinimum, nextSize, lod, results); + } +} + int MetavoxelVisitor::encodeOrder(int first, int second, int third, int fourth, int fifth, int sixth, int seventh, int eighth) { return first | (second << 3) | (third << 6) | (fourth << 9) | @@ -1034,6 +1089,25 @@ int MetavoxelVisitor::encodeOrder(const glm::vec3& direction) { indexDistances.at(6).index, indexDistances.at(7).index); } +const int ORDER_ELEMENT_BITS = 3; +const int ORDER_ELEMENT_MASK = (1 << ORDER_ELEMENT_BITS) - 1; + +int MetavoxelVisitor::encodeRandomOrder() { + // see http://en.wikipedia.org/wiki/Fisher%E2%80%93Yates_shuffle#The_.22inside-out.22_algorithm + int order; + int randomValues = rand(); + for (int i = 0, iShift = 0; i < MetavoxelNode::CHILD_COUNT; i++, iShift += ORDER_ELEMENT_BITS) { + int j = (randomValues >> iShift) % (i + 1); + int jShift = j * ORDER_ELEMENT_BITS; + if (j != i) { + int jValue = (order >> jShift) & ORDER_ELEMENT_MASK; + order = (order & ~(ORDER_ELEMENT_MASK << iShift)) | (jValue << iShift); + } + order = (order & ~(ORDER_ELEMENT_MASK << jShift)) | (i << jShift); + } + return order; +} + const int MetavoxelVisitor::DEFAULT_ORDER = encodeOrder(0, 1, 2, 3, 4, 5, 6, 7); const int MetavoxelVisitor::STOP_RECURSION = 0; const int MetavoxelVisitor::SHORT_CIRCUIT = -1; @@ -1227,8 +1301,6 @@ bool DefaultMetavoxelGuide::guide(MetavoxelVisitation& visitation) { QVector(visitation.outputNodes.size()) } }; for (int i = 0; i < MetavoxelNode::CHILD_COUNT; i++) { // the encoded order tells us the child indices for each iteration - const int ORDER_ELEMENT_BITS = 3; - const int ORDER_ELEMENT_MASK = (1 << ORDER_ELEMENT_BITS) - 1; int index = encodedOrder & ORDER_ELEMENT_MASK; encodedOrder >>= ORDER_ELEMENT_BITS; for (int j = 0; j < visitation.inputNodes.size(); j++) { @@ -1269,7 +1341,7 @@ bool DefaultMetavoxelGuide::guide(MetavoxelVisitation& visitation) { } } MetavoxelNode* node = visitation.outputNodes.at(j); - MetavoxelNode* child = node->getChild(i); + MetavoxelNode* child = node->getChild(index); if (child) { child->decrementReferenceCount(value.getAttribute()); } else { diff --git a/libraries/metavoxels/src/MetavoxelData.h b/libraries/metavoxels/src/MetavoxelData.h index 2e6f6c4437..6a7ba33eb5 100644 --- a/libraries/metavoxels/src/MetavoxelData.h +++ b/libraries/metavoxels/src/MetavoxelData.h @@ -34,7 +34,8 @@ class NetworkValue; class Spanner; class SpannerRenderer; -/// Determines whether to subdivide each node when traversing. +/// Determines whether to subdivide each node when traversing. Contains the position (presumed to be of the viewer) and a +/// threshold value, where lower thresholds cause smaller/more distant voxels to be subdivided. class MetavoxelLOD { STREAMABLE @@ -46,6 +47,7 @@ public: bool isValid() const { return threshold > 0.0f; } + /// Checks whether, according to this LOD, we should subdivide the described voxel. bool shouldSubdivide(const glm::vec3& minimum, float size, float multiplier = 1.0f) const; /// Checks whether the node or any of the nodes underneath it have had subdivision enabled as compared to the reference. @@ -54,7 +56,8 @@ public: DECLARE_STREAMABLE_METATYPE(MetavoxelLOD) -/// The base metavoxel representation shared between server and client. +/// The base metavoxel representation shared between server and client. Contains a size (for all dimensions) and a set of +/// octrees for different attributes. class MetavoxelData { public: @@ -64,30 +67,38 @@ public: MetavoxelData& operator=(const MetavoxelData& other); + /// Sets the size in all dimensions. void setSize(float size) { _size = size; } float getSize() const { return _size; } + /// Returns the minimum extent of the octrees (which are centered about the origin). glm::vec3 getMinimum() const { return glm::vec3(_size, _size, _size) * -0.5f; } + /// Returns the bounds of the octrees. Box getBounds() const; /// Applies the specified visitor to the contained voxels. void guide(MetavoxelVisitor& visitor); + /// Inserts a spanner into the specified attribute layer. void insert(const AttributePointer& attribute, const SharedObjectPointer& object); void insert(const AttributePointer& attribute, const Box& bounds, float granularity, const SharedObjectPointer& object); + /// Removes a spanner from the specified attribute layer. void remove(const AttributePointer& attribute, const SharedObjectPointer& object); void remove(const AttributePointer& attribute, const Box& bounds, float granularity, const SharedObjectPointer& object); + /// Toggles the existence of a spanner in the specified attribute layer (removes if present, adds if not). void toggle(const AttributePointer& attribute, const SharedObjectPointer& object); void toggle(const AttributePointer& attribute, const Box& bounds, float granularity, const SharedObjectPointer& object); + /// Replaces a spanner in the specified attribute layer. void replace(const AttributePointer& attribute, const SharedObjectPointer& oldObject, const SharedObjectPointer& newObject); void replace(const AttributePointer& attribute, const Box& bounds, float granularity, const SharedObjectPointer& oldObject, const SharedObjectPointer& newObject); - + + /// Clears all data in the specified attribute layer. void clear(const AttributePointer& attribute); /// Convenience function that finds the first spanner intersecting the provided ray. @@ -97,7 +108,7 @@ public: /// Sets part of the data. void set(const glm::vec3& minimum, const MetavoxelData& data, bool blend = false); - /// Expands the tree, increasing its capacity in all dimensions. + /// Expands the tree, doubling its size in all dimensions (that is, increasing its volume eightfold). void expand(); void read(Bitstream& in, const MetavoxelLOD& lod = MetavoxelLOD()); @@ -110,6 +121,10 @@ public: MetavoxelNode* getRoot(const AttributePointer& attribute) const { return _roots.value(attribute); } MetavoxelNode* createRoot(const AttributePointer& attribute); + /// Performs a deep comparison between this data and the specified other (as opposed to the == operator, which does a + /// shallow comparison). + bool deepEquals(const MetavoxelData& other, const MetavoxelLOD& lod = MetavoxelLOD()) const; + bool operator==(const MetavoxelData& other) const; bool operator!=(const MetavoxelData& other) const; @@ -198,6 +213,14 @@ public: void clearChildren(const AttributePointer& attribute); + /// Performs a deep comparison between this and the specified other node. + bool deepEquals(const AttributePointer& attribute, const MetavoxelNode& other, + const glm::vec3& minimum, float size, const MetavoxelLOD& lod) const; + + /// Retrieves all spanners satisfying the LOD constraint, placing them in the provided set. + void getSpanners(const AttributePointer& attribute, const glm::vec3& minimum, + float size, const MetavoxelLOD& lod, SharedObjectSet& results) const; + private: Q_DISABLE_COPY(MetavoxelNode) @@ -234,6 +257,9 @@ public: /// Encodes a visitation order sequence that visits each child as sorted along the specified direction. static int encodeOrder(const glm::vec3& direction); + /// Returns a random visitation order sequence. + static int encodeRandomOrder(); + /// The default visitation order. static const int DEFAULT_ORDER; diff --git a/libraries/metavoxels/src/SharedObject.cpp b/libraries/metavoxels/src/SharedObject.cpp index 05af5f1bf8..d0a1842d31 100644 --- a/libraries/metavoxels/src/SharedObject.cpp +++ b/libraries/metavoxels/src/SharedObject.cpp @@ -84,11 +84,19 @@ bool SharedObject::equals(const SharedObject* other, bool sharedAncestry) const if (metaObject != other->metaObject() && !sharedAncestry) { return false; } - for (int i = 0; i < metaObject->propertyCount(); i++) { - QMetaProperty property = metaObject->property(i); - if (property.isStored() && property.read(this) != property.read(other)) { + // use the streamer, if we have one + const ObjectStreamer* streamer = Bitstream::getObjectStreamer(metaObject); + if (streamer) { + if (!streamer->equal(this, other)) { return false; } + } else { + for (int i = 0; i < metaObject->propertyCount(); i++) { + QMetaProperty property = metaObject->property(i); + if (property.isStored() && property.read(this) != property.read(other)) { + return false; + } + } } QList dynamicPropertyNames = this->dynamicPropertyNames(); if (dynamicPropertyNames.size() != other->dynamicPropertyNames().size()) { diff --git a/libraries/octree/src/Octree.cpp b/libraries/octree/src/Octree.cpp index d5f2afaedb..fbed6210e2 100644 --- a/libraries/octree/src/Octree.cpp +++ b/libraries/octree/src/Octree.cpp @@ -766,7 +766,7 @@ bool findShapeCollisionsOp(OctreeElement* element, void* extraData) { // coarse check against bounds AACube cube = element->getAACube(); cube.scale(TREE_SCALE); - if (!cube.expandedContains(args->shape->getPosition(), args->shape->getBoundingRadius())) { + if (!cube.expandedContains(args->shape->getTranslation(), args->shape->getBoundingRadius())) { return false; } if (!element->isLeaf()) { diff --git a/libraries/octree/src/OctreeEditPacketSender.cpp b/libraries/octree/src/OctreeEditPacketSender.cpp index 43e253b2da..d67306e8c7 100644 --- a/libraries/octree/src/OctreeEditPacketSender.cpp +++ b/libraries/octree/src/OctreeEditPacketSender.cpp @@ -354,7 +354,7 @@ void OctreeEditPacketSender::processNackPacket(const QByteArray& packet) { // read number of sequence numbers uint16_t numSequenceNumbers = (*(uint16_t*)dataAt); dataAt += sizeof(uint16_t); - + // read sequence numbers and queue packets for resend for (int i = 0; i < numSequenceNumbers; i++) { unsigned short int sequenceNumber = (*(unsigned short int*)dataAt); diff --git a/libraries/particles/src/ParticleCollisionSystem.cpp b/libraries/particles/src/ParticleCollisionSystem.cpp index 358c5a1b84..d8d5887d97 100644 --- a/libraries/particles/src/ParticleCollisionSystem.cpp +++ b/libraries/particles/src/ParticleCollisionSystem.cpp @@ -202,14 +202,13 @@ void ParticleCollisionSystem::updateCollisionWithAvatars(Particle* particle) { foreach (const AvatarSharedPointer& avatarPointer, _avatars->getAvatarHash()) { AvatarData* avatar = avatarPointer.data(); - // use a very generous bounding radius since the arms can stretch - float totalRadius = 2.f * avatar->getBoundingRadius() + radius; + float totalRadius = avatar->getBoundingRadius() + radius; glm::vec3 relativePosition = center - avatar->getPosition(); if (glm::dot(relativePosition, relativePosition) > (totalRadius * totalRadius)) { continue; } - if (avatar->findParticleCollisions(center, radius, _collisions)) { + if (avatar->findSphereCollisions(center, radius, _collisions)) { int numCollisions = _collisions.size(); for (int i = 0; i < numCollisions; ++i) { CollisionInfo* collision = _collisions.getCollision(i); @@ -222,25 +221,6 @@ void ParticleCollisionSystem::updateCollisionWithAvatars(Particle* particle) { if (glm::dot(relativeVelocity, collision->_penetration) <= 0.f) { // only collide when particle and collision point are moving toward each other // (doing this prevents some "collision snagging" when particle penetrates the object) - - // HACK BEGIN: to allow paddle hands to "hold" particles we attenuate soft collisions against them. - if (collision->_type == COLLISION_TYPE_PADDLE_HAND) { - // NOTE: the physics are wrong (particles cannot roll) but it IS possible to catch a slow moving particle. - // TODO: make this less hacky when we have more per-collision details - float elasticity = ELASTICITY; - float attenuationFactor = glm::length(collision->_addedVelocity) / HALTING_SPEED; - float damping = DAMPING; - if (attenuationFactor < 1.f) { - collision->_addedVelocity *= attenuationFactor; - elasticity *= attenuationFactor; - // NOTE: the math below keeps the damping piecewise continuous, - // while ramping it up to 1 when attenuationFactor = 0 - damping = DAMPING + (1.f - attenuationFactor) * (1.f - DAMPING); - } - collision->_damping = damping; - } - // HACK END - updateCollisionSound(particle, collision->_penetration, COLLISION_FREQUENCY); collision->_penetration /= (float)(TREE_SCALE); particle->applyHardCollision(*collision); diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp index 5416ff92a6..12ab6ba479 100644 --- a/libraries/shared/src/CapsuleShape.cpp +++ b/libraries/shared/src/CapsuleShape.cpp @@ -18,9 +18,6 @@ #include "SharedUtil.h" -// default axis of CapsuleShape is Y-axis -const glm::vec3 localAxis(0.0f, 1.0f, 0.0f); - CapsuleShape::CapsuleShape() : Shape(Shape::CAPSULE_SHAPE), _radius(0.0f), _halfHeight(0.0f) {} CapsuleShape::CapsuleShape(float radius, float halfHeight) : Shape(Shape::CAPSULE_SHAPE), @@ -40,17 +37,17 @@ CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm: /// \param[out] startPoint is the center of start cap void CapsuleShape::getStartPoint(glm::vec3& startPoint) const { - startPoint = _position - _rotation * glm::vec3(0.0f, _halfHeight, 0.0f); + startPoint = _translation - _rotation * glm::vec3(0.0f, _halfHeight, 0.0f); } /// \param[out] endPoint is the center of the end cap void CapsuleShape::getEndPoint(glm::vec3& endPoint) const { - endPoint = _position + _rotation * glm::vec3(0.0f, _halfHeight, 0.0f); + endPoint = _translation + _rotation * glm::vec3(0.0f, _halfHeight, 0.0f); } void CapsuleShape::computeNormalizedAxis(glm::vec3& axis) const { // default axis of a capsule is along the yAxis - axis = _rotation * glm::vec3(0.0f, 1.0f, 0.0f); + axis = _rotation * DEFAULT_CAPSULE_AXIS; } void CapsuleShape::setRadius(float radius) { @@ -71,17 +68,12 @@ void CapsuleShape::setRadiusAndHalfHeight(float radius, float halfHeight) { void CapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint) { glm::vec3 axis = endPoint - startPoint; - _position = 0.5f * (endPoint + startPoint); + _translation = 0.5f * (endPoint + startPoint); float height = glm::length(axis); if (height > EPSILON) { _halfHeight = 0.5f * height; axis /= height; - glm::vec3 yAxis(0.0f, 1.0f, 0.0f); - float angle = glm::angle(axis, yAxis); - if (angle > EPSILON) { - axis = glm::normalize(glm::cross(yAxis, axis)); - _rotation = glm::angleAxis(angle, axis); - } + computeNewRotation(axis); } updateBoundingRadius(); } @@ -94,3 +86,13 @@ bool CapsuleShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec // TODO: implement the raycast to return inside surface intersection for the internal rayStart. return findRayCapsuleIntersection(rayStart, rayDirection, capsuleStart, capsuleEnd, _radius, distance); } + +// static +glm::quat CapsuleShape::computeNewRotation(const glm::vec3& newAxis) { + float angle = glm::angle(newAxis, DEFAULT_CAPSULE_AXIS); + if (angle > EPSILON) { + glm::vec3 rotationAxis = glm::normalize(glm::cross(DEFAULT_CAPSULE_AXIS, newAxis)); + return glm::angleAxis(angle, rotationAxis); + } + return glm::quat(); +} diff --git a/libraries/shared/src/CapsuleShape.h b/libraries/shared/src/CapsuleShape.h index fdd6c3eda6..8d84e32a97 100644 --- a/libraries/shared/src/CapsuleShape.h +++ b/libraries/shared/src/CapsuleShape.h @@ -14,7 +14,11 @@ #include "Shape.h" +#include "SharedUtil.h" + // default axis of CapsuleShape is Y-axis +const glm::vec3 DEFAULT_CAPSULE_AXIS(0.0f, 1.0f, 0.0f); + class CapsuleShape : public Shape { public: @@ -23,26 +27,33 @@ public: CapsuleShape(float radius, float halfHeight, const glm::vec3& position, const glm::quat& rotation); CapsuleShape(float radius, const glm::vec3& startPoint, const glm::vec3& endPoint); + virtual ~CapsuleShape() {} + float getRadius() const { return _radius; } - float getHalfHeight() const { return _halfHeight; } + virtual float getHalfHeight() const { return _halfHeight; } /// \param[out] startPoint is the center of start cap - void getStartPoint(glm::vec3& startPoint) const; + virtual void getStartPoint(glm::vec3& startPoint) const; /// \param[out] endPoint is the center of the end cap - void getEndPoint(glm::vec3& endPoint) const; + virtual void getEndPoint(glm::vec3& endPoint) const; - void computeNormalizedAxis(glm::vec3& axis) const; + virtual void computeNormalizedAxis(glm::vec3& axis) const; void setRadius(float radius); - void setHalfHeight(float height); - void setRadiusAndHalfHeight(float radius, float height); - void setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint); + virtual void setHalfHeight(float height); + virtual void setRadiusAndHalfHeight(float radius, float height); + + /// Sets the endpoints and updates center, rotation, and halfHeight to agree. + virtual void setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint); bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const; + virtual float getVolume() const { return (PI * _radius * _radius) * (1.3333333333f * _radius + getHalfHeight()); } + protected: - void updateBoundingRadius() { _boundingRadius = _radius + _halfHeight; } + virtual void updateBoundingRadius() { _boundingRadius = _radius + getHalfHeight(); } + static glm::quat computeNewRotation(const glm::vec3& newAxis); float _radius; float _halfHeight; diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp index 38e3a4b2db..e862a22f4a 100644 --- a/libraries/shared/src/CollisionInfo.cpp +++ b/libraries/shared/src/CollisionInfo.cpp @@ -11,6 +11,20 @@ #include "CollisionInfo.h" +#include "Shape.h" +#include "SharedUtil.h" + +CollisionInfo::CollisionInfo() : + _data(NULL), + _intData(0), + _shapeA(NULL), + _shapeB(NULL), + _damping(0.f), + _elasticity(1.f), + _contactPoint(0.f), + _penetration(0.f), + _addedVelocity(0.f) { +} CollisionList::CollisionList(int maxSize) : _maxSize(maxSize), @@ -18,6 +32,29 @@ CollisionList::CollisionList(int maxSize) : _collisions.resize(_maxSize); } +void CollisionInfo::apply() { + assert(_shapeA); + // NOTE: Shape::computeEffectiveMass() has side effects: computes and caches partial Lagrangian coefficients + Shape* shapeA = const_cast(_shapeA); + float massA = shapeA->computeEffectiveMass(_penetration, _contactPoint); + float massB = MAX_SHAPE_MASS; + float totalMass = massA + massB; + if (_shapeB) { + Shape* shapeB = const_cast(_shapeB); + massB = shapeB->computeEffectiveMass(-_penetration, _contactPoint - _penetration); + totalMass = massA + massB; + if (totalMass < EPSILON) { + massA = massB = 1.0f; + totalMass = 2.0f; + } + // remember that _penetration points from A into B + shapeB->accumulateDelta(massA / totalMass, _penetration); + } + // NOTE: Shape::accumulateDelta() uses the coefficients from previous call to Shape::computeEffectiveMass() + // remember that _penetration points from A into B + shapeA->accumulateDelta(massB / totalMass, -_penetration); +} + CollisionInfo* CollisionList::getNewCollision() { // return pointer to existing CollisionInfo, or NULL of list is full return (_size < _maxSize) ? &(_collisions[_size++]) : NULL; @@ -38,17 +75,17 @@ CollisionInfo* CollisionList::getLastCollision() { } void CollisionList::clear() { - // we rely on the external context to properly set or clear the data members of a collision - // whenever it is used. + // we rely on the external context to properly set or clear the data members of CollisionInfos /* for (int i = 0; i < _size; ++i) { // we only clear the important stuff CollisionInfo& collision = _collisions[i]; - collision._type = COLLISION_TYPE_UNKNOWN; //collision._data = NULL; //collision._intData = 0; //collision._floatDAta = 0.0f; //collision._vecData = glm::vec3(0.0f); + //collision._shapeA = NULL; + //collision._shapeB = NULL; //collision._damping; //collision._elasticity; //collision._contactPoint; diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h index 52d5298fde..1ab06e2ef5 100644 --- a/libraries/shared/src/CollisionInfo.h +++ b/libraries/shared/src/CollisionInfo.h @@ -17,16 +17,7 @@ #include -enum CollisionType { - COLLISION_TYPE_UNKNOWN = 0, - COLLISION_TYPE_PADDLE_HAND, - COLLISION_TYPE_MODEL, - // _data = pointer to Model that owns joint - // _intData = joint index - COLLISION_TYPE_AACUBE, - // _floatData = cube side - // _vecData = cube center -}; +class Shape; const quint32 COLLISION_GROUP_ENVIRONMENT = 1U << 0; const quint32 COLLISION_GROUP_AVATARS = 1U << 1; @@ -41,38 +32,24 @@ const quint32 VALID_COLLISION_GROUPS = 0x0f; class CollisionInfo { public: - CollisionInfo() - : _type(0), - _data(NULL), - _intData(0), - _damping(0.f), - _elasticity(1.f), - _contactPoint(0.f), - _penetration(0.f), - _addedVelocity(0.f) { - } - - CollisionInfo(qint32 type) - : _type(type), - _data(NULL), - _intData(0), - _damping(0.f), - _elasticity(1.f), - _contactPoint(0.f), - _penetration(0.f), - _addedVelocity(0.f) { - } - + CollisionInfo(); ~CollisionInfo() {} - int _type; // type of Collision - - // the value of the *Data fields depend on the type + // TODO: Andrew to get rid of these data members void* _data; int _intData; float _floatData; glm::vec3 _vecData; + /// accumulates position changes for the shapes in this collision to resolve penetration + void apply(); + + Shape* getShapeA() const { return const_cast(_shapeA); } + Shape* getShapeB() const { return const_cast(_shapeB); } + + const Shape* _shapeA; // pointer to shapeA in this collision + const Shape* _shapeB; // pointer to shapeB in this collision + float _damping; // range [0,1] of friction coeficient float _elasticity; // range [0,1] of energy conservation glm::vec3 _contactPoint; // world-frame point on BodyA that is deepest into BodyB diff --git a/libraries/shared/src/ListShape.cpp b/libraries/shared/src/ListShape.cpp index dcea97826e..67ec32d4b1 100644 --- a/libraries/shared/src/ListShape.cpp +++ b/libraries/shared/src/ListShape.cpp @@ -14,7 +14,7 @@ // ListShapeEntry void ListShapeEntry::updateTransform(const glm::vec3& rootPosition, const glm::quat& rootRotation) { - _shape->setPosition(rootPosition + rootRotation * _localPosition); + _shape->setTranslation(rootPosition + rootRotation * _localPosition); _shape->setRotation(_localRotation * rootRotation); } @@ -24,9 +24,9 @@ ListShape::~ListShape() { clear(); } -void ListShape::setPosition(const glm::vec3& position) { +void ListShape::setTranslation(const glm::vec3& position) { _subShapeTransformsAreDirty = true; - Shape::setPosition(position); + Shape::setTranslation(position); } void ListShape::setRotation(const glm::quat& rotation) { @@ -44,7 +44,7 @@ const Shape* ListShape::getSubShape(int index) const { void ListShape::updateSubTransforms() { if (_subShapeTransformsAreDirty) { for (int i = 0; i < _subShapeEntries.size(); ++i) { - _subShapeEntries[i].updateTransform(_position, _rotation); + _subShapeEntries[i].updateTransform(_translation, _rotation); } _subShapeTransformsAreDirty = false; } diff --git a/libraries/shared/src/ListShape.h b/libraries/shared/src/ListShape.h index 17e7d7b2b6..bd150c8246 100644 --- a/libraries/shared/src/ListShape.h +++ b/libraries/shared/src/ListShape.h @@ -42,7 +42,7 @@ public: ~ListShape(); - void setPosition(const glm::vec3& position); + void setTranslation(const glm::vec3& position); void setRotation(const glm::quat& rotation); const Shape* getSubShape(int index) const; diff --git a/libraries/shared/src/PhysicsEntity.cpp b/libraries/shared/src/PhysicsEntity.cpp new file mode 100644 index 0000000000..37d1a88d67 --- /dev/null +++ b/libraries/shared/src/PhysicsEntity.cpp @@ -0,0 +1,211 @@ +// +// PhysicsEntity.cpp +// libraries/shared/src +// +// Created by Andrew Meadows 2014.06.11 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "PhysicsEntity.h" + +#include "PhysicsSimulation.h" +#include "Shape.h" +#include "ShapeCollider.h" + +PhysicsEntity::PhysicsEntity() : + _translation(0.0f), + _rotation(), + _boundingRadius(0.0f), + _shapesAreDirty(true), + _enableShapes(false), + _simulation(NULL) { +} + +PhysicsEntity::~PhysicsEntity() { + if (_simulation) { + _simulation->removeEntity(this); + _simulation = NULL; + } +} + +void PhysicsEntity::setTranslation(const glm::vec3& translation) { + if (_translation != translation) { + _shapesAreDirty = !_shapes.isEmpty(); + _translation = translation; + } +} + +void PhysicsEntity::setRotation(const glm::quat& rotation) { + if (_rotation != rotation) { + _shapesAreDirty = !_shapes.isEmpty(); + _rotation = rotation; + } +} + +void PhysicsEntity::setShapeBackPointers() { + for (int i = 0; i < _shapes.size(); i++) { + Shape* shape = _shapes[i]; + if (shape) { + shape->setEntity(this); + } + } +} + +void PhysicsEntity::setEnableShapes(bool enable) { + if (enable != _enableShapes) { + clearShapes(); + _enableShapes = enable; + if (_enableShapes) { + buildShapes(); + } + } +} + +void PhysicsEntity::clearShapes() { + for (int i = 0; i < _shapes.size(); ++i) { + delete _shapes[i]; + } + _shapes.clear(); +} + +bool PhysicsEntity::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const { + int numShapes = _shapes.size(); + float minDistance = FLT_MAX; + for (int j = 0; j < numShapes; ++j) { + const Shape* shape = _shapes[j]; + float thisDistance = FLT_MAX; + if (shape && shape->findRayIntersection(origin, direction, thisDistance)) { + if (thisDistance < minDistance) { + minDistance = thisDistance; + } + } + } + if (minDistance < FLT_MAX) { + distance = minDistance; + return true; + } + return false; +} + +bool PhysicsEntity::findCollisions(const QVector shapes, CollisionList& collisions) { + bool collided = false; + int numTheirShapes = shapes.size(); + for (int i = 0; i < numTheirShapes; ++i) { + const Shape* theirShape = shapes[i]; + if (!theirShape) { + continue; + } + int numOurShapes = _shapes.size(); + for (int j = 0; j < numOurShapes; ++j) { + const Shape* ourShape = _shapes.at(j); + if (ourShape && ShapeCollider::collideShapes(theirShape, ourShape, collisions)) { + collided = true; + } + } + } + return collided; +} + +bool PhysicsEntity::findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadius, CollisionList& collisions) { + bool collided = false; + SphereShape sphere(sphereRadius, sphereCenter); + for (int i = 0; i < _shapes.size(); i++) { + Shape* shape = _shapes[i]; + if (!shape) { + continue; + } + if (ShapeCollider::collideShapes(&sphere, shape, collisions)) { + CollisionInfo* collision = collisions.getLastCollision(); + collision->_data = (void*)(this); + collision->_intData = i; + collided = true; + } + } + return collided; +} + +bool PhysicsEntity::findPlaneCollisions(const glm::vec4& plane, CollisionList& collisions) { + bool collided = false; + PlaneShape planeShape(plane); + for (int i = 0; i < _shapes.size(); i++) { + if (_shapes.at(i) && ShapeCollider::collideShapes(&planeShape, _shapes.at(i), collisions)) { + CollisionInfo* collision = collisions.getLastCollision(); + collision->_data = (void*)(this); + collision->_intData = i; + collided = true; + } + } + return collided; +} + +// ----------------------------------------------------------- +// TODO: enforce this maximum when shapes are actually built. The gotcha here is +// that the Model class (derived from PhysicsEntity) expects numShapes == numJoints, +// so we have to modify that code to be safe. +const int MAX_SHAPES_PER_ENTITY = 256; + +// the first 256 prime numbers +const int primes[256] = { + 2, 3, 5, 7, 11, 13, 17, 19, 23, 29, + 31, 37, 41, 43, 47, 53, 59, 61, 67, 71, + 73, 79, 83, 89, 97, 101, 103, 107, 109, 113, + 127, 131, 137, 139, 149, 151, 157, 163, 167, 173, + 179, 181, 191, 193, 197, 199, 211, 223, 227, 229, + 233, 239, 241, 251, 257, 263, 269, 271, 277, 281, + 283, 293, 307, 311, 313, 317, 331, 337, 347, 349, + 353, 359, 367, 373, 379, 383, 389, 397, 401, 409, + 419, 421, 431, 433, 439, 443, 449, 457, 461, 463, + 467, 479, 487, 491, 499, 503, 509, 521, 523, 541, + 547, 557, 563, 569, 571, 577, 587, 593, 599, 601, + 607, 613, 617, 619, 631, 641, 643, 647, 653, 659, + 661, 673, 677, 683, 691, 701, 709, 719, 727, 733, + 739, 743, 751, 757, 761, 769, 773, 787, 797, 809, + 811, 821, 823, 827, 829, 839, 853, 857, 859, 863, + 877, 881, 883, 887, 907, 911, 919, 929, 937, 941, + 947, 953, 967, 971, 977, 983, 991, 997, 1009, 1013, + 1019, 1021, 1031, 1033, 1039, 1049, 1051, 1061, 1063, 1069, + 1087, 1091, 1093, 1097, 1103, 1109, 1117, 1123, 1129, 1151, + 1153, 1163, 1171, 1181, 1187, 1193, 1201, 1213, 1217, 1223, + 1229, 1231, 1237, 1249, 1259, 1277, 1279, 1283, 1289, 1291, + 1297, 1301, 1303, 1307, 1319, 1321, 1327, 1361, 1367, 1373, + 1381, 1399, 1409, 1423, 1427, 1429, 1433, 1439, 1447, 1451, + 1453, 1459, 1471, 1481, 1483, 1487, 1489, 1493, 1499, 1511, + 1523, 1531, 1543, 1549, 1553, 1559, 1567, 1571, 1579, 1583, + 1597, 1601, 1607, 1609, 1613, 1619 }; + +void PhysicsEntity::disableCollisions(int shapeIndexA, int shapeIndexB) { + if (shapeIndexA < MAX_SHAPES_PER_ENTITY && shapeIndexB < MAX_SHAPES_PER_ENTITY) { + _disabledCollisions.insert(primes[shapeIndexA] * primes[shapeIndexB]); + } +} + +bool PhysicsEntity::collisionsAreEnabled(int shapeIndexA, int shapeIndexB) const { + if (shapeIndexA < MAX_SHAPES_PER_ENTITY && shapeIndexB < MAX_SHAPES_PER_ENTITY) { + return !_disabledCollisions.contains(primes[shapeIndexA] * primes[shapeIndexB]); + } + return false; +} + +void PhysicsEntity::disableCurrentSelfCollisions() { + CollisionList collisions(10); + int numShapes = _shapes.size(); + for (int i = 0; i < numShapes; ++i) { + const Shape* shape = _shapes.at(i); + if (!shape) { + continue; + } + for (int j = i+1; j < numShapes; ++j) { + if (!collisionsAreEnabled(i, j)) { + continue; + } + const Shape* otherShape = _shapes.at(j); + if (otherShape && ShapeCollider::collideShapes(shape, otherShape, collisions)) { + disableCollisions(i, j); + collisions.clear(); + } + } + } +} diff --git a/libraries/shared/src/PhysicsEntity.h b/libraries/shared/src/PhysicsEntity.h new file mode 100644 index 0000000000..3407ac8421 --- /dev/null +++ b/libraries/shared/src/PhysicsEntity.h @@ -0,0 +1,78 @@ +// +// PhysicsEntity.h +// libraries/shared/src +// +// Created by Andrew Meadows 2014.05.30 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_PhysicsEntity_h +#define hifi_PhysicsEntity_h + +#include +#include + +#include +#include + +#include "CollisionInfo.h" + +class Shape; +class PhysicsSimulation; + +// PhysicsEntity is the base class for anything that owns one or more Shapes that collide in a +// PhysicsSimulation. Each CollisionInfo generated by a PhysicsSimulation has back pointers to the +// two Shapes involved, and those Shapes may (optionally) have valid back pointers to their PhysicsEntity. + +class PhysicsEntity { + +public: + PhysicsEntity(); + virtual ~PhysicsEntity(); + + void setTranslation(const glm::vec3& translation); + void setRotation(const glm::quat& rotation); + + const glm::vec3& getTranslation() const { return _translation; } + const glm::quat& getRotation() const { return _rotation; } + float getBoundingRadius() const { return _boundingRadius; } + + void setShapeBackPointers(); + + void setEnableShapes(bool enable); + + virtual void buildShapes() = 0; + virtual void clearShapes(); + const QVector getShapes() const { return _shapes; } + + PhysicsSimulation* getSimulation() const { return _simulation; } + + bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const; + bool findCollisions(const QVector shapes, CollisionList& collisions); + bool findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadius, CollisionList& collisions); + bool findPlaneCollisions(const glm::vec4& plane, CollisionList& collisions); + + void disableCollisions(int shapeIndexA, int shapeIndexB); + bool collisionsAreEnabled(int shapeIndexA, int shapeIndexB) const; + + void disableCurrentSelfCollisions(); + +protected: + glm::vec3 _translation; + glm::quat _rotation; + float _boundingRadius; + bool _shapesAreDirty; + bool _enableShapes; + QVector _shapes; + QSet _disabledCollisions; + +private: + // PhysicsSimulation is a friend so that it can set the protected _simulation backpointer + friend class PhysicsSimulation; + PhysicsSimulation* _simulation; +}; + +#endif // hifi_PhysicsEntity_h diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp new file mode 100644 index 0000000000..637a5e955c --- /dev/null +++ b/libraries/shared/src/PhysicsSimulation.cpp @@ -0,0 +1,244 @@ +// +// PhysicsSimulation.cpp +// interface/src/avatar +// +// Created by Andrew Meadows 2014.06.06 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include +#include + +#include "PhysicsSimulation.h" + +#include "PhysicsEntity.h" +#include "Ragdoll.h" +#include "SharedUtil.h" +#include "ShapeCollider.h" + +int MAX_DOLLS_PER_SIMULATION = 16; +int MAX_ENTITIES_PER_SIMULATION = 64; +int MAX_COLLISIONS_PER_SIMULATION = 256; + + +const int NUM_SHAPE_BITS = 6; +const int SHAPE_INDEX_MASK = (1 << (NUM_SHAPE_BITS + 1)) - 1; + +PhysicsSimulation::PhysicsSimulation() : _collisionList(MAX_COLLISIONS_PER_SIMULATION), + _numIterations(0), _numCollisions(0), _constraintError(0.0f), _stepTime(0) { +} + +PhysicsSimulation::~PhysicsSimulation() { + // entities have a backpointer to this simulator that must be cleaned up + int numEntities = _entities.size(); + for (int i = 0; i < numEntities; ++i) { + _entities[i]->_simulation = NULL; + } + _entities.clear(); + + // but Ragdolls do not + _dolls.clear(); +} + +bool PhysicsSimulation::addEntity(PhysicsEntity* entity) { + if (!entity) { + return false; + } + if (entity->_simulation == this) { + int numEntities = _entities.size(); + for (int i = 0; i < numEntities; ++i) { + if (entity == _entities.at(i)) { + // already in list + assert(entity->_simulation == this); + return true; + } + } + // belongs to some other simulation + return false; + } + int numEntities = _entities.size(); + if (numEntities > MAX_ENTITIES_PER_SIMULATION) { + // list is full + return false; + } + // add to list + entity->_simulation = this; + _entities.push_back(entity); + return true; +} + +void PhysicsSimulation::removeEntity(PhysicsEntity* entity) { + if (!entity || !entity->_simulation || !(entity->_simulation == this)) { + return; + } + int numEntities = _entities.size(); + for (int i = 0; i < numEntities; ++i) { + if (entity == _entities.at(i)) { + if (i == numEntities - 1) { + // remove it + _entities.pop_back(); + } else { + // swap the last for this one + PhysicsEntity* lastEntity = _entities[numEntities - 1]; + _entities.pop_back(); + _entities[i] = lastEntity; + } + entity->_simulation = NULL; + break; + } + } +} + +bool PhysicsSimulation::addRagdoll(Ragdoll* doll) { + if (!doll) { + return false; + } + int numDolls = _dolls.size(); + if (numDolls > MAX_DOLLS_PER_SIMULATION) { + // list is full + return false; + } + for (int i = 0; i < numDolls; ++i) { + if (doll == _dolls[i]) { + // already in list + return true; + } + } + // add to list + _dolls.push_back(doll); + return true; +} + +void PhysicsSimulation::removeRagdoll(Ragdoll* doll) { + int numDolls = _dolls.size(); + for (int i = 0; i < numDolls; ++i) { + if (doll == _dolls[i]) { + if (i == numDolls - 1) { + // remove it + _dolls.pop_back(); + } else { + // swap the last for this one + Ragdoll* lastDoll = _dolls[numDolls - 1]; + _dolls.pop_back(); + _dolls[i] = lastDoll; + } + break; + } + } +} +// TODO: Andrew to implement: +// DONE (1) joints pull points (SpecialCapsuleShape would help solve this) +// DONE (2) points slam shapes (SpecialCapsuleShape would help solve this) +// DONE (3) detect collisions +// DONE (4) collisions move points (SpecialCapsuleShape would help solve this) +// DONE (5) enforce constraints +// DONE (6) make sure MyAvatar creates shapes, adds to simulation with ragdoll support +// DONE (7) support for pairwise collision bypass +// DONE (8) process collisions +// DONE (8a) stubbery +// DONE (8b) shapes actually accumulate movement +// DONE (9) verify that avatar shapes self collide +// (10) slave rendered SkeletonModel to physical shapes +// (10a) give SkeletonModel duplicate JointState data +// (10b) figure out how to slave dupe JointStates to physical shapes +// (11) add and enforce angular contraints for joints +void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) { + quint64 now = usecTimestampNow(); + quint64 startTime = now; + quint64 expiry = startTime + maxUsec; + + moveRagdolls(deltaTime); + + int numDolls = _dolls.size(); + _numCollisions = 0; + int iterations = 0; + float error = 0.0f; + do { + computeCollisions(); + processCollisions(); + + // enforce constraints + error = 0.0f; + for (int i = 0; i < numDolls; ++i) { + error = glm::max(error, _dolls[i]->enforceRagdollConstraints()); + } + ++iterations; + + now = usecTimestampNow(); + } while (_numCollisions != 0 && (iterations < maxIterations) && (error > minError) && (now < expiry)); + + _numIterations = iterations; + _constraintError = error; + _stepTime = usecTimestampNow()- startTime; + +#ifdef ANDREW_DEBUG + // temporary debug info for watching simulation performance + static int adebug = 0; ++adebug; + if (0 == (adebug % 100)) { + std::cout << "adebug Ni = " << _numIterations << " E = " << error << " t = " << _stepTime << std::endl; // adebug + } +#endif // ANDREW_DEBUG +} + +void PhysicsSimulation::moveRagdolls(float deltaTime) { + int numDolls = _dolls.size(); + for (int i = 0; i < numDolls; ++i) { + _dolls.at(i)->stepRagdollForward(deltaTime); + } +} + +void PhysicsSimulation::computeCollisions() { + _collisionList.clear(); + // TODO: keep track of QSet collidedEntities; + int numEntities = _entities.size(); + for (int i = 0; i < numEntities; ++i) { + PhysicsEntity* entity = _entities.at(i); + const QVector shapes = entity->getShapes(); + int numShapes = shapes.size(); + // collide with self + for (int j = 0; j < numShapes; ++j) { + const Shape* shape = shapes.at(j); + if (!shape) { + continue; + } + for (int k = j+1; k < numShapes; ++k) { + const Shape* otherShape = shapes.at(k); + if (otherShape && entity->collisionsAreEnabled(j, k)) { + ShapeCollider::collideShapes(shape, otherShape, _collisionList); + } + } + } + + // collide with others + for (int j = i+1; j < numEntities; ++j) { + const QVector otherShapes = _entities.at(j)->getShapes(); + ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisionList); + } + } + _numCollisions = _collisionList.size(); +} + +void PhysicsSimulation::processCollisions() { + // walk all collisions, accumulate movement on shapes, and build a list of affected shapes + QSet shapes; + int numCollisions = _collisionList.size(); + for (int i = 0; i < numCollisions; ++i) { + CollisionInfo* collision = _collisionList.getCollision(i); + collision->apply(); + // there is always a shapeA + shapes.insert(collision->getShapeA()); + // but need to check for valid shapeB + if (collision->_shapeB) { + shapes.insert(collision->getShapeB()); + } + } + // walk all affected shapes and apply accumulated movement + QSet::const_iterator shapeItr = shapes.constBegin(); + while (shapeItr != shapes.constEnd()) { + (*shapeItr)->applyAccumulatedDelta(); + ++shapeItr; + } +} diff --git a/libraries/shared/src/PhysicsSimulation.h b/libraries/shared/src/PhysicsSimulation.h new file mode 100644 index 0000000000..c611e06870 --- /dev/null +++ b/libraries/shared/src/PhysicsSimulation.h @@ -0,0 +1,60 @@ +// +// PhysicsSimulation.h +// interface/src/avatar +// +// Created by Andrew Meadows 2014.06.06 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_PhysicsSimulation +#define hifi_PhysicsSimulation + +#include + +#include "CollisionInfo.h" + +class PhysicsEntity; +class Ragdoll; + +class PhysicsSimulation { +public: + + PhysicsSimulation(); + ~PhysicsSimulation(); + + /// \return true if entity was added to or is already in the list + bool addEntity(PhysicsEntity* entity); + + void removeEntity(PhysicsEntity* entity); + + /// \return true if doll was added to or is already in the list + bool addRagdoll(Ragdoll* doll); + + void removeRagdoll(Ragdoll* doll); + + /// \param minError constraint motion below this value is considered "close enough" + /// \param maxIterations max number of iterations before giving up + /// \param maxUsec max number of usec to spend enforcing constraints + /// \return distance of largest movement + void stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec); + + void moveRagdolls(float deltaTime); + void computeCollisions(); + void processCollisions(); + +private: + CollisionList _collisionList; + QVector _entities; + QVector _dolls; + + // some stats + int _numIterations; + int _numCollisions; + float _constraintError; + quint64 _stepTime; +}; + +#endif // hifi_PhysicsSimulation diff --git a/libraries/shared/src/PlaneShape.cpp b/libraries/shared/src/PlaneShape.cpp index e9563c6d8b..15ea281510 100644 --- a/libraries/shared/src/PlaneShape.cpp +++ b/libraries/shared/src/PlaneShape.cpp @@ -18,7 +18,7 @@ PlaneShape::PlaneShape(const glm::vec4& coefficients) : Shape(Shape::PLANE_SHAPE) { glm::vec3 normal = glm::vec3(coefficients); - _position = -normal * coefficients.w; + _translation = -normal * coefficients.w; float angle = acosf(glm::dot(normal, UNROTATED_NORMAL)); if (angle > EPSILON) { @@ -36,7 +36,7 @@ glm::vec3 PlaneShape::getNormal() const { glm::vec4 PlaneShape::getCoefficients() const { glm::vec3 normal = _rotation * UNROTATED_NORMAL; - return glm::vec4(normal.x, normal.y, normal.z, -glm::dot(normal, _position)); + return glm::vec4(normal.x, normal.y, normal.z, -glm::dot(normal, _translation)); } bool PlaneShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const { @@ -44,9 +44,9 @@ bool PlaneShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& float denominator = glm::dot(n, rayDirection); if (fabsf(denominator) < EPSILON) { // line is parallel to plane - return glm::dot(_position - rayStart, n) < EPSILON; + return glm::dot(_translation - rayStart, n) < EPSILON; } else { - float d = glm::dot(_position - rayStart, n) / denominator; + float d = glm::dot(_translation - rayStart, n) / denominator; if (d > 0.0f) { // ray points toward plane distance = d; diff --git a/libraries/shared/src/Ragdoll.cpp b/libraries/shared/src/Ragdoll.cpp new file mode 100644 index 0000000000..1d24e74864 --- /dev/null +++ b/libraries/shared/src/Ragdoll.cpp @@ -0,0 +1,121 @@ +// +// Ragdoll.cpp +// libraries/shared/src +// +// Created by Andrew Meadows 2014.05.30 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "Ragdoll.h" + +#include "CapsuleShape.h" +#include "CollisionInfo.h" +#include "SharedUtil.h" +#include "SphereShape.h" + +// ---------------------------------------------------------------------------- +// VerletPoint +// ---------------------------------------------------------------------------- +void VerletPoint::accumulateDelta(const glm::vec3& delta) { + _accumulatedDelta += delta; + ++_numDeltas; +} + +void VerletPoint::applyAccumulatedDelta() { + if (_numDeltas > 0) { + _position += _accumulatedDelta / (float)_numDeltas; + _accumulatedDelta = glm::vec3(0.0f); + _numDeltas = 0; + } +} + +// ---------------------------------------------------------------------------- +// FixedConstraint +// ---------------------------------------------------------------------------- +FixedConstraint::FixedConstraint(VerletPoint* point, const glm::vec3& anchor) : _point(point), _anchor(anchor) { +} + +float FixedConstraint::enforce() { + assert(_point != NULL); + // TODO: use fast approximate sqrt here + float distance = glm::distance(_anchor, _point->_position); + _point->_position = _anchor; + return distance; +} + +void FixedConstraint::setPoint(VerletPoint* point) { + assert(point); + _point = point; + _point->_mass = MAX_SHAPE_MASS; +} + +void FixedConstraint::setAnchor(const glm::vec3& anchor) { + _anchor = anchor; +} + +// ---------------------------------------------------------------------------- +// DistanceConstraint +// ---------------------------------------------------------------------------- +DistanceConstraint::DistanceConstraint(VerletPoint* startPoint, VerletPoint* endPoint) : _distance(-1.0f) { + _points[0] = startPoint; + _points[1] = endPoint; + _distance = glm::distance(_points[0]->_position, _points[1]->_position); +} + +DistanceConstraint::DistanceConstraint(const DistanceConstraint& other) { + _distance = other._distance; + _points[0] = other._points[0]; + _points[1] = other._points[1]; +} + +void DistanceConstraint::setDistance(float distance) { + _distance = fabsf(distance); +} + +float DistanceConstraint::enforce() { + // TODO: use a fast distance approximation + float newDistance = glm::distance(_points[0]->_position, _points[1]->_position); + glm::vec3 direction(0.0f, 1.0f, 0.0f); + if (newDistance > EPSILON) { + direction = (_points[0]->_position - _points[1]->_position) / newDistance; + } + glm::vec3 center = 0.5f * (_points[0]->_position + _points[1]->_position); + _points[0]->_position = center + (0.5f * _distance) * direction; + _points[1]->_position = center - (0.5f * _distance) * direction; + return glm::abs(newDistance - _distance); +} + +// ---------------------------------------------------------------------------- +// Ragdoll +// ---------------------------------------------------------------------------- + +Ragdoll::Ragdoll() { +} + +Ragdoll::~Ragdoll() { + clearRagdollConstraintsAndPoints(); +} + +void Ragdoll::clearRagdollConstraintsAndPoints() { + int numConstraints = _ragdollConstraints.size(); + for (int i = 0; i < numConstraints; ++i) { + delete _ragdollConstraints[i]; + } + _ragdollConstraints.clear(); + _ragdollPoints.clear(); +} + +float Ragdoll::enforceRagdollConstraints() { + float maxDistance = 0.0f; + const int numConstraints = _ragdollConstraints.size(); + for (int i = 0; i < numConstraints; ++i) { + DistanceConstraint* c = static_cast(_ragdollConstraints[i]); + //maxDistance = glm::max(maxDistance, _ragdollConstraints[i]->enforce()); + maxDistance = glm::max(maxDistance, c->enforce()); + } + return maxDistance; +} + diff --git a/libraries/shared/src/Ragdoll.h b/libraries/shared/src/Ragdoll.h new file mode 100644 index 0000000000..59c1291725 --- /dev/null +++ b/libraries/shared/src/Ragdoll.h @@ -0,0 +1,107 @@ +// +// Ragdoll.h +// libraries/shared/src +// +// Created by Andrew Meadows 2014.05.30 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_Ragdoll_h +#define hifi_Ragdoll_h + +#include +#include + +#include + +class Shape; + +// TODO: Andrew to move VerletPoint class to its own file +class VerletPoint { +public: + VerletPoint() : _position(0.0f), _lastPosition(0.0f), _mass(1.0f), _accumulatedDelta(0.0f), _numDeltas(0) {} + + void accumulateDelta(const glm::vec3& delta); + void applyAccumulatedDelta(); + + glm::vec3 getAccumulatedDelta() const { + glm::vec3 foo(0.0f); + if (_numDeltas > 0) { + foo = _accumulatedDelta / (float)_numDeltas; + } + return foo; + } + + glm::vec3 _position; + glm::vec3 _lastPosition; + float _mass; + +private: + glm::vec3 _accumulatedDelta; + int _numDeltas; +}; + +class Constraint { +public: + Constraint() {} + virtual ~Constraint() {} + + /// Enforce contraint by moving relevant points. + /// \return max distance of point movement + virtual float enforce() = 0; + +protected: + int _type; +}; + +class FixedConstraint : public Constraint { +public: + FixedConstraint(VerletPoint* point, const glm::vec3& anchor); + float enforce(); + void setPoint(VerletPoint* point); + void setAnchor(const glm::vec3& anchor); +private: + VerletPoint* _point; + glm::vec3 _anchor; +}; + +class DistanceConstraint : public Constraint { +public: + DistanceConstraint(VerletPoint* startPoint, VerletPoint* endPoint); + DistanceConstraint(const DistanceConstraint& other); + float enforce(); + void setDistance(float distance); + float getDistance() const { return _distance; } +private: + float _distance; + VerletPoint* _points[2]; +}; + +class Ragdoll { +public: + + Ragdoll(); + virtual ~Ragdoll(); + + virtual void stepRagdollForward(float deltaTime) = 0; + + /// \return max distance of point movement + float enforceRagdollConstraints(); + + // both const and non-const getPoints() + const QVector& getRagdollPoints() const { return _ragdollPoints; } + QVector& getRagdollPoints() { return _ragdollPoints; } + +protected: + void clearRagdollConstraintsAndPoints(); + virtual void initRagdollPoints() = 0; + virtual void buildRagdollConstraints() = 0; + + QVector _ragdollPoints; + QVector _ragdollConstraints; +}; + +#endif // hifi_Ragdoll_h diff --git a/libraries/shared/src/Shape.h b/libraries/shared/src/Shape.h index 3926f6cd07..09ed30a116 100644 --- a/libraries/shared/src/Shape.h +++ b/libraries/shared/src/Shape.h @@ -15,47 +15,76 @@ #include #include +class PhysicsEntity; + +const float MAX_SHAPE_MASS = 1.0e18f; // something less than sqrt(FLT_MAX) class Shape { public: + enum Type{ UNKNOWN_SHAPE = 0, SPHERE_SHAPE, CAPSULE_SHAPE, PLANE_SHAPE, - BOX_SHAPE, LIST_SHAPE }; - Shape() : _type(UNKNOWN_SHAPE), _boundingRadius(0.f), _position(0.f), _rotation() { } + Shape() : _type(UNKNOWN_SHAPE), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation(), _mass(MAX_SHAPE_MASS) { } virtual ~Shape() {} int getType() const { return _type; } - float getBoundingRadius() const { return _boundingRadius; } - const glm::vec3& getPosition() const { return _position; } - const glm::quat& getRotation() const { return _rotation; } - virtual void setPosition(const glm::vec3& position) { _position = position; } + void setEntity(PhysicsEntity* entity) { _owningEntity = entity; } + PhysicsEntity* getEntity() const { return _owningEntity; } + + float getBoundingRadius() const { return _boundingRadius; } + + virtual const glm::quat& getRotation() const { return _rotation; } virtual void setRotation(const glm::quat& rotation) { _rotation = rotation; } + virtual void setTranslation(const glm::vec3& translation) { _translation = translation; } + virtual const glm::vec3& getTranslation() const { return _translation; } + + virtual void setMass(float mass) { _mass = mass; } + virtual float getMass() const { return _mass; } + virtual bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const = 0; + /// \param penetration of collision + /// \param contactPoint of collision + /// \return the effective mass for the collision + /// For most shapes has side effects: computes and caches the partial Lagrangian coefficients which will be + /// used in the next accumulateDelta() call. + virtual float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) { return _mass; } + + /// \param relativeMassFactor the final ingredient for partial Lagrangian coefficients from computeEffectiveMass() + /// \param penetration the delta movement + virtual void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) {} + + virtual void applyAccumulatedDelta() {} + + /// \return volume of shape in cubic meters + virtual float getVolume() const { return 1.0; } + protected: // these ctors are protected (used by derived classes only) - Shape(Type type) : _type(type), _boundingRadius(0.f), _position(0.f), _rotation() {} + Shape(Type type) : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(0.f), _rotation() {} Shape(Type type, const glm::vec3& position) - : _type(type), _boundingRadius(0.f), _position(position), _rotation() {} + : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(position), _rotation() {} Shape(Type type, const glm::vec3& position, const glm::quat& rotation) - : _type(type), _boundingRadius(0.f), _position(position), _rotation(rotation) {} + : _type(type), _owningEntity(NULL), _boundingRadius(0.f), _translation(position), _rotation(rotation) {} void setBoundingRadius(float radius) { _boundingRadius = radius; } int _type; + PhysicsEntity* _owningEntity; float _boundingRadius; - glm::vec3 _position; + glm::vec3 _translation; glm::quat _rotation; + float _mass; }; #endif // hifi_Shape_h diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index bbedeb401d..ffb51660e2 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -24,7 +24,6 @@ namespace ShapeCollider { bool collideShapes(const Shape* shapeA, const Shape* shapeB, CollisionList& collisions) { - // ATM we only have two shape types so we just check every case. // TODO: make a fast lookup for correct method int typeA = shapeA->getType(); int typeB = shapeB->getType(); @@ -74,7 +73,7 @@ bool collideShapesCoarse(const QVector& shapesA, const QVector 0) { @@ -87,11 +86,52 @@ bool collideShapesCoarse(const QVector& shapesA, const QVector& shapes, int startIndex, CollisionList& collisions) { + bool collided = false; + if (shapeA) { + int numShapes = shapes.size(); + for (int i = startIndex; i < numShapes; ++i) { + const Shape* shapeB = shapes.at(i); + if (!shapeB) { + continue; + } + if (collideShapes(shapeA, shapeB, collisions)) { + collided = true; + if (collisions.isFull()) { + break; + } + } + } + } + return collided; +} + +bool collideShapesWithShapes(const QVector& shapesA, const QVector& shapesB, CollisionList& collisions) { + bool collided = false; + int numShapesA = shapesA.size(); + for (int i = 0; i < numShapesA; ++i) { + Shape* shapeA = shapesA.at(i); + if (!shapeA) { + continue; + } + if (collideShapeWithShapes(shapeA, shapesB, 0, collisions)) { + collided = true; + if (collisions.isFull()) { + break; + } + } + } + return collided; +} + bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { int typeA = shapeA->getType(); if (typeA == Shape::SPHERE_SHAPE) { @@ -116,7 +156,7 @@ bool collideShapeWithAACube(const Shape* shapeA, const glm::vec3& cubeCenter, fl } bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, CollisionList& collisions) { - glm::vec3 BA = sphereB->getPosition() - sphereA->getPosition(); + glm::vec3 BA = sphereB->getTranslation() - sphereA->getTranslation(); float distanceSquared = glm::dot(BA, BA); float totalRadius = sphereA->getRadius() + sphereB->getRadius(); if (distanceSquared < totalRadius * totalRadius) { @@ -132,10 +172,11 @@ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, Collis // penetration points from A into B CollisionInfo* collision = collisions.getNewCollision(); if (collision) { - collision->_type = COLLISION_TYPE_UNKNOWN; collision->_penetration = BA * (totalRadius - distance); // contactPoint is on surface of A - collision->_contactPoint = sphereA->getPosition() + sphereA->getRadius() * BA; + collision->_contactPoint = sphereA->getTranslation() + sphereA->getRadius() * BA; + collision->_shapeA = sphereA; + collision->_shapeB = sphereB; return true; } } @@ -144,7 +185,7 @@ bool sphereSphere(const SphereShape* sphereA, const SphereShape* sphereB, Collis bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, CollisionList& collisions) { // find sphereA's closest approach to axis of capsuleB - glm::vec3 BA = capsuleB->getPosition() - sphereA->getPosition(); + glm::vec3 BA = capsuleB->getTranslation() - sphereA->getTranslation(); glm::vec3 capsuleAxis; capsuleB->computeNormalizedAxis(capsuleAxis); float axialDistance = - glm::dot(BA, capsuleAxis); @@ -179,8 +220,9 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col // penetration points from A into B collision->_penetration = (totalRadius - radialDistance) * radialAxis; // points from A into B // contactPoint is on surface of sphereA - collision->_contactPoint = sphereA->getPosition() + sphereA->getRadius() * radialAxis; - collision->_type = COLLISION_TYPE_UNKNOWN; + collision->_contactPoint = sphereA->getTranslation() + sphereA->getRadius() * radialAxis; + collision->_shapeA = sphereA; + collision->_shapeB = capsuleB; } else { // A is on B's axis, so the penetration is undefined... if (absAxialDistance > capsuleB->getHalfHeight()) { @@ -201,8 +243,9 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col float sign = (axialDistance > 0.0f) ? -1.0f : 1.0f; collision->_penetration = (sign * (totalRadius + capsuleB->getHalfHeight() - absAxialDistance)) * capsuleAxis; // contactPoint is on surface of sphereA - collision->_contactPoint = sphereA->getPosition() + (sign * sphereA->getRadius()) * capsuleAxis; - collision->_type = COLLISION_TYPE_UNKNOWN; + collision->_contactPoint = sphereA->getTranslation() + (sign * sphereA->getRadius()) * capsuleAxis; + collision->_shapeA = sphereA; + collision->_shapeB = capsuleB; } return true; } @@ -211,14 +254,15 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col bool spherePlane(const SphereShape* sphereA, const PlaneShape* planeB, CollisionList& collisions) { glm::vec3 penetration; - if (findSpherePlanePenetration(sphereA->getPosition(), sphereA->getRadius(), planeB->getCoefficients(), penetration)) { + if (findSpherePlanePenetration(sphereA->getTranslation(), sphereA->getRadius(), planeB->getCoefficients(), penetration)) { CollisionInfo* collision = collisions.getNewCollision(); if (!collision) { return false; // collision list is full } collision->_penetration = penetration; - collision->_contactPoint = sphereA->getPosition() + sphereA->getRadius() * glm::normalize(penetration); - collision->_type = COLLISION_TYPE_UNKNOWN; + collision->_contactPoint = sphereA->getTranslation() + sphereA->getRadius() * glm::normalize(penetration); + collision->_shapeA = sphereA; + collision->_shapeB = planeB; return true; } return false; @@ -226,7 +270,7 @@ bool spherePlane(const SphereShape* sphereA, const PlaneShape* planeB, Collision bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, CollisionList& collisions) { // find sphereB's closest approach to axis of capsuleA - glm::vec3 AB = capsuleA->getPosition() - sphereB->getPosition(); + glm::vec3 AB = capsuleA->getTranslation() - sphereB->getTranslation(); glm::vec3 capsuleAxis; capsuleA->computeNormalizedAxis(capsuleAxis); float axialDistance = - glm::dot(AB, capsuleAxis); @@ -242,14 +286,14 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col } // closestApproach = point on capsuleA's axis that is closest to sphereB's center - glm::vec3 closestApproach = capsuleA->getPosition() + axialDistance * capsuleAxis; + glm::vec3 closestApproach = capsuleA->getTranslation() + axialDistance * capsuleAxis; if (absAxialDistance > capsuleA->getHalfHeight()) { // sphere hits capsule on a cap // --> recompute radialAxis and closestApproach float sign = (axialDistance > 0.0f) ? 1.0f : -1.0f; - closestApproach = capsuleA->getPosition() + (sign * capsuleA->getHalfHeight()) * capsuleAxis; - radialAxis = closestApproach - sphereB->getPosition(); + closestApproach = capsuleA->getTranslation() + (sign * capsuleA->getHalfHeight()) * capsuleAxis; + radialAxis = closestApproach - sphereB->getTranslation(); radialDistance2 = glm::length2(radialAxis); if (radialDistance2 > totalRadius2) { return false; @@ -268,7 +312,8 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col collision->_penetration = (radialDistance - totalRadius) * radialAxis; // points from A into B // contactPoint is on surface of capsuleA collision->_contactPoint = closestApproach - capsuleA->getRadius() * radialAxis; - collision->_type = COLLISION_TYPE_UNKNOWN; + collision->_shapeA = capsuleA; + collision->_shapeB = sphereB; } else { // A is on B's axis, so the penetration is undefined... if (absAxialDistance > capsuleA->getHalfHeight()) { @@ -289,7 +334,8 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col collision->_penetration = (sign * (totalRadius + capsuleA->getHalfHeight() - absAxialDistance)) * capsuleAxis; // contactPoint is on surface of sphereA collision->_contactPoint = closestApproach + (sign * capsuleA->getRadius()) * capsuleAxis; - collision->_type = COLLISION_TYPE_UNKNOWN; + collision->_shapeA = capsuleA; + collision->_shapeB = sphereB; } } return true; @@ -302,8 +348,8 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, capsuleA->computeNormalizedAxis(axisA); glm::vec3 axisB; capsuleB->computeNormalizedAxis(axisB); - glm::vec3 centerA = capsuleA->getPosition(); - glm::vec3 centerB = capsuleB->getPosition(); + glm::vec3 centerA = capsuleA->getTranslation(); + glm::vec3 centerB = capsuleB->getTranslation(); // NOTE: The formula for closest approach between two lines is: // d = [(B - A) . (a - (a.b)b)] / (1 - (a.b)^2) @@ -361,7 +407,8 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, collision->_penetration = BA * (totalRadius - distance); // contactPoint is on surface of A collision->_contactPoint = centerA + distanceA * axisA + capsuleA->getRadius() * BA; - collision->_type = COLLISION_TYPE_UNKNOWN; + collision->_shapeA = capsuleA; + collision->_shapeB = capsuleB; return true; } } else { @@ -427,7 +474,8 @@ bool capsuleCapsule(const CapsuleShape* capsuleA, const CapsuleShape* capsuleB, // average the internal pair, and then do the math from centerB collision->_contactPoint = centerB + (0.5f * (points[1] + points[2])) * axisB + (capsuleA->getRadius() - distance) * BA; - collision->_type = COLLISION_TYPE_UNKNOWN; + collision->_shapeA = capsuleA; + collision->_shapeB = capsuleB; return true; } } @@ -447,7 +495,8 @@ bool capsulePlane(const CapsuleShape* capsuleA, const PlaneShape* planeB, Collis collision->_penetration = penetration; glm::vec3 deepestEnd = (glm::dot(start, glm::vec3(plane)) < glm::dot(end, glm::vec3(plane))) ? start : end; collision->_contactPoint = deepestEnd + capsuleA->getRadius() * glm::normalize(penetration); - collision->_type = COLLISION_TYPE_UNKNOWN; + collision->_shapeA = capsuleA; + collision->_shapeB = planeB; return true; } return false; @@ -455,15 +504,16 @@ bool capsulePlane(const CapsuleShape* capsuleA, const PlaneShape* planeB, Collis bool planeSphere(const PlaneShape* planeA, const SphereShape* sphereB, CollisionList& collisions) { glm::vec3 penetration; - if (findSpherePlanePenetration(sphereB->getPosition(), sphereB->getRadius(), planeA->getCoefficients(), penetration)) { + if (findSpherePlanePenetration(sphereB->getTranslation(), sphereB->getRadius(), planeA->getCoefficients(), penetration)) { CollisionInfo* collision = collisions.getNewCollision(); if (!collision) { return false; // collision list is full } collision->_penetration = -penetration; - collision->_contactPoint = sphereB->getPosition() + + collision->_contactPoint = sphereB->getTranslation() + (sphereB->getRadius() / glm::length(penetration) - 1.0f) * penetration; - collision->_type = COLLISION_TYPE_UNKNOWN; + collision->_shapeA = planeA; + collision->_shapeB = sphereB; return true; } return false; @@ -482,7 +532,8 @@ bool planeCapsule(const PlaneShape* planeA, const CapsuleShape* capsuleB, Collis collision->_penetration = -penetration; glm::vec3 deepestEnd = (glm::dot(start, glm::vec3(plane)) < glm::dot(end, glm::vec3(plane))) ? start : end; collision->_contactPoint = deepestEnd + (capsuleB->getRadius() / glm::length(penetration) - 1.0f) * penetration; - collision->_type = COLLISION_TYPE_UNKNOWN; + collision->_shapeA = planeA; + collision->_shapeB = capsuleB; return true; } return false; @@ -668,15 +719,15 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm:: direction /= lengthDirection; // compute collision details - collision->_type = COLLISION_TYPE_AACUBE; collision->_floatData = cubeSide; collision->_vecData = cubeCenter; collision->_penetration = (halfCubeSide * lengthDirection + sphereRadius - maxBA * glm::dot(BA, direction)) * direction; collision->_contactPoint = sphereCenter + sphereRadius * direction; } - collision->_type = COLLISION_TYPE_AACUBE; collision->_floatData = cubeSide; collision->_vecData = cubeCenter; + collision->_shapeA = NULL; + collision->_shapeB = NULL; return true; } else if (sphereRadius + halfCubeSide > distance) { // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means @@ -688,9 +739,10 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm:: // contactPoint is on surface of A collision->_contactPoint = sphereCenter + collision->_penetration; - collision->_type = COLLISION_TYPE_AACUBE; collision->_floatData = cubeSide; collision->_vecData = cubeCenter; + collision->_shapeA = NULL; + collision->_shapeB = NULL; return true; } } @@ -726,6 +778,8 @@ bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius, collision->_penetration = glm::dot(surfaceAB, direction) * direction; // contactPoint is on surface of A collision->_contactPoint = sphereCenter + sphereRadius * direction; + collision->_shapeA = NULL; + collision->_shapeB = NULL; return true; } } @@ -738,6 +792,8 @@ bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius, collision->_penetration = (sphereRadius + 0.5f * cubeSide) * glm::vec3(0.0f, -1.0f, 0.0f); // contactPoint is on surface of A collision->_contactPoint = sphereCenter + collision->_penetration; + collision->_shapeA = NULL; + collision->_shapeB = NULL; return true; } } @@ -746,21 +802,21 @@ bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius, */ bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { - return sphereAACube(sphereA->getPosition(), sphereA->getRadius(), cubeCenter, cubeSide, collisions); + return sphereAACube(sphereA->getTranslation(), sphereA->getRadius(), cubeCenter, cubeSide, collisions); } bool capsuleAACube(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { // find nerest approach of capsule line segment to cube glm::vec3 capsuleAxis; capsuleA->computeNormalizedAxis(capsuleAxis); - float offset = glm::dot(cubeCenter - capsuleA->getPosition(), capsuleAxis); + float offset = glm::dot(cubeCenter - capsuleA->getTranslation(), capsuleAxis); float halfHeight = capsuleA->getHalfHeight(); if (offset > halfHeight) { offset = halfHeight; } else if (offset < -halfHeight) { offset = -halfHeight; } - glm::vec3 nearestApproach = capsuleA->getPosition() + offset * capsuleAxis; + glm::vec3 nearestApproach = capsuleA->getTranslation() + offset * capsuleAxis; // collide nearest approach like a sphere at that point return sphereAACube(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions); } diff --git a/libraries/shared/src/ShapeCollider.h b/libraries/shared/src/ShapeCollider.h index 8261aceaf3..b1be75fa40 100644 --- a/libraries/shared/src/ShapeCollider.h +++ b/libraries/shared/src/ShapeCollider.h @@ -12,6 +12,8 @@ #ifndef hifi_ShapeCollider_h #define hifi_ShapeCollider_h +#include + #include "CapsuleShape.h" #include "CollisionInfo.h" #include "ListShape.h" @@ -33,6 +35,9 @@ namespace ShapeCollider { /// \return true if any shapes collide bool collideShapesCoarse(const QVector& shapesA, const QVector& shapesB, CollisionInfo& collision); + bool collideShapeWithShapes(const Shape* shapeA, const QVector& shapes, int startIndex, CollisionList& collisions); + bool collideShapesWithShapes(const QVector& shapesA, const QVector& shapesB, CollisionList& collisions); + /// \param shapeA a pointer to a shape (cannot be NULL) /// \param cubeCenter center of cube /// \param cubeSide lenght of side of cube diff --git a/libraries/shared/src/SharedUtil.h b/libraries/shared/src/SharedUtil.h index dbbfb02365..e5c2a0afc9 100644 --- a/libraries/shared/src/SharedUtil.h +++ b/libraries/shared/src/SharedUtil.h @@ -68,7 +68,7 @@ float randFloat(); int randIntInRange (int min, int max); float randFloatInRange (float min,float max); float randomSign(); /// \return -1.0 or 1.0 -unsigned char randomColorValue(int minimum); +unsigned char randomColorValue(int minimum = 0); bool randomBoolean(); glm::quat safeMix(const glm::quat& q1, const glm::quat& q2, float alpha); diff --git a/libraries/shared/src/SphereShape.cpp b/libraries/shared/src/SphereShape.cpp index 49137fac43..c77b0c97fb 100644 --- a/libraries/shared/src/SphereShape.cpp +++ b/libraries/shared/src/SphereShape.cpp @@ -17,14 +17,14 @@ bool SphereShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3 float r2 = _boundingRadius * _boundingRadius; // compute closest approach (CA) - float a = glm::dot(_position - rayStart, rayDirection); // a = distance from ray-start to CA - float b2 = glm::distance2(_position, rayStart + a * rayDirection); // b2 = squared distance from sphere-center to CA + float a = glm::dot(_translation - rayStart, rayDirection); // a = distance from ray-start to CA + float b2 = glm::distance2(_translation, rayStart + a * rayDirection); // b2 = squared distance from sphere-center to CA if (b2 > r2) { // ray does not hit sphere return false; } float c = sqrtf(r2 - b2); // c = distance from CA to sphere surface along rayDirection - float d2 = glm::distance2(rayStart, _position); // d2 = squared distance from sphere-center to ray-start + float d2 = glm::distance2(rayStart, _translation); // d2 = squared distance from sphere-center to ray-start if (a < 0.0f) { // ray points away from sphere-center if (d2 > r2) { diff --git a/libraries/shared/src/SphereShape.h b/libraries/shared/src/SphereShape.h index e87b8acab1..d2f2a8596f 100644 --- a/libraries/shared/src/SphereShape.h +++ b/libraries/shared/src/SphereShape.h @@ -14,6 +14,8 @@ #include "Shape.h" +#include "SharedUtil.h" + class SphereShape : public Shape { public: SphereShape() : Shape(Shape::SPHERE_SHAPE) {} @@ -26,11 +28,15 @@ public: _boundingRadius = radius; } + virtual ~SphereShape() {} + float getRadius() const { return _boundingRadius; } void setRadius(float radius) { _boundingRadius = radius; } bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const; + + float getVolume() const { return 1.3333333333f * PI * _boundingRadius * _boundingRadius * _boundingRadius; } }; #endif // hifi_SphereShape_h diff --git a/libraries/shared/src/VerletCapsuleShape.cpp b/libraries/shared/src/VerletCapsuleShape.cpp new file mode 100644 index 0000000000..3ac4899682 --- /dev/null +++ b/libraries/shared/src/VerletCapsuleShape.cpp @@ -0,0 +1,166 @@ +// +// VerletCapsuleShape.cpp +// libraries/shared/src +// +// Created by Andrew Meadows on 2014.06.16 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "VerletCapsuleShape.h" + +#include "Ragdoll.h" // for VerletPoint +#include "SharedUtil.h" + +VerletCapsuleShape::VerletCapsuleShape(VerletPoint* startPoint, VerletPoint* endPoint) : + CapsuleShape(), _startPoint(startPoint), _endPoint(endPoint), _startLagrangeCoef(0.5f), _endLagrangeCoef(0.5f) { + assert(startPoint); + assert(endPoint); + _halfHeight = 0.5f * glm::distance(_startPoint->_position, _endPoint->_position); + updateBoundingRadius(); +} + +VerletCapsuleShape::VerletCapsuleShape(float radius, VerletPoint* startPoint, VerletPoint* endPoint) : + CapsuleShape(radius, 1.0f), _startPoint(startPoint), _endPoint(endPoint), + _startLagrangeCoef(0.5f), _endLagrangeCoef(0.5f) { + assert(startPoint); + assert(endPoint); + _halfHeight = 0.5f * glm::distance(_startPoint->_position, _endPoint->_position); + updateBoundingRadius(); +} + +const glm::quat& VerletCapsuleShape::getRotation() const { + // NOTE: The "rotation" of this shape must be computed on the fly, + // which makes this method MUCH more more expensive than you might expect. + glm::vec3 axis; + computeNormalizedAxis(axis); + VerletCapsuleShape* thisCapsule = const_cast(this); + thisCapsule->_rotation = computeNewRotation(axis); + return _rotation; +} + +void VerletCapsuleShape::setRotation(const glm::quat& rotation) { + // NOTE: this method will update the verlet points, which is probably not + // what you want to do. Only call this method if you know what you're doing. + + // update points such that they have the same center but a different axis + glm::vec3 center = getTranslation(); + float halfHeight = getHalfHeight(); + glm::vec3 axis = rotation * DEFAULT_CAPSULE_AXIS; + _startPoint->_position = center - halfHeight * axis; + _endPoint->_position = center + halfHeight * axis; +} + +void VerletCapsuleShape::setTranslation(const glm::vec3& position) { + // NOTE: this method will update the verlet points, which is probably not + // what you want to do. Only call this method if you know what you're doing. + + // update the points such that their center is at position + glm::vec3 movement = position - getTranslation(); + _startPoint->_position += movement; + _endPoint->_position += movement; +} + +const glm::vec3& VerletCapsuleShape::getTranslation() const { + // the "translation" of this shape must be computed on the fly + VerletCapsuleShape* thisCapsule = const_cast(this); + thisCapsule->_translation = 0.5f * (_startPoint->_position + _endPoint->_position); + return _translation; +} + +float VerletCapsuleShape::computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) { + glm::vec3 startLeg = _startPoint->_position - contactPoint; + glm::vec3 endLeg = _endPoint->_position - contactPoint; + + // TODO: use fast approximate distance calculations here + float startLength = glm::length(startLeg); + float endlength = glm::length(endLeg); + + // The raw coefficient is proportional to the other leg's length multiplied by the dot-product + // of the penetration and this leg direction. We don't worry about the common penetration length + // because it is normalized out later. + float startCoef = glm::abs(glm::dot(startLeg, penetration)) * endlength / (startLength + EPSILON); + float endCoef = glm::abs(glm::dot(endLeg, penetration)) * startLength / (endlength + EPSILON); + + float maxCoef = glm::max(startCoef, endCoef); + if (maxCoef > EPSILON) { + // One of these coeficients will be 1.0, the other will be less --> + // one endpoint will move the full amount while the other will move less. + _startLagrangeCoef = startCoef / maxCoef; + _endLagrangeCoef = endCoef / maxCoef; + assert(!glm::isnan(_startLagrangeCoef)); + assert(!glm::isnan(_startLagrangeCoef)); + } else { + // The coefficients are the same --> the collision will move both equally + // as if the object were solid. + _startLagrangeCoef = 1.0f; + _endLagrangeCoef = 1.0f; + } + // the effective mass is the weighted sum of the two endpoints + return _startLagrangeCoef * _startPoint->_mass + _endLagrangeCoef * _endPoint->_mass; +} + +void VerletCapsuleShape::accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) { + assert(!glm::isnan(relativeMassFactor)); + _startPoint->accumulateDelta(relativeMassFactor * _startLagrangeCoef * penetration); + _endPoint->accumulateDelta(relativeMassFactor * _endLagrangeCoef * penetration); +} + +void VerletCapsuleShape::applyAccumulatedDelta() { + _startPoint->applyAccumulatedDelta(); + _endPoint->applyAccumulatedDelta(); +} + +// virtual +float VerletCapsuleShape::getHalfHeight() const { + return 0.5f * glm::distance(_startPoint->_position, _endPoint->_position); +} + +// virtual +void VerletCapsuleShape::getStartPoint(glm::vec3& startPoint) const { + startPoint = _startPoint->_position; +} + +// virtual +void VerletCapsuleShape::getEndPoint(glm::vec3& endPoint) const { + endPoint = _endPoint->_position; +} + +// virtual +void VerletCapsuleShape::computeNormalizedAxis(glm::vec3& axis) const { + glm::vec3 unormalizedAxis = _endPoint->_position - _startPoint->_position; + float fullLength = glm::length(unormalizedAxis); + if (fullLength > EPSILON) { + axis = unormalizedAxis / fullLength; + } else { + // the axis is meaningless, but we fill it with a normalized direction + // just in case the calling context assumes it really is normalized. + axis = glm::vec3(0.0f, 1.0f, 0.0f); + } +} + +// virtual +void VerletCapsuleShape::setHalfHeight(float halfHeight) { + // push points along axis so they are 2*halfHeight apart + glm::vec3 center = getTranslation(); + glm::vec3 axis; + computeNormalizedAxis(axis); + _startPoint->_position = center - halfHeight * axis; + _endPoint->_position = center + halfHeight * axis; + _boundingRadius = _radius + halfHeight; +} + +// virtual +void VerletCapsuleShape::setRadiusAndHalfHeight(float radius, float halfHeight) { + _radius = radius; + setHalfHeight(halfHeight); +} + +// virtual +void VerletCapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint) { + _startPoint->_position = startPoint; + _endPoint->_position = endPoint; + updateBoundingRadius(); +} diff --git a/libraries/shared/src/VerletCapsuleShape.h b/libraries/shared/src/VerletCapsuleShape.h new file mode 100644 index 0000000000..1fd84f5b1e --- /dev/null +++ b/libraries/shared/src/VerletCapsuleShape.h @@ -0,0 +1,83 @@ +// +// VerletCapsuleShape.h +// libraries/shared/src +// +// Created by Andrew Meadows on 2014.06.16 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_VerletCapsuleShape_h +#define hifi_VerletCapsuleShape_h + +#include "CapsuleShape.h" + + +// The VerletCapsuleShape is similar to a regular CapsuleShape, except it keeps a pointer +// to its endpoints which are owned by some other data structure (a verlet simulation system). +// This makes it easier for the points to be moved around by constraints in the system +// as well as collisions with the shape, however it has some drawbacks: +// +// (1) The Shape::_translation and ::_rotation data members are not used (wasted) +// +// (2) A VerletShape doesn't own the points that it uses, so you must be careful not to +// leave dangling pointers around. +// +// (3) Some const methods of VerletCapsuleShape are much more expensive than you might think. +// For example getHalfHeight() and setHalfHeight() methods must do extra computation. In +// particular setRotation() is significantly more expensive than for the CapsuleShape. +// Not too expensive to use when setting up shapes, but you woudln't want to use it deep +// down in a hot simulation loop, such as when processing collision results. Best to +// just let the verlet simulation do its thing and not try to constantly force a rotation. + +class VerletPoint; + +class VerletCapsuleShape : public CapsuleShape { +public: + VerletCapsuleShape(VerletPoint* startPoint, VerletPoint* endPoint); + VerletCapsuleShape(float radius, VerletPoint* startPoint, VerletPoint* endPoint); + + // virtual overrides from Shape + const glm::quat& getRotation() const; + void setRotation(const glm::quat& rotation); + void setTranslation(const glm::vec3& position); + const glm::vec3& getTranslation() const; + float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint); + void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration); + void applyAccumulatedDelta(); + + //float getRadius() const { return _radius; } + virtual float getHalfHeight() const; + + /// \param[out] startPoint is the center of start cap + void getStartPoint(glm::vec3& startPoint) const; + + /// \param[out] endPoint is the center of the end cap + void getEndPoint(glm::vec3& endPoint) const; + + /// \param[out] axis is a normalized vector that points from start to end + void computeNormalizedAxis(glm::vec3& axis) const; + + //void setRadius(float radius); + void setHalfHeight(float halfHeight); + void setRadiusAndHalfHeight(float radius, float halfHeight); + void setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint); + + //void assignEndPoints(glm::vec3* startPoint, glm::vec3* endPoint); + +protected: + // NOTE: VerletCapsuleShape does NOT own the data in its points. + VerletPoint* _startPoint; + VerletPoint* _endPoint; + + // The LagrangeCoef's are numerical weights for distributing collision movement + // between the relevant VerletPoints associated with this shape. They are functions + // of the movement parameters and are computed (and cached) in computeEffectiveMass() + // and then used in the subsequent accumulateDelta(). + float _startLagrangeCoef; + float _endLagrangeCoef; +}; + +#endif // hifi_VerletCapsuleShape_h diff --git a/libraries/shared/src/VerletSphereShape.cpp b/libraries/shared/src/VerletSphereShape.cpp new file mode 100644 index 0000000000..10c40c6611 --- /dev/null +++ b/libraries/shared/src/VerletSphereShape.cpp @@ -0,0 +1,50 @@ +// +// VerletSphereShape.cpp +// libraries/shared/src +// +// Created by Andrew Meadows on 2014.06.16 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "VerletSphereShape.h" + +#include "Ragdoll.h" // for VerletPoint + +VerletSphereShape::VerletSphereShape(VerletPoint* centerPoint) : SphereShape() { + assert(centerPoint); + _point = centerPoint; +} + +VerletSphereShape::VerletSphereShape(float radius, VerletPoint* centerPoint) : SphereShape(radius) { + assert(centerPoint); + _point = centerPoint; +} + +// virtual from Shape class +void VerletSphereShape::setTranslation(const glm::vec3& position) { + _point->_position = position; + _point->_lastPosition = position; +} + +// virtual from Shape class +const glm::vec3& VerletSphereShape::getTranslation() const { + return _point->_position; +} + +// virtual +float VerletSphereShape::computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) { + return _point->_mass; +} + +// virtual +void VerletSphereShape::accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) { + _point->accumulateDelta(relativeMassFactor * penetration); +} + +// virtual +void VerletSphereShape::applyAccumulatedDelta() { + _point->applyAccumulatedDelta(); +} diff --git a/libraries/shared/src/VerletSphereShape.h b/libraries/shared/src/VerletSphereShape.h new file mode 100644 index 0000000000..65da3b2597 --- /dev/null +++ b/libraries/shared/src/VerletSphereShape.h @@ -0,0 +1,47 @@ +// +// VerletSphereShape.h +// libraries/shared/src +// +// Created by Andrew Meadows on 2014.06.16 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_VerletSphereShape_h +#define hifi_VerletSphereShape_h + +#include "SphereShape.h" + +// The VerletSphereShape is similar to a regular SphereShape, except it keeps a pointer +// to its center which is owned by some other data structure (a verlet simulation system). +// This makes it easier for the points to be moved around by constraints in the system +// as well as collisions with the shape, however it has some drawbacks: +// +// (1) The Shape::_translation data member is not used (wasted) +// +// (2) A VerletShape doesn't own the points that it uses, so you must be careful not to +// leave dangling pointers around. + +class VerletPoint; + +class VerletSphereShape : public SphereShape { +public: + VerletSphereShape(VerletPoint* point); + + VerletSphereShape(float radius, VerletPoint* centerPoint); + + // virtual overrides from Shape + void setTranslation(const glm::vec3& position); + const glm::vec3& getTranslation() const; + float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint); + void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration); + void applyAccumulatedDelta(); + +protected: + // NOTE: VerletSphereShape does NOT own its _point + VerletPoint* _point; +}; + +#endif // hifi_VerletSphereShape_h diff --git a/tests/metavoxels/src/MetavoxelTests.cpp b/tests/metavoxels/src/MetavoxelTests.cpp index c9bce27cc3..287d3a648c 100644 --- a/tests/metavoxels/src/MetavoxelTests.cpp +++ b/tests/metavoxels/src/MetavoxelTests.cpp @@ -32,6 +32,8 @@ static int datagramsSent = 0; static int datagramsReceived = 0; static int bytesSent = 0; static int bytesReceived = 0; +static int maxDatagramsPerPacket = 0; +static int maxBytesPerPacket = 0; static int highPriorityMessagesSent = 0; static int highPriorityMessagesReceived = 0; static int unreliableMessagesSent = 0; @@ -45,6 +47,8 @@ static int sharedObjectsDestroyed = 0; static int objectMutationsPerformed = 0; static int scriptObjectsCreated = 0; static int scriptMutationsPerformed = 0; +static int metavoxelMutationsPerformed = 0; +static int spannerMutationsPerformed = 0; static QByteArray createRandomBytes(int minimumSize, int maximumSize) { QByteArray bytes(randIntInRange(minimumSize, maximumSize), 0); @@ -321,44 +325,80 @@ static bool testSerialization(Bitstream::MetadataType metadataType) { } bool MetavoxelTests::run() { - - qDebug() << "Running transmission tests..."; - qDebug(); - // seed the random number generator so that our tests are reproducible srand(0xBAAAAABE); - // create two endpoints with the same header + // check for an optional command line argument specifying a single test + QStringList arguments = this->arguments(); + int test = (arguments.size() > 1) ? arguments.at(1).toInt() : 0; + QByteArray datagramHeader("testheader"); - Endpoint alice(datagramHeader), bob(datagramHeader); - - alice.setOther(&bob); - bob.setOther(&alice); - - // perform a large number of simulation iterations const int SIMULATION_ITERATIONS = 10000; - for (int i = 0; i < SIMULATION_ITERATIONS; i++) { - if (alice.simulate(i) || bob.simulate(i)) { + if (test == 0 || test == 1) { + qDebug() << "Running transmission tests..."; + qDebug(); + + // create two endpoints with the same header + Endpoint alice(datagramHeader), bob(datagramHeader); + + alice.setOther(&bob); + bob.setOther(&alice); + + // perform a large number of simulation iterations + for (int i = 0; i < SIMULATION_ITERATIONS; i++) { + if (alice.simulate(i) || bob.simulate(i)) { + return true; + } + } + + qDebug() << "Sent" << highPriorityMessagesSent << "high priority messages, received" << highPriorityMessagesReceived; + qDebug() << "Sent" << unreliableMessagesSent << "unreliable messages, received" << unreliableMessagesReceived; + qDebug() << "Sent" << reliableMessagesSent << "reliable messages, received" << reliableMessagesReceived; + qDebug() << "Sent" << streamedBytesSent << "streamed bytes, received" << streamedBytesReceived; + qDebug() << "Sent" << datagramsSent << "datagrams with" << bytesSent << "bytes, received" << + datagramsReceived << "with" << bytesReceived << "bytes"; + qDebug() << "Max" << maxDatagramsPerPacket << "datagrams," << maxBytesPerPacket << "bytes per packet"; + qDebug() << "Created" << sharedObjectsCreated << "shared objects, destroyed" << sharedObjectsDestroyed; + qDebug() << "Performed" << objectMutationsPerformed << "object mutations"; + qDebug() << "Created" << scriptObjectsCreated << "script objects, mutated" << scriptMutationsPerformed; + qDebug(); + } + + if (test == 0 || test == 2) { + qDebug() << "Running serialization tests..."; + qDebug(); + + if (testSerialization(Bitstream::HASH_METADATA) || testSerialization(Bitstream::FULL_METADATA)) { return true; } } - qDebug() << "Sent" << highPriorityMessagesSent << "high priority messages, received" << highPriorityMessagesReceived; - qDebug() << "Sent" << unreliableMessagesSent << "unreliable messages, received" << unreliableMessagesReceived; - qDebug() << "Sent" << reliableMessagesSent << "reliable messages, received" << reliableMessagesReceived; - qDebug() << "Sent" << streamedBytesSent << "streamed bytes, received" << streamedBytesReceived; - qDebug() << "Sent" << datagramsSent << "datagrams with" << bytesSent << "bytes, received" << - datagramsReceived << "with" << bytesReceived << "bytes"; - qDebug() << "Created" << sharedObjectsCreated << "shared objects, destroyed" << sharedObjectsDestroyed; - qDebug() << "Performed" << objectMutationsPerformed << "object mutations"; - qDebug() << "Created" << scriptObjectsCreated << "script objects, mutated" << scriptMutationsPerformed; - qDebug(); + if (test == 0 || test == 3) { + qDebug() << "Running metavoxel data tests..."; + qDebug(); - qDebug() << "Running serialization tests..."; - qDebug(); + // clear the stats + datagramsSent = bytesSent = datagramsReceived = bytesReceived = maxDatagramsPerPacket = maxBytesPerPacket = 0; - if (testSerialization(Bitstream::HASH_METADATA) || testSerialization(Bitstream::FULL_METADATA)) { - return true; + // create client and server endpoints + Endpoint client(datagramHeader, Endpoint::METAVOXEL_CLIENT_MODE); + Endpoint server(datagramHeader, Endpoint::METAVOXEL_SERVER_MODE); + + client.setOther(&server); + server.setOther(&client); + + // simulate + for (int i = 0; i < SIMULATION_ITERATIONS; i++) { + if (client.simulate(i) || server.simulate(i)) { + return true; + } + } + + qDebug() << "Sent" << datagramsSent << "datagrams with" << bytesSent << "bytes, received" << + datagramsReceived << "with" << bytesReceived << "bytes"; + qDebug() << "Max" << maxDatagramsPerPacket << "datagrams," << maxBytesPerPacket << "bytes per packet"; + qDebug() << "Performed" << metavoxelMutationsPerformed << "metavoxel mutations," << spannerMutationsPerformed << + "spanner mutations"; } qDebug() << "All tests passed!"; @@ -375,7 +415,36 @@ static SharedObjectPointer createRandomSharedObject() { } } -Endpoint::Endpoint(const QByteArray& datagramHeader) : +class RandomVisitor : public MetavoxelVisitor { +public: + + int leafCount; + + RandomVisitor(); + virtual int visit(MetavoxelInfo& info); +}; + +RandomVisitor::RandomVisitor() : + MetavoxelVisitor(QVector(), + QVector() << AttributeRegistry::getInstance()->getColorAttribute()), + leafCount(0) { +} + +const float MAXIMUM_LEAF_SIZE = 0.5f; +const float MINIMUM_LEAF_SIZE = 0.25f; + +int RandomVisitor::visit(MetavoxelInfo& info) { + if (info.size > MAXIMUM_LEAF_SIZE || (info.size > MINIMUM_LEAF_SIZE && randomBoolean())) { + return DEFAULT_ORDER; + } + info.outputValues[0] = OwnedAttributeValue(_outputs.at(0), encodeInline(qRgb(randomColorValue(), + randomColorValue(), randomColorValue()))); + leafCount++; + return STOP_RECURSION; +} + +Endpoint::Endpoint(const QByteArray& datagramHeader, Mode mode) : + _mode(mode), _sequencer(new DatagramSequencer(datagramHeader, this)), _highPriorityMessagesToSend(0.0f), _reliableMessagesToSend(0.0f) { @@ -396,6 +465,25 @@ Endpoint::Endpoint(const QByteArray& datagramHeader) : ReceiveRecord receiveRecord = { 0 }; _receiveRecords.append(receiveRecord); + if (mode == METAVOXEL_CLIENT_MODE) { + _lod = MetavoxelLOD(glm::vec3(), 0.01f); + return; + } + if (mode == METAVOXEL_SERVER_MODE) { + _data.expand(); + _data.expand(); + + RandomVisitor visitor; + _data.guide(visitor); + qDebug() << "Created" << visitor.leafCount << "base leaves"; + + _data.insert(AttributeRegistry::getInstance()->getSpannersAttribute(), new Sphere()); + + _sphere = new Sphere(); + static_cast(_sphere.data())->setScale(0.01f); + _data.insert(AttributeRegistry::getInstance()->getSpannersAttribute(), _sphere); + return; + } // create the object that represents out delta-encoded state _localState = new TestSharedObjectA(); @@ -415,7 +503,7 @@ Endpoint::Endpoint(const QByteArray& datagramHeader) : QByteArray bytes = createRandomBytes(MIN_STREAM_BYTES, MAX_STREAM_BYTES); _dataStreamed.append(bytes); output->getBuffer().write(bytes); - streamedBytesSent += bytes.size(); + streamedBytesSent += bytes.size(); } static QVariant createRandomMessage() { @@ -512,6 +600,37 @@ static bool messagesEqual(const QVariant& firstMessage, const QVariant& secondMe } } +class MutateVisitor : public MetavoxelVisitor { +public: + + MutateVisitor(); + virtual int visit(MetavoxelInfo& info); + +private: + + int _mutationsRemaining; +}; + +MutateVisitor::MutateVisitor() : + MetavoxelVisitor(QVector(), + QVector() << AttributeRegistry::getInstance()->getColorAttribute()), + _mutationsRemaining(randIntInRange(2, 4)) { +} + +int MutateVisitor::visit(MetavoxelInfo& info) { + if (_mutationsRemaining <= 0) { + return STOP_RECURSION; + } + if (info.size > MAXIMUM_LEAF_SIZE || (info.size > MINIMUM_LEAF_SIZE && randomBoolean())) { + return encodeRandomOrder(); + } + info.outputValues[0] = OwnedAttributeValue(_outputs.at(0), encodeInline(qRgb(randomColorValue(), + randomColorValue(), randomColorValue()))); + _mutationsRemaining--; + metavoxelMutationsPerformed++; + return STOP_RECURSION; +} + bool Endpoint::simulate(int iterationNumber) { // update/send our delayed datagrams for (QList >::iterator it = _delayedDatagrams.begin(); it != _delayedDatagrams.end(); ) { @@ -525,51 +644,101 @@ bool Endpoint::simulate(int iterationNumber) { } } - // enqueue some number of high priority messages - const float MIN_HIGH_PRIORITY_MESSAGES = 0.0f; - const float MAX_HIGH_PRIORITY_MESSAGES = 2.0f; - _highPriorityMessagesToSend += randFloatInRange(MIN_HIGH_PRIORITY_MESSAGES, MAX_HIGH_PRIORITY_MESSAGES); - while (_highPriorityMessagesToSend >= 1.0f) { - QVariant message = createRandomMessage(); - _highPriorityMessagesSent.append(message); - _sequencer->sendHighPriorityMessage(message); - highPriorityMessagesSent++; - _highPriorityMessagesToSend -= 1.0f; - } - - // and some number of reliable messages - const float MIN_RELIABLE_MESSAGES = 0.0f; - const float MAX_RELIABLE_MESSAGES = 4.0f; - _reliableMessagesToSend += randFloatInRange(MIN_RELIABLE_MESSAGES, MAX_RELIABLE_MESSAGES); - while (_reliableMessagesToSend >= 1.0f) { - QVariant message = createRandomMessage(); - _reliableMessagesSent.append(message); - _sequencer->getReliableOutputChannel()->sendMessage(message); - reliableMessagesSent++; - _reliableMessagesToSend -= 1.0f; - } - - // tweak the local state - _localState = mutate(_localState); - - // send a packet - try { + int oldDatagramsSent = datagramsSent; + int oldBytesSent = bytesSent; + if (_mode == METAVOXEL_CLIENT_MODE) { Bitstream& out = _sequencer->startPacket(); - SequencedTestMessage message = { iterationNumber, createRandomMessage(), _localState }; - _unreliableMessagesSent.append(message); - unreliableMessagesSent++; - out << message; + + ClientStateMessage state = { _lod }; + out << QVariant::fromValue(state); _sequencer->endPacket(); + + // record the send + SendRecord record = { _sequencer->getOutgoingPacketNumber(), SharedObjectPointer(), MetavoxelData(), _lod }; + _sendRecords.append(record); + + } else if (_mode == METAVOXEL_SERVER_MODE) { + // make a random change + MutateVisitor visitor; + _data.guide(visitor); + + // perhaps mutate the spanner + if (randomBoolean()) { + SharedObjectPointer oldSphere = _sphere; + _sphere = _sphere->clone(true); + Sphere* newSphere = static_cast(_sphere.data()); + if (randomBoolean()) { + newSphere->setColor(QColor(randomColorValue(), randomColorValue(), randomColorValue())); + } else { + newSphere->setTranslation(newSphere->getTranslation() + glm::vec3(randFloatInRange(-0.01f, 0.01f), + randFloatInRange(-0.01f, 0.01f), randFloatInRange(-0.01f, 0.01f))); + } + _data.replace(AttributeRegistry::getInstance()->getSpannersAttribute(), oldSphere, _sphere); + spannerMutationsPerformed++; + } + + // wait until we have a valid lod before sending + if (!_lod.isValid()) { + return false; + } + Bitstream& out = _sequencer->startPacket(); + out << QVariant::fromValue(MetavoxelDeltaMessage()); + _data.writeDelta(_sendRecords.first().data, _sendRecords.first().lod, out, _lod); + + // record the send + SendRecord record = { _sequencer->getOutgoingPacketNumber() + 1, SharedObjectPointer(), _data, _lod }; + _sendRecords.append(record); + + _sequencer->endPacket(); + + } else { + // enqueue some number of high priority messages + const float MIN_HIGH_PRIORITY_MESSAGES = 0.0f; + const float MAX_HIGH_PRIORITY_MESSAGES = 2.0f; + _highPriorityMessagesToSend += randFloatInRange(MIN_HIGH_PRIORITY_MESSAGES, MAX_HIGH_PRIORITY_MESSAGES); + while (_highPriorityMessagesToSend >= 1.0f) { + QVariant message = createRandomMessage(); + _highPriorityMessagesSent.append(message); + _sequencer->sendHighPriorityMessage(message); + highPriorityMessagesSent++; + _highPriorityMessagesToSend -= 1.0f; + } + + // and some number of reliable messages + const float MIN_RELIABLE_MESSAGES = 0.0f; + const float MAX_RELIABLE_MESSAGES = 4.0f; + _reliableMessagesToSend += randFloatInRange(MIN_RELIABLE_MESSAGES, MAX_RELIABLE_MESSAGES); + while (_reliableMessagesToSend >= 1.0f) { + QVariant message = createRandomMessage(); + _reliableMessagesSent.append(message); + _sequencer->getReliableOutputChannel()->sendMessage(message); + reliableMessagesSent++; + _reliableMessagesToSend -= 1.0f; + } + + // tweak the local state + _localState = mutate(_localState); + + // send a packet + try { + Bitstream& out = _sequencer->startPacket(); + SequencedTestMessage message = { iterationNumber, createRandomMessage(), _localState }; + _unreliableMessagesSent.append(message); + unreliableMessagesSent++; + out << message; + _sequencer->endPacket(); + + } catch (const QString& message) { + qDebug() << message; + return true; + } - } catch (const QString& message) { - qDebug() << message; - return true; + // record the send + SendRecord record = { _sequencer->getOutgoingPacketNumber(), _localState }; + _sendRecords.append(record); } - - // record the send - SendRecord record = { _sequencer->getOutgoingPacketNumber(), _localState }; - _sendRecords.append(record); - + maxDatagramsPerPacket = qMax(maxDatagramsPerPacket, datagramsSent - oldDatagramsSent); + maxBytesPerPacket = qMax(maxBytesPerPacket, bytesSent - oldBytesSent); return false; } @@ -619,6 +788,39 @@ void Endpoint::handleHighPriorityMessage(const QVariant& message) { } void Endpoint::readMessage(Bitstream& in) { + if (_mode == METAVOXEL_CLIENT_MODE) { + QVariant message; + in >> message; + handleMessage(message, in); + + // deep-compare data to sent version + int packetNumber = _sequencer->getIncomingPacketNumber(); + foreach (const SendRecord& sendRecord, _other->_sendRecords) { + if (sendRecord.packetNumber == packetNumber) { + if (!sendRecord.data.deepEquals(_data, _sendRecords.first().lod)) { + qDebug() << "Sent/received metavoxel data mismatch."; + exit(true); + } + break; + } + } + + // record the receipt + ReceiveRecord record = { packetNumber, SharedObjectPointer(), _data, _sendRecords.first().lod }; + _receiveRecords.append(record); + return; + } + if (_mode == METAVOXEL_SERVER_MODE) { + QVariant message; + in >> message; + handleMessage(message, in); + + // record the receipt + ReceiveRecord record = { _sequencer->getIncomingPacketNumber() }; + _receiveRecords.append(record); + return; + } + SequencedTestMessage message; in >> message; @@ -632,17 +834,20 @@ void Endpoint::readMessage(Bitstream& in) { it != _other->_unreliableMessagesSent.end(); it++) { if (it->sequenceNumber == message.sequenceNumber) { if (!messagesEqual(it->submessage, message.submessage)) { - throw QString("Sent/received unreliable message mismatch."); + qDebug() << "Sent/received unreliable message mismatch."; + exit(true); } if (!it->state->equals(message.state)) { - throw QString("Delta-encoded object mismatch."); + qDebug() << "Delta-encoded object mismatch."; + exit(true); } _other->_unreliableMessagesSent.erase(_other->_unreliableMessagesSent.begin(), it + 1); unreliableMessagesReceived++; return; } } - throw QString("Received unsent/already sent unreliable message."); + qDebug() << "Received unsent/already sent unreliable message."; + exit(true); } void Endpoint::handleReliableMessage(const QVariant& message) { @@ -682,6 +887,22 @@ void Endpoint::clearReceiveRecordsBefore(int index) { _receiveRecords.erase(_receiveRecords.begin(), _receiveRecords.begin() + index + 1); } +void Endpoint::handleMessage(const QVariant& message, Bitstream& in) { + int userType = message.userType(); + if (userType == ClientStateMessage::Type) { + ClientStateMessage state = message.value(); + _lod = state.lod; + + } else if (userType == MetavoxelDeltaMessage::Type) { + _data.readDelta(_receiveRecords.first().data, _receiveRecords.first().lod, in, _sendRecords.first().lod); + + } else if (userType == QMetaType::QVariantList) { + foreach (const QVariant& element, message.toList()) { + handleMessage(element, in); + } + } +} + TestSharedObjectA::TestSharedObjectA(float foo, TestEnum baz, TestFlags bong) : _foo(foo), _baz(baz), diff --git a/tests/metavoxels/src/MetavoxelTests.h b/tests/metavoxels/src/MetavoxelTests.h index ac9eda2659..c340d78963 100644 --- a/tests/metavoxels/src/MetavoxelTests.h +++ b/tests/metavoxels/src/MetavoxelTests.h @@ -16,6 +16,7 @@ #include #include +#include #include class SequencedTestMessage; @@ -39,7 +40,9 @@ class Endpoint : public QObject { public: - Endpoint(const QByteArray& datagramHeader); + enum Mode { BASIC_PEER_MODE, METAVOXEL_SERVER_MODE, METAVOXEL_CLIENT_MODE }; + + Endpoint(const QByteArray& datagramHeader, Mode mode = BASIC_PEER_MODE); void setOther(Endpoint* other) { _other = other; } @@ -60,18 +63,26 @@ private slots: private: + void handleMessage(const QVariant& message, Bitstream& in); + class SendRecord { public: int packetNumber; SharedObjectPointer localState; + MetavoxelData data; + MetavoxelLOD lod; }; class ReceiveRecord { public: int packetNumber; SharedObjectPointer remoteState; + MetavoxelData data; + MetavoxelLOD lod; }; + Mode _mode; + DatagramSequencer* _sequencer; QList _sendRecords; QList _receiveRecords; @@ -79,6 +90,11 @@ private: SharedObjectPointer _localState; SharedObjectPointer _remoteState; + MetavoxelData _data; + MetavoxelLOD _lod; + + SharedObjectPointer _sphere; + Endpoint* _other; QList > _delayedDatagrams; float _highPriorityMessagesToSend; diff --git a/tests/octree/src/ModelTests.cpp b/tests/octree/src/ModelTests.cpp index 0a85af77a9..8358e4f7a5 100644 --- a/tests/octree/src/ModelTests.cpp +++ b/tests/octree/src/ModelTests.cpp @@ -236,6 +236,45 @@ void ModelTests::modelTreeTests(bool verbose) { qDebug() << "TIME - Test" << testsTaken <<":" << qPrintable(testName) << "elapsed=" << elapsedInMSecs << "msecs"; } + { + testsTaken++; + QString testName = "Performance - add model to tree 10,000 times"; + if (verbose) { + qDebug() << "Test" << testsTaken <<":" << qPrintable(testName); + } + + const int TEST_ITERATIONS = 10000; + quint64 start = usecTimestampNow(); + for (int i = 0; i < TEST_ITERATIONS; i++) { + uint32_t id = i + 2; // make sure it doesn't collide with previous model ids + ModelItemID modelID(id); + modelID.isKnownID = false; // this is a temporary workaround to allow local tree models to be added with known IDs + + float randomX = randFloatInRange(0.0f ,(float)TREE_SCALE); + float randomY = randFloatInRange(0.0f ,(float)TREE_SCALE); + float randomZ = randFloatInRange(0.0f ,(float)TREE_SCALE); + glm::vec3 randomPositionInMeters(randomX,randomY,randomZ); + + properties.setPosition(randomPositionInMeters); + properties.setRadius(halfMeter); + properties.setModelURL("https://s3-us-west-1.amazonaws.com/highfidelity-public/ozan/theater.fbx"); + + tree.addModel(modelID, properties); + } + quint64 end = usecTimestampNow(); + + bool passed = true; + if (passed) { + testsPassed++; + } else { + testsFailed++; + qDebug() << "FAILED - Test" << testsTaken <<":" << qPrintable(testName); + } + float USECS_PER_MSECS = 1000.0f; + float elapsedInMSecs = (float)(end - start) / USECS_PER_MSECS; + qDebug() << "TIME - Test" << testsTaken <<":" << qPrintable(testName) << "elapsed=" << elapsedInMSecs << "msecs"; + } + qDebug() << " tests passed:" << testsPassed << "out of" << testsTaken; if (verbose) { qDebug() << "******************************************************************************************"; diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index 608e012998..4d3c90b905 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -123,8 +123,8 @@ void ShapeColliderTests::sphereTouchesSphere() { } // contactPoint is on surface of sphereA - glm::vec3 AtoB = sphereB.getPosition() - sphereA.getPosition(); - glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * glm::normalize(AtoB); + glm::vec3 AtoB = sphereB.getTranslation() - sphereA.getTranslation(); + glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * glm::normalize(AtoB); inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ @@ -153,8 +153,8 @@ void ShapeColliderTests::sphereTouchesSphere() { } // contactPoint is on surface of sphereA - glm::vec3 BtoA = sphereA.getPosition() - sphereB.getPosition(); - glm::vec3 expectedContactPoint = sphereB.getPosition() + radiusB * glm::normalize(BtoA); + glm::vec3 BtoA = sphereA.getTranslation() - sphereB.getTranslation(); + glm::vec3 expectedContactPoint = sphereB.getTranslation() + radiusB * glm::normalize(BtoA); inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ @@ -182,7 +182,7 @@ void ShapeColliderTests::sphereMissesCapsule() { glm::quat rotation = glm::angleAxis(angle, axis); glm::vec3 translation(15.1f, -27.1f, -38.6f); capsuleB.setRotation(rotation); - capsuleB.setPosition(translation); + capsuleB.setTranslation(translation); CollisionList collisions(16); @@ -193,7 +193,7 @@ void ShapeColliderTests::sphereMissesCapsule() { for (int i = 0; i < numberOfSteps; ++i) { // translate sphereA into world-frame glm::vec3 localPosition = localStartPosition + ((float)i * delta) * yAxis; - sphereA.setPosition(rotation * localPosition + translation); + sphereA.setTranslation(rotation * localPosition + translation); // sphereA agains capsuleB if (ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) @@ -236,7 +236,7 @@ void ShapeColliderTests::sphereTouchesCapsule() { int numCollisions = 0; { // sphereA collides with capsuleB's cylindrical wall - sphereA.setPosition(radialOffset * xAxis); + sphereA.setTranslation(radialOffset * xAxis); if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) { @@ -258,7 +258,7 @@ void ShapeColliderTests::sphereTouchesCapsule() { } // contactPoint is on surface of sphereA - glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * xAxis; + glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * xAxis; inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ @@ -287,8 +287,8 @@ void ShapeColliderTests::sphereTouchesCapsule() { } // contactPoint is on surface of capsuleB - glm::vec3 BtoA = sphereA.getPosition() - capsuleB.getPosition(); - glm::vec3 closestApproach = capsuleB.getPosition() + glm::dot(BtoA, yAxis) * yAxis; + glm::vec3 BtoA = sphereA.getTranslation() - capsuleB.getTranslation(); + glm::vec3 closestApproach = capsuleB.getTranslation() + glm::dot(BtoA, yAxis) * yAxis; expectedContactPoint = closestApproach + radiusB * glm::normalize(BtoA - closestApproach); inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { @@ -299,7 +299,7 @@ void ShapeColliderTests::sphereTouchesCapsule() { } { // sphereA hits end cap at axis glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; - sphereA.setPosition(axialOffset * yAxis); + sphereA.setTranslation(axialOffset * yAxis); if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) { @@ -321,7 +321,7 @@ void ShapeColliderTests::sphereTouchesCapsule() { } // contactPoint is on surface of sphereA - glm::vec3 expectedContactPoint = sphereA.getPosition() - radiusA * yAxis; + glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * yAxis; inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ @@ -362,7 +362,7 @@ void ShapeColliderTests::sphereTouchesCapsule() { } { // sphereA hits start cap at axis glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; - sphereA.setPosition(axialOffset * yAxis); + sphereA.setTranslation(axialOffset * yAxis); if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) { @@ -384,7 +384,7 @@ void ShapeColliderTests::sphereTouchesCapsule() { } // contactPoint is on surface of sphereA - glm::vec3 expectedContactPoint = sphereA.getPosition() + radiusA * yAxis; + glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * yAxis; inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ @@ -441,12 +441,12 @@ void ShapeColliderTests::capsuleMissesCapsule() { float totalHalfLength = totalRadius + halfHeightA + halfHeightB; CapsuleShape capsuleA(radiusA, halfHeightA); - CapsuleShape capsuleB(radiusA, halfHeightA); + CapsuleShape capsuleB(radiusB, halfHeightB); CollisionList collisions(16); // side by side - capsuleB.setPosition((1.01f * totalRadius) * xAxis); + capsuleB.setTranslation((1.01f * totalRadius) * xAxis); if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ @@ -461,7 +461,7 @@ void ShapeColliderTests::capsuleMissesCapsule() { } // end to end - capsuleB.setPosition((1.01f * totalHalfLength) * xAxis); + capsuleB.setTranslation((1.01f * totalHalfLength) * xAxis); if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ @@ -478,7 +478,7 @@ void ShapeColliderTests::capsuleMissesCapsule() { // rotate B and move it to the side glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); capsuleB.setRotation(rotation); - capsuleB.setPosition((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); + capsuleB.setTranslation((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ @@ -516,7 +516,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { int numCollisions = 0; { // side by side - capsuleB.setPosition((0.99f * totalRadius) * xAxis); + capsuleB.setTranslation((0.99f * totalRadius) * xAxis); if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { std::cout << __FILE__ << ":" << __LINE__ @@ -536,7 +536,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { } { // end to end - capsuleB.setPosition((0.99f * totalHalfLength) * yAxis); + capsuleB.setTranslation((0.99f * totalHalfLength) * yAxis); if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { @@ -559,7 +559,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { { // rotate B and move it to the side glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); capsuleB.setRotation(rotation); - capsuleB.setPosition((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); + capsuleB.setTranslation((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) { @@ -584,7 +584,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); capsuleB.setRotation(rotation); glm::vec3 positionB = ((totalRadius + capsuleB.getHalfHeight()) - overlap) * xAxis; - capsuleB.setPosition(positionB); + capsuleB.setTranslation(positionB); // capsuleA vs capsuleB if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) @@ -605,7 +605,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { << " actual = " << collision->_penetration; } - glm::vec3 expectedContactPoint = capsuleA.getPosition() + radiusA * xAxis; + glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * xAxis; inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ @@ -633,7 +633,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { << std::endl; } - expectedContactPoint = capsuleB.getPosition() - (radiusB + halfHeightB) * xAxis; + expectedContactPoint = capsuleB.getTranslation() - (radiusB + halfHeightB) * xAxis; inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ @@ -649,7 +649,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); capsuleB.setRotation(rotation); glm::vec3 positionB = (totalRadius - overlap) * zAxis + shift * yAxis; - capsuleB.setPosition(positionB); + capsuleB.setTranslation(positionB); // capsuleA vs capsuleB if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) @@ -671,7 +671,7 @@ void ShapeColliderTests::capsuleTouchesCapsule() { << std::endl; } - glm::vec3 expectedContactPoint = capsuleA.getPosition() + radiusA * zAxis + shift * yAxis; + glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * zAxis + shift * yAxis; inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); if (fabs(inaccuracy) > EPSILON) { std::cout << __FILE__ << ":" << __LINE__ @@ -708,7 +708,7 @@ void ShapeColliderTests::sphereTouchesAACubeFaces() { float overlap = 0.25f; float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap; sphereCenter = cubeCenter + sphereOffset * axis; - sphere.setPosition(sphereCenter); + sphere.setTranslation(sphereCenter); if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis << std::endl; @@ -741,7 +741,7 @@ void ShapeColliderTests::sphereTouchesAACubeFaces() { float overlap = 1.25f * sphereRadius; float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap; sphereCenter = cubeCenter + sphereOffset * axis; - sphere.setPosition(sphereCenter); + sphere.setTranslation(sphereCenter); if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube." @@ -815,7 +815,7 @@ void ShapeColliderTests::sphereTouchesAACubeEdges() { float overlap = 0.25f; sphereCenter = cubeCenter + (lengthAxis * 0.5f * cubeSide + sphereRadius - overlap) * axis; - sphere.setPosition(sphereCenter); + sphere.setTranslation(sphereCenter); if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis << std::endl; @@ -857,42 +857,42 @@ void ShapeColliderTests::sphereMissesAACube() { // top sphereCenter = cubeCenter + sphereOffset * yAxis; - sphere.setPosition(sphereCenter); + sphere.setTranslation(sphereCenter); if (ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; } // bottom sphereCenter = cubeCenter - sphereOffset * yAxis; - sphere.setPosition(sphereCenter); + sphere.setTranslation(sphereCenter); if (ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; } // left sphereCenter = cubeCenter + sphereOffset * xAxis; - sphere.setPosition(sphereCenter); + sphere.setTranslation(sphereCenter); if (ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; } // right sphereCenter = cubeCenter - sphereOffset * xAxis; - sphere.setPosition(sphereCenter); + sphere.setTranslation(sphereCenter); if (ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; } // forward sphereCenter = cubeCenter + sphereOffset * zAxis; - sphere.setPosition(sphereCenter); + sphere.setTranslation(sphereCenter); if (ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; } // back sphereCenter = cubeCenter - sphereOffset * zAxis; - sphere.setPosition(sphereCenter); + sphere.setTranslation(sphereCenter); if (ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should NOT collide with cube" << std::endl; } @@ -955,7 +955,7 @@ void ShapeColliderTests::rayHitsSphere() { rayDirection = rotation * unrotatedRayDirection; sphere.setRadius(radius); - sphere.setPosition(rotation * translation); + sphere.setTranslation(rotation * translation); float distance = FLT_MAX; if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) { @@ -994,7 +994,7 @@ void ShapeColliderTests::rayBarelyHitsSphere() { rayStart = rotation * (rayStart + translation); rayDirection = rotation * rayDirection; - sphere.setPosition(rotation * translation); + sphere.setTranslation(rotation * translation); // ...and test again distance = FLT_MAX; @@ -1032,7 +1032,7 @@ void ShapeColliderTests::rayBarelyMissesSphere() { rayStart = rotation * (rayStart + translation); rayDirection = rotation * rayDirection; - sphere.setPosition(rotation * translation); + sphere.setTranslation(rotation * translation); // ...and test again distance = FLT_MAX; @@ -1186,7 +1186,7 @@ void ShapeColliderTests::rayHitsPlane() { float planeDistanceFromOrigin = 3.579; glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f); PlaneShape plane; - plane.setPosition(planePosition); + plane.setTranslation(planePosition); // make a simple ray float startDistance = 1.234f; @@ -1209,7 +1209,7 @@ void ShapeColliderTests::rayHitsPlane() { glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); glm::quat rotation = glm::angleAxis(angle, axis); - plane.setPosition(rotation * planePosition); + plane.setTranslation(rotation * planePosition); plane.setRotation(rotation); rayStart = rotation * rayStart; rayDirection = rotation * rayDirection; @@ -1231,7 +1231,7 @@ void ShapeColliderTests::rayMissesPlane() { float planeDistanceFromOrigin = 3.579; glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f); PlaneShape plane; - plane.setPosition(planePosition); + plane.setTranslation(planePosition); { // parallel rays should miss float startDistance = 1.234f; @@ -1251,7 +1251,7 @@ void ShapeColliderTests::rayMissesPlane() { glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); glm::quat rotation = glm::angleAxis(angle, axis); - plane.setPosition(rotation * planePosition); + plane.setTranslation(rotation * planePosition); plane.setRotation(rotation); rayStart = rotation * rayStart; rayDirection = rotation * rayDirection; @@ -1283,7 +1283,7 @@ void ShapeColliderTests::rayMissesPlane() { glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); glm::quat rotation = glm::angleAxis(angle, axis); - plane.setPosition(rotation * planePosition); + plane.setTranslation(rotation * planePosition); plane.setRotation(rotation); rayStart = rotation * rayStart; rayDirection = rotation * rayDirection; diff --git a/tests/physics/src/VerletShapeTests.cpp b/tests/physics/src/VerletShapeTests.cpp new file mode 100644 index 0000000000..3a3bd43278 --- /dev/null +++ b/tests/physics/src/VerletShapeTests.cpp @@ -0,0 +1,769 @@ +// +// VerletShapeTests.cpp +// tests/physics/src +// +// Created by Andrew Meadows on 02/21/2014. +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +//#include +#include +#include + +#include +#include + +#include +#include // for VerletPoint +#include +#include +#include +#include +#include + +#include "VerletShapeTests.h" + +const glm::vec3 origin(0.0f); +static const glm::vec3 xAxis(1.0f, 0.0f, 0.0f); +static const glm::vec3 yAxis(0.0f, 1.0f, 0.0f); +static const glm::vec3 zAxis(0.0f, 0.0f, 1.0f); + +void VerletShapeTests::setSpherePosition() { + float radius = 1.0f; + glm::vec3 offset(1.23f, 4.56f, 7.89f); + VerletPoint point; + VerletSphereShape sphere(radius, &point); + + point._position = glm::vec3(0.f); + float d = glm::distance(glm::vec3(0.0f), sphere.getTranslation()); + if (d != 0.0f) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should be at origin" << std::endl; + } + + point._position = offset; + d = glm::distance(glm::vec3(0.0f), sphere.getTranslation()); + if (d != glm::length(offset)) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should be at offset" << std::endl; + } +} + +void VerletShapeTests::sphereMissesSphere() { + // non-overlapping spheres of unequal size + + float radiusA = 7.0f; + float radiusB = 3.0f; + float alpha = 1.2f; + float beta = 1.3f; + glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); + float offsetDistance = alpha * radiusA + beta * radiusB; + + // create points for the sphere centers + VerletPoint points[2]; + + // give pointers to the spheres + VerletSphereShape sphereA(radiusA, (points + 0)); + VerletSphereShape sphereB(radiusB, (points + 1)); + + // set the positions of the spheres by slamming the points directly + points[0]._position = origin; + points[1]._position = offsetDistance * offsetDirection; + + CollisionList collisions(16); + + // collide A to B... + { + bool touching = ShapeCollider::collideShapes(&sphereA, &sphereB, collisions); + if (touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should NOT touch" << std::endl; + } + } + + // collide B to A... + { + bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions); + if (touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should NOT touch" << std::endl; + } + } + + // also test shapeShape + { + bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions); + if (touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should NOT touch" << std::endl; + } + } + + if (collisions.size() > 0) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected empty collision list but size is " << collisions.size() + << std::endl; + } +} + +void VerletShapeTests::sphereTouchesSphere() { + // overlapping spheres of unequal size + float radiusA = 7.0f; + float radiusB = 3.0f; + float alpha = 0.2f; + float beta = 0.3f; + glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); + float offsetDistance = alpha * radiusA + beta * radiusB; + float expectedPenetrationDistance = (1.0f - alpha) * radiusA + (1.0f - beta) * radiusB; + glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection; + + // create two points for the sphere centers + VerletPoint points[2]; + + // give pointers to the spheres + VerletSphereShape sphereA(radiusA, points+0); + VerletSphereShape sphereB(radiusB, points+1); + + // set the positions of the spheres by slamming the points directly + points[0]._position = origin; + points[1]._position = offsetDistance * offsetDirection; + + CollisionList collisions(16); + int numCollisions = 0; + + // collide A to B... + { + bool touching = ShapeCollider::collideShapes(&sphereA, &sphereB, collisions); + if (!touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should touch" << std::endl; + } else { + ++numCollisions; + } + + // verify state of collisions + if (numCollisions != collisions.size()) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected collisions size of " << numCollisions << " but actual size is " << collisions.size() + << std::endl; + } + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: null collision" << std::endl; + } + + // penetration points from sphereA into sphereB + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of sphereA + glm::vec3 AtoB = sphereB.getTranslation() - sphereA.getTranslation(); + glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * glm::normalize(AtoB); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + } + + // collide B to A... + { + bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions); + if (!touching) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphereA and sphereB should touch" << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into sphereB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + float inaccuracy = glm::length(collision->_penetration + expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of sphereA + glm::vec3 BtoA = sphereA.getTranslation() - sphereB.getTranslation(); + glm::vec3 expectedContactPoint = sphereB.getTranslation() + radiusB * glm::normalize(BtoA); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + } +} + +void VerletShapeTests::sphereMissesCapsule() { + // non-overlapping sphere and capsule + float radiusA = 1.5f; + float radiusB = 2.3f; + float totalRadius = radiusA + radiusB; + float halfHeightB = 1.7f; + float axialOffset = totalRadius + 1.1f * halfHeightB; + float radialOffset = 1.2f * radiusA + 1.3f * radiusB; + + // create points for the sphere + capsule + VerletPoint points[3]; + for (int i = 0; i < 3; ++i) { + points[i]._position = glm::vec3(0.0f); + } + + // give the points to the shapes + VerletSphereShape sphereA(radiusA, points); + VerletCapsuleShape capsuleB(radiusB, points+1, points+2); + capsuleB.setHalfHeight(halfHeightB); + + // give the capsule some arbitrary transform + float angle = 37.8f; + glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) ); + glm::quat rotation = glm::angleAxis(angle, axis); + glm::vec3 translation(15.1f, -27.1f, -38.6f); + capsuleB.setRotation(rotation); + capsuleB.setTranslation(translation); + + CollisionList collisions(16); + + // walk sphereA along the local yAxis next to, but not touching, capsuleB + glm::vec3 localStartPosition(radialOffset, axialOffset, 0.0f); + int numberOfSteps = 10; + float delta = 1.3f * (totalRadius + halfHeightB) / (numberOfSteps - 1); + for (int i = 0; i < numberOfSteps; ++i) { + // translate sphereA into world-frame + glm::vec3 localPosition = localStartPosition + ((float)i * delta) * yAxis; + sphereA.setTranslation(rotation * localPosition + translation); + + // sphereA agains capsuleB + if (ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should NOT touch" + << std::endl; + } + + // capsuleB against sphereA + if (ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should NOT touch" + << std::endl; + } + } + + if (collisions.size() > 0) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected empty collision list but size is " << collisions.size() + << std::endl; + } +} + +void VerletShapeTests::sphereTouchesCapsule() { + // overlapping sphere and capsule + float radiusA = 2.0f; + float radiusB = 1.0f; + float totalRadius = radiusA + radiusB; + float halfHeightB = 2.0f; + float alpha = 0.5f; + float beta = 0.5f; + float radialOffset = alpha * radiusA + beta * radiusB; + + // create points for the sphere + capsule + VerletPoint points[3]; + for (int i = 0; i < 3; ++i) { + points[i]._position = glm::vec3(0.0f); + } + + // give the points to the shapes + VerletSphereShape sphereA(radiusA, points); + VerletCapsuleShape capsuleB(radiusB, points+1, points+2); + capsuleB.setHalfHeight(halfHeightB); + + CollisionList collisions(16); + int numCollisions = 0; + + { // sphereA collides with capsuleB's cylindrical wall + sphereA.setTranslation(radialOffset * xAxis); + + if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into capsuleB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + glm::vec3 expectedPenetration = (radialOffset - totalRadius) * xAxis; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of sphereA + glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * xAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + + // capsuleB collides with sphereA + if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and sphere should touch" + << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into capsuleB + collision = collisions.getCollision(numCollisions - 1); + expectedPenetration = - (radialOffset - totalRadius) * xAxis; + inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of capsuleB + glm::vec3 BtoA = sphereA.getTranslation() - capsuleB.getTranslation(); + glm::vec3 closestApproach = capsuleB.getTranslation() + glm::dot(BtoA, yAxis) * yAxis; + expectedContactPoint = closestApproach + radiusB * glm::normalize(BtoA - closestApproach); + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + } + { // sphereA hits end cap at axis + glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; + sphereA.setTranslation(axialOffset * yAxis); + + if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into capsuleB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + glm::vec3 expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of sphereA + glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * yAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + + // capsuleB collides with sphereA + if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and sphere should touch" + << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into capsuleB + collision = collisions.getCollision(numCollisions - 1); + expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; + inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of capsuleB + glm::vec3 endPoint; + capsuleB.getEndPoint(endPoint); + expectedContactPoint = endPoint + radiusB * yAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + } + { // sphereA hits start cap at axis + glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis; + sphereA.setTranslation(axialOffset * yAxis); + + if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: sphere and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into capsuleB + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + glm::vec3 expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of sphereA + glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * yAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + + // capsuleB collides with sphereA + if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and sphere should touch" + << std::endl; + } else { + ++numCollisions; + } + + // penetration points from sphereA into capsuleB + collision = collisions.getCollision(numCollisions - 1); + expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis; + inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + // contactPoint is on surface of capsuleB + glm::vec3 startPoint; + capsuleB.getStartPoint(startPoint); + expectedContactPoint = startPoint - radiusB * yAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + } + if (collisions.size() != numCollisions) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected " << numCollisions << " collisions but actual number is " << collisions.size() + << std::endl; + } +} + +void VerletShapeTests::capsuleMissesCapsule() { + // non-overlapping capsules + float radiusA = 2.0f; + float halfHeightA = 3.0f; + float radiusB = 3.0f; + float halfHeightB = 4.0f; + + float totalRadius = radiusA + radiusB; + float totalHalfLength = totalRadius + halfHeightA + halfHeightB; + + // create points for the shapes + VerletPoint points[4]; + for (int i = 0; i < 4; ++i) { + points[i]._position = glm::vec3(0.0f); + } + + // give the points to the shapes + VerletCapsuleShape capsuleA(radiusA, points+0, points+1); + VerletCapsuleShape capsuleB(radiusB, points+2, points+3); + capsuleA.setHalfHeight(halfHeightA); + capsuleA.setHalfHeight(halfHeightB); + + CollisionList collisions(16); + + // side by side + capsuleB.setTranslation((1.01f * totalRadius) * xAxis); + if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should NOT touch" + << std::endl; + } + if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should NOT touch" + << std::endl; + } + + // end to end + capsuleB.setTranslation((1.01f * totalHalfLength) * xAxis); + if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should NOT touch" + << std::endl; + } + if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should NOT touch" + << std::endl; + } + + // rotate B and move it to the side + glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); + capsuleB.setRotation(rotation); + capsuleB.setTranslation((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); + if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should NOT touch" + << std::endl; + } + if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should NOT touch" + << std::endl; + } + + if (collisions.size() > 0) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: expected empty collision list but size is " << collisions.size() + << std::endl; + } +} + +void VerletShapeTests::capsuleTouchesCapsule() { + // overlapping capsules + float radiusA = 2.0f; + float halfHeightA = 3.0f; + float radiusB = 3.0f; + float halfHeightB = 4.0f; + + float totalRadius = radiusA + radiusB; + float totalHalfLength = totalRadius + halfHeightA + halfHeightB; + + // create points for the shapes + VerletPoint points[4]; + for (int i = 0; i < 4; ++i) { + points[i]._position = glm::vec3(0.0f); + } + + // give the points to the shapes + VerletCapsuleShape capsuleA(radiusA, points+0, points+1); + VerletCapsuleShape capsuleB(radiusB, points+2, points+3); + capsuleA.setHalfHeight(halfHeightA); + capsuleB.setHalfHeight(halfHeightB); + + CollisionList collisions(16); + int numCollisions = 0; + + { // side by side + capsuleB.setTranslation((0.99f * totalRadius) * xAxis); + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + } + + { // end to end + capsuleB.setTranslation((0.99f * totalHalfLength) * yAxis); + + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + } + + { // rotate B and move it to the side + glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); + capsuleB.setRotation(rotation); + capsuleB.setTranslation((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis); + + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + } + + { // again, but this time check collision details + float overlap = 0.1f; + glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); + capsuleB.setRotation(rotation); + glm::vec3 positionB = ((totalRadius + capsuleB.getHalfHeight()) - overlap) * xAxis; + capsuleB.setTranslation(positionB); + + // capsuleA vs capsuleB + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + glm::vec3 expectedPenetration = overlap * xAxis; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration; + } + + glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * xAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint; + } + + // capsuleB vs capsuleA + if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + + collision = collisions.getCollision(numCollisions - 1); + expectedPenetration = - overlap * xAxis; + inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration + << std::endl; + } + + expectedContactPoint = capsuleB.getTranslation() - (radiusB + halfHeightB) * xAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint + << std::endl; + } + } + + { // collide cylinder wall against cylinder wall + float overlap = 0.137f; + float shift = 0.317f * halfHeightA; + glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis); + capsuleB.setRotation(rotation); + glm::vec3 positionB = (totalRadius - overlap) * zAxis + shift * yAxis; + capsuleB.setTranslation(positionB); + + // capsuleA vs capsuleB + if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions)) + { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: capsule and capsule should touch" + << std::endl; + } else { + ++numCollisions; + } + + CollisionInfo* collision = collisions.getCollision(numCollisions - 1); + glm::vec3 expectedPenetration = overlap * zAxis; + float inaccuracy = glm::length(collision->_penetration - expectedPenetration); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad penetration: expected = " << expectedPenetration + << " actual = " << collision->_penetration + << std::endl; + } + + glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * zAxis + shift * yAxis; + inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint); + if (fabs(inaccuracy) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ + << " ERROR: bad contactPoint: expected = " << expectedContactPoint + << " actual = " << collision->_contactPoint + << std::endl; + } + } +} + +void VerletShapeTests::runAllTests() { + setSpherePosition(); + sphereMissesSphere(); + sphereTouchesSphere(); + + sphereMissesCapsule(); + sphereTouchesCapsule(); + + capsuleMissesCapsule(); + capsuleTouchesCapsule(); +} diff --git a/tests/physics/src/VerletShapeTests.h b/tests/physics/src/VerletShapeTests.h new file mode 100644 index 0000000000..36e2fe0cbd --- /dev/null +++ b/tests/physics/src/VerletShapeTests.h @@ -0,0 +1,30 @@ +// +// VerletShapeTests.h +// tests/physics/src +// +// Created by Andrew Meadows on 2014.06.18 +// Copyright 2014 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_VerletShapeTests_h +#define hifi_VerletShapeTests_h + +namespace VerletShapeTests { + void setSpherePosition(); + + void sphereMissesSphere(); + void sphereTouchesSphere(); + + void sphereMissesCapsule(); + void sphereTouchesCapsule(); + + void capsuleMissesCapsule(); + void capsuleTouchesCapsule(); + + void runAllTests(); +} + +#endif // hifi_VerletShapeTests_h diff --git a/tests/physics/src/main.cpp b/tests/physics/src/main.cpp index ca98f4d546..086bff4dcd 100644 --- a/tests/physics/src/main.cpp +++ b/tests/physics/src/main.cpp @@ -9,8 +9,10 @@ // #include "ShapeColliderTests.h" +#include "VerletShapeTests.h" int main(int argc, char** argv) { ShapeColliderTests::runAllTests(); + VerletShapeTests::runAllTests(); return 0; }