Merge branch 'master' of https://github.com/highfidelity/hifi into deference

assignment-client/src/audio/AudioMixer.cpp

@@ -102,7 +102,7 @@ AudioMixer::~AudioMixer() {
 
 const float ATTENUATION_BEGINS_AT_DISTANCE = 1.0f;
 const float ATTENUATION_AMOUNT_PER_DOUBLING_IN_DISTANCE = 0.18f;
-const float ATTENUATION_EPSILON_DISTANCE = 0.1f;
+const float RADIUS_OF_HEAD = 0.076f;
 
 int AudioMixer::addStreamToMixForListeningNodeWithStream(PositionalAudioStream* streamToAdd,
                                                           AvatarAudioStream* listeningNodeStream) {
@@ -112,6 +112,8 @@ int AudioMixer::addStreamToMixForListeningNodeWithStream(PositionalAudioStream*
     // Basically, we'll repeat that last frame until it has a frame to mix.  Depending on how many times
     // we've repeated that frame in a row, we'll gradually fade that repeated frame into silence.
     // This improves the perceived quality of the audio slightly.
+    
+    bool showDebug = false;  // (randFloat() < 0.05f);
 
     float repeatedFrameFadeFactor = 1.0f;
 
@@ -140,112 +142,117 @@ int AudioMixer::addStreamToMixForListeningNodeWithStream(PositionalAudioStream*
     int numSamplesDelay = 0;
     float weakChannelAmplitudeRatio = 1.0f;
     
-    bool shouldAttenuate = (streamToAdd != listeningNodeStream);
+    bool shouldDistanceAttenuate = true;
     
-    if (shouldAttenuate) {
+    //  Is the source that I am mixing my own?
-        
+    bool sourceIsSelf = (streamToAdd == listeningNodeStream);
-        // if the two stream pointers do not match then these are different streams
+    
-        glm::vec3 relativePosition = streamToAdd->getPosition() - listeningNodeStream->getPosition();
+    glm::vec3 relativePosition = streamToAdd->getPosition() - listeningNodeStream->getPosition();
-        
+    
-        float distanceBetween = glm::length(relativePosition);
+    float distanceBetween = glm::length(relativePosition);
-        
+    
-        if (distanceBetween < EPSILON) {
+    if (distanceBetween < EPSILON) {
-            distanceBetween = EPSILON;
+        distanceBetween = EPSILON;
+    }
+    
+    if (streamToAdd->getLastPopOutputTrailingLoudness() / distanceBetween <= _minAudibilityThreshold) {
+        // according to mixer performance we have decided this does not get to be mixed in
+        // bail out
+        return 0;
+    }
+    
+    ++_sumMixes;
+    
+    if (streamToAdd->getListenerUnattenuatedZone()) {
+        shouldDistanceAttenuate = !streamToAdd->getListenerUnattenuatedZone()->contains(listeningNodeStream->getPosition());
+    }
+    
+    if (streamToAdd->getType() == PositionalAudioStream::Injector) {
+        attenuationCoefficient *= reinterpret_cast<InjectedAudioStream*>(streamToAdd)->getAttenuationRatio();
+        if (showDebug) {
+            qDebug() << "AttenuationRatio: " << reinterpret_cast<InjectedAudioStream*>(streamToAdd)->getAttenuationRatio();
         }
+    }
+    
+    if (showDebug) {
+        qDebug() << "distance: " << distanceBetween;
+    }
+    
+    glm::quat inverseOrientation = glm::inverse(listeningNodeStream->getOrientation());
+    
+    if (!sourceIsSelf && (streamToAdd->getType() == PositionalAudioStream::Microphone)) {
+        //  source is another avatar, apply fixed off-axis attenuation to make them quieter as they turn away from listener
+        glm::vec3 rotatedListenerPosition = glm::inverse(streamToAdd->getOrientation()) * relativePosition;
         
-        if (streamToAdd->getLastPopOutputTrailingLoudness() / distanceBetween <= _minAudibilityThreshold) {
+        float angleOfDelivery = glm::angle(glm::vec3(0.0f, 0.0f, -1.0f),
-            // according to mixer performance we have decided this does not get to be mixed in
+                                           glm::normalize(rotatedListenerPosition));
-            // bail out
-            return 0;
-        }
         
-        ++_sumMixes;
+        const float MAX_OFF_AXIS_ATTENUATION = 0.2f;
+        const float OFF_AXIS_ATTENUATION_FORMULA_STEP = (1 - MAX_OFF_AXIS_ATTENUATION) / 2.0f;
         
-        if (streamToAdd->getListenerUnattenuatedZone()) {
+        float offAxisCoefficient = MAX_OFF_AXIS_ATTENUATION +
-            shouldAttenuate = !streamToAdd->getListenerUnattenuatedZone()->contains(listeningNodeStream->getPosition());
+                                    (OFF_AXIS_ATTENUATION_FORMULA_STEP * (angleOfDelivery / PI_OVER_TWO));
-        }
         
-        if (streamToAdd->getType() == PositionalAudioStream::Injector) {
+        if (showDebug) {
-            attenuationCoefficient *= reinterpret_cast<InjectedAudioStream*>(streamToAdd)->getAttenuationRatio();
+            qDebug() << "angleOfDelivery" << angleOfDelivery << "offAxisCoefficient: " << offAxisCoefficient;
-        }
-        
-        shouldAttenuate = shouldAttenuate && distanceBetween > ATTENUATION_EPSILON_DISTANCE;
-        
-        if (shouldAttenuate) {
-            glm::quat inverseOrientation = glm::inverse(listeningNodeStream->getOrientation());
             
-            float distanceSquareToSource = glm::dot(relativePosition, relativePosition);
-            float radius = 0.0f;
-            
-            if (streamToAdd->getType() == PositionalAudioStream::Injector) {
-                radius = reinterpret_cast<InjectedAudioStream*>(streamToAdd)->getRadius();
-            }
-            
-            if (radius == 0 || (distanceSquareToSource > radius * radius)) {
-                // this is either not a spherical source, or the listener is outside the sphere
-                
-                if (radius > 0) {
-                    // this is a spherical source - the distance used for the coefficient
-                    // needs to be the closest point on the boundary to the source
-                    
-                    // ovveride the distance to the node with the distance to the point on the
-                    // boundary of the sphere
-                    distanceSquareToSource -= (radius * radius);
-                    
-                } else {
-                    // calculate the angle delivery for off-axis attenuation
-                    glm::vec3 rotatedListenerPosition = glm::inverse(streamToAdd->getOrientation()) * relativePosition;
-                    
-                    float angleOfDelivery = glm::angle(glm::vec3(0.0f, 0.0f, -1.0f),
-                                                       glm::normalize(rotatedListenerPosition));
-                    
-                    const float MAX_OFF_AXIS_ATTENUATION = 0.2f;
-                    const float OFF_AXIS_ATTENUATION_FORMULA_STEP = (1 - MAX_OFF_AXIS_ATTENUATION) / 2.0f;
-                    
-                    float offAxisCoefficient = MAX_OFF_AXIS_ATTENUATION +
-                        (OFF_AXIS_ATTENUATION_FORMULA_STEP * (angleOfDelivery / PI_OVER_TWO));
-                    
-                    // multiply the current attenuation coefficient by the calculated off axis coefficient
-                    attenuationCoefficient *= offAxisCoefficient;
-                }
-                
-                glm::vec3 rotatedSourcePosition = inverseOrientation * relativePosition;
-                
-                if (distanceBetween >= ATTENUATION_BEGINS_AT_DISTANCE) {
-                    // calculate the distance coefficient using the distance to this node
-                    float distanceCoefficient = 1 - (logf(distanceBetween / ATTENUATION_BEGINS_AT_DISTANCE) / logf(2.0f)
-                                                     * ATTENUATION_AMOUNT_PER_DOUBLING_IN_DISTANCE);
-                    
-                    if (distanceCoefficient < 0) {
-                        distanceCoefficient = 0;
-                    }
-                    
-                    // multiply the current attenuation coefficient by the distance coefficient
-                    attenuationCoefficient *= distanceCoefficient;
-                }
-                
-                // project the rotated source position vector onto the XZ plane
-                rotatedSourcePosition.y = 0.0f;
-                
-                // produce an oriented angle about the y-axis
-                bearingRelativeAngleToSource = glm::orientedAngle(glm::vec3(0.0f, 0.0f, -1.0f),
-                                                                  glm::normalize(rotatedSourcePosition),
-                                                                  glm::vec3(0.0f, 1.0f, 0.0f));
-                
-                const float PHASE_AMPLITUDE_RATIO_AT_90 = 0.5;
-                
-                // figure out the number of samples of delay and the ratio of the amplitude
-                // in the weak channel for audio spatialization
-                float sinRatio = fabsf(sinf(bearingRelativeAngleToSource));
-                numSamplesDelay = SAMPLE_PHASE_DELAY_AT_90 * sinRatio;
-                weakChannelAmplitudeRatio = 1 - (PHASE_AMPLITUDE_RATIO_AT_90 * sinRatio);
-            }
         }
+        // multiply the current attenuation coefficient by the calculated off axis coefficient
+        
+        attenuationCoefficient *= offAxisCoefficient;
+    }
+    
+    if (shouldDistanceAttenuate && (distanceBetween >= ATTENUATION_BEGINS_AT_DISTANCE)) {
+        // calculate the distance coefficient using the distance to this node
+        float distanceCoefficient = 1 - (logf(distanceBetween / ATTENUATION_BEGINS_AT_DISTANCE) / logf(2.0f)
+                                         * ATTENUATION_AMOUNT_PER_DOUBLING_IN_DISTANCE);
+        
+        if (distanceCoefficient < 0) {
+            distanceCoefficient = 0;
+        }
+        
+        // multiply the current attenuation coefficient by the distance coefficient
+        attenuationCoefficient *= distanceCoefficient;
+        if (showDebug) {
+            qDebug() << "distanceCoefficient: " << distanceCoefficient;
+        }
+    }
+    
+    if (!sourceIsSelf) {
+        //  Compute sample delay for the two ears to create phase panning
+        glm::vec3 rotatedSourcePosition = inverseOrientation * relativePosition;
+
+        // project the rotated source position vector onto the XZ plane
+        rotatedSourcePosition.y = 0.0f;
+        
+        // produce an oriented angle about the y-axis
+        bearingRelativeAngleToSource = glm::orientedAngle(glm::vec3(0.0f, 0.0f, -1.0f),
+                                                          glm::normalize(rotatedSourcePosition),
+                                                          glm::vec3(0.0f, 1.0f, 0.0f));
+        
+        const float PHASE_AMPLITUDE_RATIO_AT_90 = 0.5;
+        
+        // figure out the number of samples of delay and the ratio of the amplitude
+        // in the weak channel for audio spatialization
+        float sinRatio = fabsf(sinf(bearingRelativeAngleToSource));
+        numSamplesDelay = SAMPLE_PHASE_DELAY_AT_90 * sinRatio;
+        weakChannelAmplitudeRatio = 1 - (PHASE_AMPLITUDE_RATIO_AT_90 * sinRatio);
+        
+        if (distanceBetween < RADIUS_OF_HEAD) {
+            // Diminish phase panning if source would be inside head
+            numSamplesDelay *= distanceBetween / RADIUS_OF_HEAD;
+            weakChannelAmplitudeRatio += (PHASE_AMPLITUDE_RATIO_AT_90 * sinRatio) * distanceBetween / RADIUS_OF_HEAD;
+        }
+    }
+    
+    if (showDebug) {
+        qDebug() << "attenuation: " << attenuationCoefficient;
+        qDebug() << "bearingRelativeAngleToSource: " << bearingRelativeAngleToSource << " numSamplesDelay: " << numSamplesDelay;
     }
     
     AudioRingBuffer::ConstIterator streamPopOutput = streamToAdd->getLastPopOutput();
     
-    if (!streamToAdd->isStereo() && shouldAttenuate) {
+    if (!streamToAdd->isStereo()) {
         // this is a mono stream, which means it gets full attenuation and spatialization
         
         // if the bearing relative angle to source is > 0 then the delayed channel is the right one
@@ -293,11 +300,7 @@ int AudioMixer::addStreamToMixForListeningNodeWithStream(PositionalAudioStream*
     } else {
         int stereoDivider = streamToAdd->isStereo() ? 1 : 2;
 
-        if (!shouldAttenuate) {
+       float attenuationAndFade = attenuationCoefficient * repeatedFrameFadeFactor;
-            attenuationCoefficient = 1.0f;
-        }
-
-        float attenuationAndFade = attenuationCoefficient * repeatedFrameFadeFactor;
 
         for (int s = 0; s < NETWORK_BUFFER_LENGTH_SAMPLES_STEREO; s++) {
             _clientSamples[s] = glm::clamp(_clientSamples[s] + (int)(streamPopOutput[s / stereoDivider] * attenuationAndFade),
@@ -305,25 +308,19 @@ int AudioMixer::addStreamToMixForListeningNodeWithStream(PositionalAudioStream*
         }
     }
 
-    if (_enableFilter && shouldAttenuate) {
+    if (!sourceIsSelf && _enableFilter) {
 
-        glm::vec3 relativePosition = streamToAdd->getPosition() - listeningNodeStream->getPosition();
-        glm::quat inverseOrientation = glm::inverse(listeningNodeStream->getOrientation());
-        glm::vec3 rotatedSourcePosition = inverseOrientation * relativePosition;
-        
-        // project the rotated source position vector onto the XZ plane
-        rotatedSourcePosition.y = 0.0f;
-        
-        // produce an oriented angle about the y-axis
-        float bearingAngleToSource = glm::orientedAngle(glm::vec3(0.0f, 0.0f, -1.0f),
-                                                        glm::normalize(rotatedSourcePosition),
-                                                        glm::vec3(0.0f, -1.0f, 0.0f));
         const float TWO_OVER_PI = 2.0f / PI;
         
         const float ZERO_DB = 1.0f;
-        const float NEGATIVE_ONE_DB = 0.891f;
+//        const float NEGATIVE_ONE_DB = 0.891f;
         const float NEGATIVE_THREE_DB = 0.708f;
-
+        const float NEGATIVE_SIX_DB = 0.501f;
+        
+        const float FILTER_GAIN_AT_0 = ZERO_DB; // source is in front
+        const float FILTER_GAIN_AT_90 = NEGATIVE_SIX_DB; // source is incident to left or right ear
+        const float FILTER_GAIN_AT_180 = NEGATIVE_SIX_DB; // source is behind
+        
         const float FILTER_CUTOFF_FREQUENCY_HZ = 1000.0f;
         
         const float penumbraFilterFrequency = FILTER_CUTOFF_FREQUENCY_HZ; // constant frequency
@@ -332,43 +329,41 @@ int AudioMixer::addStreamToMixForListeningNodeWithStream(PositionalAudioStream*
         float penumbraFilterGainL;
         float penumbraFilterGainR;
 
-        // variable gain calculation broken down by quadrent
+        // variable gain calculation broken down by quadrant
-        if (bearingAngleToSource < -PI_OVER_TWO && bearingAngleToSource > -PI) {
+        if (-bearingRelativeAngleToSource < -PI_OVER_TWO && -bearingRelativeAngleToSource > -PI) {
-            // gainL(-pi/2,0b)->(-pi,-1db)
             penumbraFilterGainL = TWO_OVER_PI * 
-                (ZERO_DB - NEGATIVE_ONE_DB) * (bearingAngleToSource + PI_OVER_TWO) + ZERO_DB;
+                (FILTER_GAIN_AT_0 - FILTER_GAIN_AT_180) * (-bearingRelativeAngleToSource + PI_OVER_TWO) + FILTER_GAIN_AT_0;
-            // gainR(-pi/2,-3db)->(-pi,-1db)
             penumbraFilterGainR = TWO_OVER_PI * 
-                (NEGATIVE_THREE_DB - NEGATIVE_ONE_DB) * (bearingAngleToSource + PI_OVER_TWO) + NEGATIVE_THREE_DB;
+                (FILTER_GAIN_AT_90 - FILTER_GAIN_AT_180) * (-bearingRelativeAngleToSource + PI_OVER_TWO) + FILTER_GAIN_AT_90;
-        } else if (bearingAngleToSource <= PI && bearingAngleToSource > PI_OVER_TWO) {
+        } else if (-bearingRelativeAngleToSource <= PI && -bearingRelativeAngleToSource > PI_OVER_TWO) {
-            // gainL(+pi,-1db)->(pi/2,-3db)
             penumbraFilterGainL = TWO_OVER_PI * 
-                (NEGATIVE_ONE_DB - NEGATIVE_THREE_DB) * (bearingAngleToSource - PI) + NEGATIVE_ONE_DB;
+                (FILTER_GAIN_AT_180 - FILTER_GAIN_AT_90) * (-bearingRelativeAngleToSource - PI) + FILTER_GAIN_AT_180;
-            // gainR(+pi,-1db)->(pi/2,0db)
             penumbraFilterGainR = TWO_OVER_PI * 
-                (NEGATIVE_ONE_DB - ZERO_DB) * (bearingAngleToSource - PI) + NEGATIVE_ONE_DB;
+                (FILTER_GAIN_AT_180 - FILTER_GAIN_AT_0) * (-bearingRelativeAngleToSource - PI) + FILTER_GAIN_AT_180;
-        } else if (bearingAngleToSource <= PI_OVER_TWO && bearingAngleToSource > 0) {
+        } else if (-bearingRelativeAngleToSource <= PI_OVER_TWO && -bearingRelativeAngleToSource > 0) {
-            // gainL(+pi/2,-3db)->(0,0db)
             penumbraFilterGainL = TWO_OVER_PI *
-                (NEGATIVE_THREE_DB - ZERO_DB) * (bearingAngleToSource - PI_OVER_TWO) + NEGATIVE_THREE_DB;
+                (FILTER_GAIN_AT_90 - FILTER_GAIN_AT_0) * (-bearingRelativeAngleToSource - PI_OVER_TWO) + FILTER_GAIN_AT_90;
-            // gainR(+p1/2,0db)->(0,0db)
+            penumbraFilterGainR = FILTER_GAIN_AT_0;            
-            penumbraFilterGainR = ZERO_DB;            
         } else {
-            // gainL(0,0db)->(-pi/2,0db)
+            penumbraFilterGainL = FILTER_GAIN_AT_0;
-            penumbraFilterGainL = ZERO_DB;
-            // gainR(0,0db)->(-pi/2,-3db)
             penumbraFilterGainR =  TWO_OVER_PI * 
-                (ZERO_DB - NEGATIVE_THREE_DB) * (bearingAngleToSource) + ZERO_DB;
+                (FILTER_GAIN_AT_0 - FILTER_GAIN_AT_90) * (-bearingRelativeAngleToSource) + FILTER_GAIN_AT_0;
         }
+        
+        if (distanceBetween < RADIUS_OF_HEAD) {
+            // Diminish effect if source would be inside head
+            penumbraFilterGainL += (1.f - penumbraFilterGainL) * (1.f - distanceBetween / RADIUS_OF_HEAD);
+            penumbraFilterGainR += (1.f - penumbraFilterGainR) * (1.f - distanceBetween / RADIUS_OF_HEAD);
+        }
+
+
 #if 0
-            qDebug() << "avatar="
+            qDebug() << "gainL="
-                     << listeningNodeStream
-                     << "gainL=" 
                      << penumbraFilterGainL
                      << "gainR="
                      << penumbraFilterGainR
                      << "angle="
-                     << bearingAngleToSource;
+                     << -bearingRelativeAngleToSource;
 #endif
         
         // set the gain on both filter channels

domain-server/resources/web/settings/describe.json

@@ -67,7 +67,7 @@
 	    "type": "checkbox",
         "label": "Enable Positional Filter",
         "help": "If enabled, positional audio stream uses lowpass filter",
-        "default": false
+        "default": true
       }
     }
   }

examples/butterflies.js

@@ -13,6 +13,9 @@
 //
 
 
+var numButterflies = 20;
+
+
 function getRandomFloat(min, max) {
     return Math.random() * (max - min) + min;
 }
@@ -73,7 +76,6 @@ function defineButterfly(entityID, targetPosition) {
 
 // Array of butterflies
 var butterflies = [];
-var numButterflies = 20;
 function addButterfly() {
     // Decide the size of butterfly 
     var color = { red: 100, green: 100, blue: 100 };
@@ -133,7 +135,8 @@ function updateButterflies(deltaTime) {
         
         // Update all the butterflies
         for (var i = 0; i < numButterflies; i++) {
-            entityID = butterflies[i].entityID;
+            entityID = Entities.identifyEntity(butterflies[i].entityID);
+            butterflies[i].entityID = entityID;
             var properties = Entities.getEntityProperties(entityID);
 			
     		if (properties.position.y > flockPosition.y + getRandomFloat(0.0,0.3)){ //0.3  //ceiling

examples/loadScriptFromMessage.js

@@ -0,0 +1,27 @@
+//
+//  loadScriptFromMessage.js
+//  examples
+//
+//  Created by Thijs Wenker on 9/15/14.
+//  Copyright 2014 High Fidelity, Inc.
+//  
+//  Filters script links out of incomming messages and prompts you to run the script.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+//Javascript link RegEX
+const JS_LINK_REGEX = /https?:\/\/[^ ]+\.js/i;
+
+function onIncomingMessage(user, message) {
+    var script_link = JS_LINK_REGEX.exec(message);
+    if (script_link == null) {
+        return;
+    }
+    if (Window.confirm("@" + user + " sent the following script:\n" + script_link + "\nwould you like to run it?")) {
+        Script.load(script_link);
+    }
+}
+
+GlobalServices.incomingMessage.connect(onIncomingMessage);

examples/playSoundLoop.js

@@ -5,39 +5,51 @@
 //  Created by David Rowe on 5/29/14.
 //  Copyright 2014 High Fidelity, Inc.
 //
-//  This example script plays a sound in a continuous loop.
+//  This example script plays a sound in a continuous loop, and creates a red sphere in front of you at the origin of the sound.
 //
 //  Distributed under the Apache License, Version 2.0.
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
+
+//  A few sample files you may want to try: 
+
 var sound = new Sound("https://s3-us-west-1.amazonaws.com/highfidelity-public/sounds/Guitars/Guitar+-+Nylon+A.raw");
+//var sound = new Sound("https://s3-us-west-1.amazonaws.com/highfidelity-public/sounds/220Sine.wav");
+//var sound = new Sound("https://s3-us-west-1.amazonaws.com/highfidelity-public/sounds/Cocktail+Party+Snippets/Bandcamp.wav");
 
 var soundPlaying = false;
+var options = new AudioInjectionOptions();
+options.position = Vec3.sum(Camera.getPosition(), Quat.getFront(MyAvatar.orientation));
+options.volume = 0.5;
+options.loop = true;
+var playing = false;
+var ball = false;
 
-function keyPressEvent(event) {
+function maybePlaySound(deltaTime) {
-    if (event.text === "1") {
+    if (sound.downloaded) {
-        if (!Audio.isInjectorPlaying(soundPlaying)) {
+        var properties = {
-            var options = new AudioInjectionOptions();
+                type: "Sphere",
-            options.position = MyAvatar.position;
+                position: options.position, 
-            options.volume = 0.5;
+                 dimensions: { x: 0.2, y: 0.2, z: 0.2 },
-            options.loop = true;
+                color: { red: 200, green: 0, blue: 0 }    
-            soundPlaying = Audio.playSound(sound, options);
+            };
-            print("Started sound loop");
+        ball = Entities.addEntity(properties);
-        } else {
+        soundPlaying = Audio.playSound(sound, options);
-            Audio.stopInjector(soundPlaying);
+        print("Started sound looping.");
-            print("Stopped sound loop");
+        Script.update.disconnect(maybePlaySound);
-        }
     }
 }
-
+        
 function scriptEnding() {
     if (Audio.isInjectorPlaying(soundPlaying)) {
         Audio.stopInjector(soundPlaying);
-        print("Stopped sound loop");
+        Entities.deleteEntity(ball);
+        print("Stopped sound.");
     }
 }
 
 // Connect a call back that happens every frame
 Script.scriptEnding.connect(scriptEnding);
-Controller.keyPressEvent.connect(keyPressEvent);
+Script.update.connect(maybePlaySound);
+

examples/radio.js

@@ -0,0 +1,79 @@
+//
+//  Radio.js
+//  examples
+//
+//  Created by Clément Brisset on 8/20/14.
+//  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
+//
+
+var position = { x:1, y: 1, z: 10 };
+var rotation = Quat.fromPitchYawRollDegrees(0, 0, 0);
+var scale = 1.0;
+
+var modelURL = "https://s3-us-west-1.amazonaws.com/highfidelity-public/models/entities/radio/Speakers2Finished.fbx";
+var soundURL = "https://s3-us-west-1.amazonaws.com/highfidelity-public/sounds/FamilyStereo.raw";
+
+var AudioRotationOffset = Quat.fromPitchYawRollDegrees(0, -90, 0);
+var audioOptions = new AudioInjectionOptions();
+audioOptions.volume = 0.7;
+audioOptions.position = position;
+audioOptions.orientation = Quat.multiply(AudioRotationOffset, rotation);
+audioOptions.loop = true;
+audioOptions.isStereo = true;
+var injector = null;
+
+var sound = new Sound(soundURL);
+
+var entity = null;
+var properties = null;
+
+function update() {
+    if (entity === null) {
+        if (sound.downloaded) {
+            print("Sound file downloaded");
+            entity = Entities.addEntity({
+                                      type: "Model",
+                                      position: position,
+                                      rotation: rotation,
+                                      radius: scale / 2.0,
+                                      modelURL: modelURL
+                                      });
+            properties = Entities.getEntityProperties(entity);
+            
+            injector = Audio.playSound(sound, audioOptions);
+        }
+    } else {
+        var newProperties = Entities.getEntityProperties(entity);
+        if (newProperties.type === "Model") {
+            if (newProperties.position != properties.position) {
+                audioOptions.position = newProperties.position;
+            }
+            if (newProperties.orientation != properties.orientation) {
+                audioOptions.orientation = newProperties.orientation;
+            }
+            
+            properties = newProperties;
+        } else {
+            entity = null;
+            Script.update.disconnect(update);
+            Script.scriptEnding.connect(scriptEnding);
+            scriptEnding();
+        }
+    }
+}
+
+function scriptEnding() {
+    if (entity != null) {
+        Entities.deleteEntity(entity);
+    }
+    if (injector != null) {
+        injector.stop();
+    }
+}
+
+Script.update.connect(update);
+Script.scriptEnding.connect(scriptEnding);
+

interface/src/Audio.cpp

@@ -506,30 +506,34 @@ void Audio::handleAudioInput() {
 
     QByteArray inputByteArray = _inputDevice->readAll();
 
-    int16_t* inputFrameData = (int16_t*)inputByteArray.data();
+    if (!_muted && (_audioSourceInjectEnabled || _peqEnabled)) {
-    const int inputFrameCount = inputByteArray.size() / sizeof(int16_t);
+        
+        int16_t* inputFrameData = (int16_t*)inputByteArray.data();
+        const int inputFrameCount = inputByteArray.size() / sizeof(int16_t);
 
-    _inputFrameBuffer.copyFrames(1, inputFrameCount, inputFrameData, false /*copy in*/);
+        _inputFrameBuffer.copyFrames(1, inputFrameCount, inputFrameData, false /*copy in*/);
 
- //   _inputGain.render(_inputFrameBuffer);  // input/mic gain+mute
+#if ENABLE_INPUT_GAIN
-    
+        _inputGain.render(_inputFrameBuffer);  // input/mic gain+mute
-    //  Add audio source injection if enabled
+#endif
-    if (_audioSourceInjectEnabled && !_muted) {
+        //  Add audio source injection if enabled
-     
+        if (_audioSourceInjectEnabled) {
-        if (_toneSourceEnabled) {  // sine generator
+         
-            _toneSource.render(_inputFrameBuffer);
+            if (_toneSourceEnabled) {  // sine generator
+                _toneSource.render(_inputFrameBuffer);
+            }
+            else if(_noiseSourceEnabled) { // pink noise generator
+                _noiseSource.render(_inputFrameBuffer);
+            }
+            _sourceGain.render(_inputFrameBuffer); // post gain
         }
-        else if(_noiseSourceEnabled) { // pink noise generator
+        if (_peqEnabled) {
-            _noiseSource.render(_inputFrameBuffer);
+            _peq.render(_inputFrameBuffer); // 3-band parametric eq
         }
-        _sourceGain.render(_inputFrameBuffer); // post gain
-    }
-    if (_peqEnabled && !_muted) {
-        _peq.render(_inputFrameBuffer); // 3-band parametric eq
-    }
 
-    _inputFrameBuffer.copyFrames(1, inputFrameCount, inputFrameData, true /*copy out*/);
+        _inputFrameBuffer.copyFrames(1, inputFrameCount, inputFrameData, true /*copy out*/);
-    
+    }
+        
     if (Menu::getInstance()->isOptionChecked(MenuOption::EchoLocalAudio) && !_muted && _audioOutput) {
         // if this person wants local loopback add that to the locally injected audio
 

interface/src/DatagramProcessor.cpp

@@ -45,17 +45,31 @@ void DatagramProcessor::processDatagrams() {
         _byteCount += incomingPacket.size();
         
         if (nodeList->packetVersionAndHashMatch(incomingPacket)) {
+            
+            PacketType incomingType = packetTypeForPacket(incomingPacket);
             // only process this packet if we have a match on the packet version
-            switch (packetTypeForPacket(incomingPacket)) {
+            switch (incomingType) {
                 case PacketTypeMixedAudio:
                 case PacketTypeSilentAudioFrame:
-                    QMetaObject::invokeMethod(&application->_audio, "addReceivedAudioToStream", Qt::QueuedConnection,
+                case PacketTypeAudioStreamStats: {
-                                              Q_ARG(QByteArray, incomingPacket));
+                    if (incomingType != PacketTypeAudioStreamStats) {
-                    break;
+                        QMetaObject::invokeMethod(&application->_audio, "addReceivedAudioToStream", Qt::QueuedConnection,
-                case PacketTypeAudioStreamStats:
+                                                  Q_ARG(QByteArray, incomingPacket));
-                    QMetaObject::invokeMethod(&application->_audio, "parseAudioStreamStatsPacket", Qt::QueuedConnection,
+                    } else {
-                        Q_ARG(QByteArray, incomingPacket));
+                        QMetaObject::invokeMethod(&application->_audio, "parseAudioStreamStatsPacket", Qt::QueuedConnection,
+                                                  Q_ARG(QByteArray, incomingPacket));
+                    }
+                    
+                    // update having heard from the audio-mixer and record the bytes received
+                    SharedNodePointer audioMixer = nodeList->sendingNodeForPacket(incomingPacket);
+                    
+                    if (audioMixer) {
+                        audioMixer->setLastHeardMicrostamp(usecTimestampNow());
+                        audioMixer->recordBytesReceived(incomingPacket.size());
+                    }
+                    
                     break;
+                }
                 case PacketTypeParticleAddResponse:
                     // this will keep creatorTokenIDs to IDs mapped correctly
                     Particle::handleAddParticleResponse(incomingPacket);

interface/src/Menu.cpp

@@ -277,7 +277,8 @@ Menu::Menu() :
             avatar, SLOT(updateMotionBehaviorsFromMenu()));
 
     QMenu* collisionsMenu = avatarMenu->addMenu("Collide With...");
-    addCheckableActionToQMenuAndActionHash(collisionsMenu, MenuOption::CollideAsRagdoll);
+    addCheckableActionToQMenuAndActionHash(collisionsMenu, MenuOption::CollideAsRagdoll, 0, false, 
+            avatar, SLOT(onToggleRagdoll()));
     addCheckableActionToQMenuAndActionHash(collisionsMenu, MenuOption::CollideWithAvatars,
             0, true, avatar, SLOT(updateCollisionGroups()));
     addCheckableActionToQMenuAndActionHash(collisionsMenu, MenuOption::CollideWithVoxels,
@@ -744,6 +745,7 @@ void Menu::loadSettings(QSettings* settings) {
     // TODO: cache more settings in MyAvatar that are checked with very high frequency.
     MyAvatar* myAvatar = Application::getInstance()->getAvatar();
     myAvatar->updateCollisionGroups();
+    myAvatar->onToggleRagdoll();
 
     if (lockedSettings) {
         Application::getInstance()->unlockSettings();

interface/src/MetavoxelSystem.cpp

@@ -2319,9 +2319,9 @@ void StaticModelRenderer::renderUnclipped(float alpha, Mode mode) {
     _model->render(alpha);
 }
 
-bool StaticModelRenderer::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
+bool StaticModelRenderer::findRayIntersection(RayIntersectionInfo& intersection,
-        const glm::vec3& clipMinimum, float clipSize, float& distance) const {
+        const glm::vec3& clipMinimum, float clipSize) const {
-    return _model->findRayIntersection(origin, direction, distance);
+    return _model->findRayIntersection(intersection);
 }
 
 void StaticModelRenderer::applyTranslation(const glm::vec3& translation) {

interface/src/MetavoxelSystem.h

@@ -370,8 +370,8 @@ public:
     
     virtual void init(Spanner* spanner);
     virtual void simulate(float deltaTime);
-    virtual bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
+    virtual bool findRayIntersection(RayIntersectionInfo& intersection,
-        const glm::vec3& clipMinimum, float clipSize, float& distance) const;
+        const glm::vec3& clipMinimum, float clipSize) const;
 
 protected:
 

interface/src/ModelUploader.cpp

@@ -196,39 +196,56 @@ bool ModelUploader::zip() {
     // mixamo blendshapes
     if (!mapping.contains(BLENDSHAPE_FIELD) && geometry.applicationName == "mixamo.com") {
         QVariantHash blendshapes;
-        blendshapes.insert("EyeBlink_L", QVariantList() << "Blink_Left" << 1.0);
+        blendshapes.insertMulti("BrowsD_L", QVariantList() << "BrowsDown_Left" << 1.0);
-        blendshapes.insert("EyeBlink_R", QVariantList() << "Blink_Right" << 1.0);
+        blendshapes.insertMulti("BrowsD_R", QVariantList() << "BrowsDown_Right" << 1.0);
-        blendshapes.insert("EyeSquint_L", QVariantList() << "Squint_Left" << 1.0);
+        blendshapes.insertMulti("BrowsU_C", QVariantList() << "BrowsUp_Left" << 1.0);
-        blendshapes.insert("EyeSquint_R", QVariantList() << "Squint_Right" << 1.0);
+        blendshapes.insertMulti("BrowsU_C", QVariantList() << "BrowsUp_Right" << 1.0);
-        blendshapes.insert("EyeOpen_L", QVariantList() << "EyesWide_Left" << 1.0);
+        blendshapes.insertMulti("BrowsU_L", QVariantList() << "BrowsUp_Left" << 1.0);
-        blendshapes.insert("EyeOpen_R", QVariantList() << "EyesWide_Right" << 1.0);
+        blendshapes.insertMulti("BrowsU_R", QVariantList() << "BrowsUp_Right" << 1.0);
-        blendshapes.insert("BrowsD_L", QVariantList() << "BrowsDown_Left" << 1.0);
+        blendshapes.insertMulti("ChinLowerRaise", QVariantList() << "Jaw_Up" << 1.0);
-        blendshapes.insert("BrowsD_R", QVariantList() << "BrowsDown_Right" << 1.0);
+        blendshapes.insertMulti("ChinUpperRaise", QVariantList() << "UpperLipUp_Left" << 0.5);
-        blendshapes.insert("BrowsU_L", QVariantList() << "BrowsUp_Left" << 1.0);
+        blendshapes.insertMulti("ChinUpperRaise", QVariantList() << "UpperLipUp_Right" << 0.5);
-        blendshapes.insert("BrowsU_R", QVariantList() << "BrowsUp_Right" << 1.0);
+        blendshapes.insertMulti("EyeBlink_L", QVariantList() << "Blink_Left" << 1.0);
-        blendshapes.insert("JawFwd", QVariantList() << "JawForeward" << 1.0);
+        blendshapes.insertMulti("EyeBlink_R", QVariantList() << "Blink_Right" << 1.0);
-        blendshapes.insert("JawOpen", QVariantList() << "Jaw_Down" << 1.0);
+        blendshapes.insertMulti("EyeOpen_L", QVariantList() << "EyesWide_Left" << 1.0);
-        blendshapes.insert("JawLeft", QVariantList() << "Jaw_Left" << 1.0);
+        blendshapes.insertMulti("EyeOpen_R", QVariantList() << "EyesWide_Right" << 1.0);
-        blendshapes.insert("JawRight", QVariantList() << "Jaw_Right" << 1.0);
+        blendshapes.insertMulti("EyeSquint_L", QVariantList() << "Squint_Left" << 1.0);
-        blendshapes.insert("JawChew", QVariantList() << "Jaw_Up" << 1.0);
+        blendshapes.insertMulti("EyeSquint_R", QVariantList() << "Squint_Right" << 1.0);
-        blendshapes.insert("MouthLeft", QVariantList() << "Midmouth_Left" << 1.0);
+        blendshapes.insertMulti("JawFwd", QVariantList() << "JawForeward" << 1.0);
-        blendshapes.insert("MouthRight", QVariantList() << "Midmouth_Right" << 1.0);
+        blendshapes.insertMulti("JawLeft", QVariantList() << "JawRotateY_Left" << 0.5);
-        blendshapes.insert("MouthFrown_L", QVariantList() << "Frown_Left" << 1.0);
+        blendshapes.insertMulti("JawOpen", QVariantList() << "MouthOpen" << 0.7);
-        blendshapes.insert("MouthFrown_R", QVariantList() << "Frown_Right" << 1.0);
+        blendshapes.insertMulti("JawRight", QVariantList() << "Jaw_Right" << 1.0);
-        blendshapes.insert("MouthSmile_L", QVariantList() << "Smile_Left" << 1.0);
+        blendshapes.insertMulti("LipsFunnel", QVariantList() << "JawForeward" << 0.39);
-        blendshapes.insert("MouthSmile_R", QVariantList() << "Smile_Right" << 1.0);
+        blendshapes.insertMulti("LipsFunnel", QVariantList() << "Jaw_Down" << 0.36);
-        blendshapes.insert("LipsUpperUp", QVariantList() << "UpperLipUp_Left" << 0.5);
+        blendshapes.insertMulti("LipsFunnel", QVariantList() << "MouthNarrow_Left" << 1.0);
-        blendshapes.insertMulti("LipsUpperUp", QVariantList() << "UpperLipUp_Right" << 0.5);
+        blendshapes.insertMulti("LipsFunnel", QVariantList() << "MouthNarrow_Right" << 1.0);
-        blendshapes.insert("Puff", QVariantList() << "CheekPuff_Left" << 0.5);
+        blendshapes.insertMulti("LipsFunnel", QVariantList() << "MouthWhistle_NarrowAdjust_Left" << 0.5);
-        blendshapes.insertMulti("Puff", QVariantList() << "CheekPuff_Right" << 0.5);
+        blendshapes.insertMulti("LipsFunnel", QVariantList() << "MouthWhistle_NarrowAdjust_Right" << 0.5);
-        blendshapes.insert("Sneer", QVariantList() << "NoseScrunch_Left" << 0.5);
+        blendshapes.insertMulti("LipsFunnel", QVariantList() << "TongueUp" << 1.0);
-        blendshapes.insertMulti("Sneer", QVariantList() << "NoseScrunch_Right" << 0.5);
+        blendshapes.insertMulti("LipsLowerClose", QVariantList() << "LowerLipIn" << 1.0);
-        blendshapes.insert("CheekSquint_L", QVariantList() << "Squint_Left" << 1.0);
+        blendshapes.insertMulti("LipsLowerDown", QVariantList() << "LowerLipDown_Left" << 0.7);
-        blendshapes.insert("CheekSquint_R", QVariantList() << "Squint_Right" << 1.0);
+        blendshapes.insertMulti("LipsLowerDown", QVariantList() << "LowerLipDown_Right" << 0.7);
-        blendshapes.insert("LipsPucker", QVariantList() << "MouthNarrow_Left" << 0.5);
+        blendshapes.insertMulti("LipsLowerOpen", QVariantList() << "LowerLipOut" << 1.0);
-        blendshapes.insertMulti("LipsPucker", QVariantList() << "MouthNarrow_Right" << 0.5);
+        blendshapes.insertMulti("LipsPucker", QVariantList() << "MouthNarrow_Left" << 1.0);
-        blendshapes.insert("LipsLowerDown", QVariantList() << "LowerLipDown_Left" << 0.5);
+        blendshapes.insertMulti("LipsPucker", QVariantList() << "MouthNarrow_Right" << 1.0);
-        blendshapes.insertMulti("LipsLowerDown", QVariantList() << "LowerLipDown_Right" << 0.5);
+        blendshapes.insertMulti("LipsUpperClose", QVariantList() << "UpperLipIn" << 1.0);
+        blendshapes.insertMulti("LipsUpperOpen", QVariantList() << "UpperLipOut" << 1.0);
+        blendshapes.insertMulti("LipsUpperUp", QVariantList() << "UpperLipUp_Left" << 0.7);
+        blendshapes.insertMulti("LipsUpperUp", QVariantList() << "UpperLipUp_Right" << 0.7);
+        blendshapes.insertMulti("MouthDimple_L", QVariantList() << "Smile_Left" << 0.25);
+        blendshapes.insertMulti("MouthDimple_R", QVariantList() << "Smile_Right" << 0.25);
+        blendshapes.insertMulti("MouthFrown_L", QVariantList() << "Frown_Left" << 1.0);
+        blendshapes.insertMulti("MouthFrown_R", QVariantList() << "Frown_Right" << 1.0);
+        blendshapes.insertMulti("MouthLeft", QVariantList() << "Midmouth_Left" << 1.0);
+        blendshapes.insertMulti("MouthRight", QVariantList() << "Midmouth_Right" << 1.0);
+        blendshapes.insertMulti("MouthSmile_L", QVariantList() << "Smile_Left" << 1.0);
+        blendshapes.insertMulti("MouthSmile_R", QVariantList() << "Smile_Right" << 1.0);
+        blendshapes.insertMulti("Puff", QVariantList() << "CheekPuff_Left" << 1.0);
+        blendshapes.insertMulti("Puff", QVariantList() << "CheekPuff_Right" << 1.0);
+        blendshapes.insertMulti("Sneer", QVariantList() << "NoseScrunch_Left" << 0.75);
+        blendshapes.insertMulti("Sneer", QVariantList() << "NoseScrunch_Right" << 0.75);
+        blendshapes.insertMulti("Sneer", QVariantList() << "Squint_Left" << 0.5);
+        blendshapes.insertMulti("Sneer", QVariantList() << "Squint_Right" << 0.5);
         mapping.insert(BLENDSHAPE_FIELD, blendshapes);
     }
     

interface/src/avatar/Avatar.cpp

@@ -714,20 +714,10 @@ void Avatar::renderDisplayName() {
     glEnable(GL_LIGHTING);
 }
 
-bool Avatar::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const {
+bool Avatar::findRayIntersection(RayIntersectionInfo& intersection) const {
-    float minDistance = FLT_MAX;
+    bool hit = _skeletonModel.findRayIntersection(intersection);
-    float modelDistance;
+    hit = getHead()->getFaceModel().findRayIntersection(intersection) || hit;
-    if (_skeletonModel.findRayIntersection(origin, direction, modelDistance)) {
+    return hit;
-        minDistance = qMin(minDistance, modelDistance);
-    }
-    if (getHead()->getFaceModel().findRayIntersection(origin, direction, modelDistance)) {
-        minDistance = qMin(minDistance, modelDistance);
-    }
-    if (minDistance < FLT_MAX) {
-        distance = minDistance;
-        return true;
-    }
-    return false;
 }
 
 bool Avatar::findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius, CollisionList& collisions) {

interface/src/avatar/Avatar.h

@@ -99,7 +99,7 @@ public:
     /// Returns the distance to use as a LOD parameter.
     float getLODDistance() const;
 
-    bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const;
+    bool findRayIntersection(RayIntersectionInfo& intersection) const;
 
     /// \param shapes list of shapes to collide against avatar
     /// \param collisions list to store collision results

interface/src/avatar/MyAvatar.cpp

@@ -49,7 +49,7 @@ const float PITCH_SPEED = 100.0f; // degrees/sec
 const float COLLISION_RADIUS_SCALAR = 1.2f; // pertains to avatar-to-avatar collisions
 const float COLLISION_RADIUS_SCALE = 0.125f;
 
-const float MIN_KEYBOARD_CONTROL_SPEED = 2.0f;
+const float MIN_KEYBOARD_CONTROL_SPEED = 1.5f;
 const float MAX_WALKING_SPEED = 3.0f * MIN_KEYBOARD_CONTROL_SPEED;
 
 // TODO: normalize avatar speed for standard avatar size, then scale all motion logic 
@@ -75,7 +75,6 @@ MyAvatar::MyAvatar() :
     _motorTimescale(DEFAULT_MOTOR_TIMESCALE),
     _maxMotorSpeed(MAX_MOTOR_SPEED),
     _motionBehaviors(AVATAR_MOTION_DEFAULTS),
-    _lastFloorContactPoint(0.0f),
     _lookAtTargetAvatar(),
     _shouldRender(true),
     _billboardValid(false),
@@ -87,11 +86,10 @@ MyAvatar::MyAvatar() :
         _driveKeys[i] = 0.0f;
     }
     _physicsSimulation.setEntity(&_skeletonModel);
+    _physicsSimulation.addEntity(&_voxelShapeManager);
 
     _skeletonModel.setEnableShapes(true);
-    Ragdoll* ragdoll = _skeletonModel.buildRagdoll();
+    _skeletonModel.buildRagdoll();
-    _physicsSimulation.setRagdoll(ragdoll);
-    _physicsSimulation.addEntity(&_voxelShapeManager);
     
     // connect to AddressManager signal for location jumps
     connect(&AddressManager::getInstance(), &AddressManager::locationChangeRequired, this, &MyAvatar::goToLocation);
@@ -217,15 +215,15 @@ void MyAvatar::simulate(float deltaTime) {
     }
 
     {
-        PerformanceTimer perfTimer("ragdoll");
+        PerformanceTimer perfTimer("physics");
+        const float minError = 0.00001f;
+        const float maxIterations = 3;
+        const quint64 maxUsec = 4000;
+        _physicsSimulation.setTranslation(_position);
+        _physicsSimulation.stepForward(deltaTime, minError, maxIterations, maxUsec);
+
         Ragdoll* ragdoll = _skeletonModel.getRagdoll();
         if (ragdoll && Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
-            const float minError = 0.00001f;
-            const float maxIterations = 3;
-            const quint64 maxUsec = 4000;
-            _physicsSimulation.setTranslation(_position);
-            _physicsSimulation.stepForward(deltaTime, minError, maxIterations, maxUsec);
-
             // harvest any displacement of the Ragdoll that is a result of collisions
             glm::vec3 ragdollDisplacement = ragdoll->getAndClearAccumulatedMovement();
             const float MAX_RAGDOLL_DISPLACEMENT_2 = 1.0f;
@@ -1086,15 +1084,6 @@ bool MyAvatar::shouldRenderHead(const glm::vec3& cameraPosition, RenderMode rend
         (glm::length(cameraPosition - head->getEyePosition()) > RENDER_HEAD_CUTOFF_DISTANCE * _scale);
 }
 
-float MyAvatar::computeDistanceToFloor(const glm::vec3& startPoint) {
-    glm::vec3 direction = -_worldUpDirection;
-    OctreeElement* elementHit; // output from findRayIntersection
-    float distance = FLT_MAX; // output from findRayIntersection
-    BoxFace face; // output from findRayIntersection
-    Application::getInstance()->getVoxelTree()->findRayIntersection(startPoint, direction, elementHit, distance, face);
-    return distance;
-}
-
 void MyAvatar::updateOrientation(float deltaTime) {
     //  Gather rotation information from keyboard
     _bodyYawDelta -= _driveKeys[ROT_RIGHT] * YAW_SPEED * deltaTime;
@@ -1152,86 +1141,69 @@ void MyAvatar::updateOrientation(float deltaTime) {
 const float NEARBY_FLOOR_THRESHOLD = 5.0f;
 
 void MyAvatar::updatePosition(float deltaTime) {
-    float keyboardInput = fabsf(_driveKeys[FWD] - _driveKeys[BACK]) + 
-        fabsf(_driveKeys[RIGHT] - _driveKeys[LEFT]) + 
-        fabsf(_driveKeys[UP] - _driveKeys[DOWN]);
-
-    bool walkingOnFloor = false;
-    float gravityLength = glm::length(_gravity) * GRAVITY_EARTH;
 
+    // check for floor by casting a ray straight down from avatar's position
+    float heightAboveFloor = FLT_MAX;
     const CapsuleShape& boundingShape = _skeletonModel.getBoundingShape();
-    glm::vec3 startCap;
+    RayIntersectionInfo intersection;
-    boundingShape.getStartPoint(startCap);
+    // NOTE: avatar is center of PhysicsSimulation, so rayStart is the origin for the purposes of the raycast
-    glm::vec3 bottom = startCap - boundingShape.getRadius() * _worldUpDirection;
+    intersection._rayStart = glm::vec3(0.0f);
+    intersection._rayDirection = - _worldUpDirection;
+    intersection._rayLength = 5.0f * boundingShape.getBoundingRadius();
+    if (_physicsSimulation.findFloorRayIntersection(intersection)) {
+        // NOTE: heightAboveFloor is the distance between the bottom of the avatar and the floor
+        heightAboveFloor = intersection._hitDistance - boundingShape.getBoundingRadius();
+    }
 
-    // velocity is initialized to the measured _velocity but will be modified 
+    // velocity is initialized to the measured _velocity but will be modified by friction, external thrust, etc
-    // by friction, external thrust, etc
     glm::vec3 velocity = _velocity;
 
-    // apply friction
+    bool pushingUp = (_driveKeys[UP] - _driveKeys[DOWN] > 0.0f);
-    if (gravityLength > EPSILON) {
+    bool walkingOnFloor = false;
-        float speedFromGravity = _scale * deltaTime * gravityLength;
+    if (_motionBehaviors & AVATAR_MOTION_STAND_ON_NEARBY_FLOORS) {
-        float distanceToFall = glm::distance(bottom, _lastFloorContactPoint);
+        const float MAX_SPEED_UNDER_GRAVITY = 2.0f * _scale * MAX_WALKING_SPEED;
-        walkingOnFloor = (distanceToFall < 2.0f * deltaTime * speedFromGravity);
+        if (pushingUp || glm::length2(velocity) > MAX_SPEED_UNDER_GRAVITY * MAX_SPEED_UNDER_GRAVITY) {
-
+            // we're pushing up or moving quickly, so disable gravity
-        if (walkingOnFloor) {
+            setLocalGravity(glm::vec3(0.0f));
-            // BEGIN HACK: to prevent the avatar from bouncing on a floor surface
-            if (distanceToFall < deltaTime * speedFromGravity) {
-                float verticalSpeed = glm::dot(velocity, _worldUpDirection);
-                if (fabs(verticalSpeed) < speedFromGravity) {
-                    // we're standing on a floor, and nearly at rest so we zero the vertical velocity component
-                    velocity -= verticalSpeed * _worldUpDirection;
-                }
-            } else {
-                // fall with gravity against floor
-                velocity -= speedFromGravity * _worldUpDirection;
-            }
-            // END HACK
         } else {
-            if (!_isBraking) {
+            const float maxFloorDistance = boundingShape.getBoundingRadius() * NEARBY_FLOOR_THRESHOLD;
-                // fall with gravity toward floor
+            if (heightAboveFloor > maxFloorDistance) {
-                velocity -= speedFromGravity * _worldUpDirection;
+                // disable local gravity when floor is too far away
-            }
+                setLocalGravity(glm::vec3(0.0f));
-
+            } else {
-            if (_motionBehaviors & AVATAR_MOTION_STAND_ON_NEARBY_FLOORS) {
+                // enable gravity
-                const float MAX_VERTICAL_FLOOR_DETECTION_SPEED = _scale * MAX_WALKING_SPEED;
+                walkingOnFloor = true;
-                if (keyboardInput && glm::dot(_motorVelocity, _worldUpDirection) > 0.0f &&
-                        glm::dot(velocity, _worldUpDirection) > MAX_VERTICAL_FLOOR_DETECTION_SPEED) {
-                    // disable local gravity when flying up
-                    setLocalGravity(glm::vec3(0.0f));
-                } else {
-                    const float maxFloorDistance = _scale * NEARBY_FLOOR_THRESHOLD;
-                    if (computeDistanceToFloor(bottom) > maxFloorDistance) {
-                        // disable local gravity when floor is too far
-                        setLocalGravity(glm::vec3(0.0f));
-                    }
-                }
-            }
-        }
-    } else if ((_collisionGroups & COLLISION_GROUP_VOXELS) && 
-        _motionBehaviors & AVATAR_MOTION_STAND_ON_NEARBY_FLOORS) {
-        const float MIN_FLOOR_DETECTION_SPEED = _scale * 1.0f;
-        if (glm::length(_velocity) < MIN_FLOOR_DETECTION_SPEED ) {
-            // scan for floor under avatar
-            const float maxFloorDistance = _scale * NEARBY_FLOOR_THRESHOLD;
-            if (computeDistanceToFloor(bottom) < maxFloorDistance) {
-                // enable local gravity
                 setLocalGravity(-_worldUpDirection);
             }
         }
     }
 
-    float speed = glm::length(velocity);
+    bool zeroDownwardVelocity = false;
-    if (keyboardInput > 0.0f || speed > 0.0f || glm::length2(_thrust) > 0.0f || ! walkingOnFloor) {
+    bool gravityEnabled = (glm::length2(_gravity) > EPSILON);
-        // update motor
+    if (gravityEnabled) {
-        if (_motionBehaviors & AVATAR_MOTION_MOTOR_KEYBOARD_ENABLED) {
+        if (heightAboveFloor < 0.0f) {
-            // Increase motor velocity until its length is equal to _maxMotorSpeed.
+            // Gravity is in effect so we assume that the avatar is colliding against the world and we need 
-            glm::vec3 localVelocity = velocity;
+            // to lift avatar out of floor, but we don't want to do it too fast (keep it smooth).
-            if (_motionBehaviors & AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME) {
+            float distanceToLift = glm::min(-heightAboveFloor, MAX_WALKING_SPEED * deltaTime);
-                glm::quat orientation = getHead()->getCameraOrientation();
+
-                localVelocity = glm::inverse(orientation) * velocity;
+            // We don't use applyPositionDelta() for this lift distance because we don't want the avatar 
-            }
+            // to come flying out of the floor.  Instead we update position directly, and set a boolean
-        
+            // that will remind us later to zero any downward component of the velocity.
+            _position += (distanceToLift - EPSILON) * _worldUpDirection;
+            zeroDownwardVelocity = true;
+        }
+        velocity += (deltaTime * GRAVITY_EARTH) * _gravity;
+    }
+
+    float motorEfficiency = glm::clamp(deltaTime / computeMotorTimescale(velocity), 0.0f, 1.0f);
+
+    // compute targetVelocity
+    glm::vec3 targetVelocity(0.0f);
+    if (_motionBehaviors & AVATAR_MOTION_MOTOR_KEYBOARD_ENABLED) {
+        float keyboardInput = fabsf(_driveKeys[FWD] - _driveKeys[BACK]) + 
+            (fabsf(_driveKeys[RIGHT] - _driveKeys[LEFT])) + 
+            fabsf(_driveKeys[UP] - _driveKeys[DOWN]);
+        if (keyboardInput) {
             // Compute keyboard input
             glm::vec3 front = (_driveKeys[FWD] - _driveKeys[BACK]) * IDENTITY_FRONT;
             glm::vec3 right = (_driveKeys[RIGHT] - _driveKeys[LEFT]) * IDENTITY_RIGHT;
@@ -1243,76 +1215,69 @@ void MyAvatar::updatePosition(float deltaTime) {
             // Compute motor magnitude
             if (directionLength > EPSILON) {
                 direction /= directionLength;
-                // the finalMotorSpeed depends on whether we are walking or not
+
+                // Compute the target keyboard velocity (which ramps up slowly, and damps very quickly)
+                // the max magnitude of which depends on what we're doing:
                 float finalMaxMotorSpeed = walkingOnFloor ? _scale * MAX_WALKING_SPEED : _scale * _maxMotorSpeed;
-        
                 float motorLength = glm::length(_motorVelocity);
                 if (motorLength < _scale * MIN_KEYBOARD_CONTROL_SPEED) {
                     // an active keyboard motor should never be slower than this
                     _motorVelocity = _scale * MIN_KEYBOARD_CONTROL_SPEED * direction;
+                    motorEfficiency = 1.0f;
                 } else {
-                    float MOTOR_LENGTH_TIMESCALE = 1.5f;
+                    float MOTOR_LENGTH_TIMESCALE = 2.0f;
-                    float tau = glm::clamp(deltaTime / MOTOR_LENGTH_TIMESCALE, 0.0f, 1.0f);
+                    float INCREASE_FACTOR = 1.8f;
-                    float INCREASE_FACTOR = 2.0f;
+                    motorLength *= 1.0f + glm::clamp(deltaTime / MOTOR_LENGTH_TIMESCALE, 0.0f, 1.0f) * INCREASE_FACTOR;
-                    //_motorVelocity *= 1.0f + tau * INCREASE_FACTOR;
-                    motorLength *= 1.0f + tau * INCREASE_FACTOR;
                     if (motorLength > finalMaxMotorSpeed) {
                         motorLength = finalMaxMotorSpeed;
                     }
                     _motorVelocity = motorLength * direction;
                 }
                 _isPushing = true;
-            } else {
+            } 
-                // motor opposes motion (wants to be at rest)
+            targetVelocity = _motorVelocity;
-                _motorVelocity = - localVelocity;
+        } else {
-            }
+            _motorVelocity = glm::vec3(0.0f);
         }
+    }
+    targetVelocity = getHead()->getCameraOrientation() * targetVelocity;
 
-        // apply motor
+    glm::vec3 deltaVelocity = targetVelocity - velocity;
-        if (_motionBehaviors & AVATAR_MOTION_MOTOR_ENABLED) {
-            glm::vec3 targetVelocity = _motorVelocity;
-            if (_motionBehaviors & AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME) {
-                // rotate targetVelocity into world frame
-                glm::quat rotation = getHead()->getCameraOrientation();
-                targetVelocity = rotation * _motorVelocity;
-            }
-    
-            glm::vec3 deltaVelocity = targetVelocity - velocity;
-        
-            if (_motionBehaviors & AVATAR_MOTION_MOTOR_COLLISION_SURFACE_ONLY && glm::length2(_gravity) > EPSILON) {
-                // For now we subtract the component parallel to gravity but what we need to do is: 
-                // TODO: subtract the component perp to the local surface normal (motor only pushes in surface plane).
-                glm::vec3 gravityDirection = glm::normalize(_gravity);
-                glm::vec3 parallelDelta = glm::dot(deltaVelocity, gravityDirection) * gravityDirection;
-                if (glm::dot(targetVelocity, velocity) > 0.0f) {
-                    // remove parallel part from deltaVelocity
-                    deltaVelocity -= parallelDelta;
-                }
-            }
-    
-            // simple critical damping
-            float timescale = computeMotorTimescale(velocity);
-            float tau = glm::clamp(deltaTime / timescale, 0.0f, 1.0f);
-            velocity += tau * deltaVelocity;
-        }
 
-        // apply thrust
+    if (walkingOnFloor && !pushingUp) {
-        velocity += _thrust * deltaTime;
+        // remove vertical component of deltaVelocity
-        speed = glm::length(velocity);
+        deltaVelocity -= glm::dot(deltaVelocity, _worldUpDirection) * _worldUpDirection;
-        if (speed > MAX_AVATAR_SPEED) {
+    }
-            velocity *= MAX_AVATAR_SPEED / speed;
-            speed = MAX_AVATAR_SPEED;
-        }
-        _thrust = glm::vec3(0.0f);
 
-        // update position
+    // apply motor
-        const float MIN_AVATAR_SPEED = 0.075f;
+    velocity += motorEfficiency * deltaVelocity;
-        if (speed > MIN_AVATAR_SPEED) {
+
-            applyPositionDelta(deltaTime * velocity);
+    // apply thrust
+    velocity += _thrust * deltaTime;
+    _thrust = glm::vec3(0.0f);
+
+    // remove downward velocity so we don't push into floor
+    if (zeroDownwardVelocity) {
+        float verticalSpeed = glm::dot(velocity, _worldUpDirection);
+        if (verticalSpeed < 0.0f) {
+            velocity += verticalSpeed * _worldUpDirection;
         }
     }
 
-    // update moving flag based on speed
+    // cap avatar speed
+    float speed = glm::length(velocity);
+    if (speed > MAX_AVATAR_SPEED) {
+        velocity *= MAX_AVATAR_SPEED / speed;
+        speed = MAX_AVATAR_SPEED;
+    }
+
+    // update position
+    const float MIN_AVATAR_SPEED = 0.075f;
+    if (speed > MIN_AVATAR_SPEED) {
+        applyPositionDelta(deltaTime * velocity);
+    }
+
+    // update _moving flag based on speed
     const float MOVING_SPEED_THRESHOLD = 0.01f;
     _moving = speed > MOVING_SPEED_THRESHOLD;
 
@@ -1331,8 +1296,8 @@ float MyAvatar::computeMotorTimescale(const glm::vec3& velocity) {
     // (3) inactive --> long timescale (gentle friction for low speeds)
 
     float MIN_MOTOR_TIMESCALE = 0.125f;
-    float MAX_MOTOR_TIMESCALE = 0.5f;
+    float MAX_MOTOR_TIMESCALE = 0.4f;
-    float MIN_BRAKE_SPEED = 0.4f;
+    float MIN_BRAKE_SPEED = 0.3f;
 
     float timescale = MAX_MOTOR_TIMESCALE;
     bool isThrust = (glm::length2(_thrust) > EPSILON);
@@ -1369,18 +1334,23 @@ void MyAvatar::updateCollisionWithEnvironment(float deltaTime, float radius) {
 static CollisionList myCollisions(64);
 
 void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) {
-    if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
+
+    quint64 now = usecTimestampNow();
+    if (_voxelShapeManager.needsUpdate(now)) {
         // We use a multiple of the avatar's boundingRadius as the size of the cube of interest.
-        float cubeScale = 4.0f * getBoundingRadius();
+        float cubeScale = 6.0f * getBoundingRadius();
         glm::vec3 corner = getPosition() - glm::vec3(0.5f * cubeScale);
         AACube boundingCube(corner, cubeScale);
 
         // query the VoxelTree for cubes that touch avatar's boundingCube
         CubeList cubes;
         if (Application::getInstance()->getVoxelTree()->findContentInCube(boundingCube, cubes)) {
-            _voxelShapeManager.updateVoxels(cubes);
+            _voxelShapeManager.updateVoxels(now, cubes);
         }
-    } else {
+    }
+
+    // TODO: Andrew to do ground/walking detection in ragdoll mode
+    if (!Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
         const float MAX_VOXEL_COLLISION_SPEED = 100.0f;
         float speed = glm::length(_velocity);
         if (speed > MAX_VOXEL_COLLISION_SPEED) {
@@ -1390,15 +1360,18 @@ void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) {
         }
         bool isTrapped = false;
         myCollisions.clear();
-        const CapsuleShape& boundingShape = _skeletonModel.getBoundingShape();
+        // copy the boundingShape and tranform into physicsSimulation frame
-        if (Application::getInstance()->getVoxelTree()->findShapeCollisions(&boundingShape, myCollisions, Octree::TryLock)) {
+        CapsuleShape boundingShape = _skeletonModel.getBoundingShape();
+        boundingShape.setTranslation(boundingShape.getTranslation() - _position);
+
+        if (_physicsSimulation.getShapeCollisions(&boundingShape, myCollisions)) {
+            // we temporarily move b
             const float VOXEL_ELASTICITY = 0.0f;
             const float VOXEL_DAMPING = 0.0f;
-            float capsuleRadius = boundingShape.getRadius();
+            const float capsuleRadius = boundingShape.getRadius();
-            float capsuleHalfHeight = boundingShape.getHalfHeight();
+            const float capsuleHalfHeight = boundingShape.getHalfHeight();
             const float MAX_STEP_HEIGHT = capsuleRadius + capsuleHalfHeight;
             const float MIN_STEP_HEIGHT = 0.0f;
-            glm::vec3 footBase = boundingShape.getTranslation() - (capsuleRadius + capsuleHalfHeight) * _worldUpDirection;
             float highestStep = 0.0f;
             float lowestStep = MAX_STEP_HEIGHT;
             glm::vec3 floorPoint;
@@ -1407,43 +1380,51 @@ void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) {
     
             for (int i = 0; i < myCollisions.size(); ++i) {
                 CollisionInfo* collision = myCollisions[i];
-                glm::vec3 cubeCenter = collision->_vecData;
+
-                float cubeSide = collision->_floatData;
                 float verticalDepth = glm::dot(collision->_penetration, _worldUpDirection);
                 float horizontalDepth = glm::length(collision->_penetration - verticalDepth * _worldUpDirection);
                 const float MAX_TRAP_PERIOD = 0.125f;
                 if (horizontalDepth > capsuleRadius || fabsf(verticalDepth) > MAX_STEP_HEIGHT) {
                     isTrapped = true;
                     if (_trapDuration > MAX_TRAP_PERIOD) {
-                        float distance = glm::dot(boundingShape.getTranslation() - cubeCenter, _worldUpDirection);
+                        RayIntersectionInfo intersection;
-                        if (distance < 0.0f) {
+                        // we pick a rayStart that we expect to be inside the boundingShape (aka shapeA)
-                            distance = fabsf(distance) + 0.5f * cubeSide;
+                        intersection._rayStart = collision->_contactPoint - MAX_STEP_HEIGHT * glm::normalize(collision->_penetration);
+                        intersection._rayDirection = -_worldUpDirection;
+                        // cast the ray down against shapeA
+                        if (collision->_shapeA->findRayIntersection(intersection)) {
+                            float firstDepth = - intersection._hitDistance;
+                            // recycle intersection and cast again in up against shapeB
+                            intersection._rayDirection = _worldUpDirection;
+                            intersection._hitDistance = FLT_MAX;
+                            if (collision->_shapeB->findRayIntersection(intersection)) {
+                                // now we know how much we need to move UP to get out
+                                totalPenetration = addPenetrations(totalPenetration, 
+                                        (firstDepth + intersection._hitDistance) * _worldUpDirection);
+                            }
                         }
-                        distance += capsuleRadius + capsuleHalfHeight;
-                        totalPenetration = addPenetrations(totalPenetration, - distance * _worldUpDirection);
                         continue;
                     }
                 } else if (_trapDuration > MAX_TRAP_PERIOD) {
-                    // we're trapped, ignore this collision
+                    // we're trapped, ignore this shallow collision
                     continue;
                 }
                 totalPenetration = addPenetrations(totalPenetration, collision->_penetration);
+                
+                // some logic to help us walk up steps
                 if (glm::dot(collision->_penetration, _velocity) >= 0.0f) {
-                    glm::vec3 cubeTop = cubeCenter + (0.5f * cubeSide) * _worldUpDirection;
+                    float stepHeight = - glm::dot(_worldUpDirection, collision->_penetration);
-                    float stepHeight = glm::dot(_worldUpDirection, cubeTop - footBase);
                     if (stepHeight > highestStep) {
                         highestStep = stepHeight;
                         stepPenetration = collision->_penetration;
                     }
                     if (stepHeight < lowestStep) {
                         lowestStep = stepHeight;
-                        floorPoint = collision->_contactPoint - collision->_penetration;
+                        // remember that collision is in _physicsSimulation frame so we must add _position
+                        floorPoint = _position + collision->_contactPoint - collision->_penetration;
                     }
                 }
             }
-            if (lowestStep < MAX_STEP_HEIGHT) {
-                _lastFloorContactPoint = floorPoint;
-            }
     
             float penetrationLength = glm::length(totalPenetration);
             if (penetrationLength < EPSILON) {
@@ -1453,12 +1434,11 @@ void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) {
             float verticalPenetration = glm::dot(totalPenetration, _worldUpDirection);
             if (highestStep > MIN_STEP_HEIGHT && highestStep < MAX_STEP_HEIGHT && verticalPenetration <= 0.0f) {
                 // we're colliding against an edge
+
+                // rotate _motorVelocity into world frame
                 glm::vec3 targetVelocity = _motorVelocity;
-                if (_motionBehaviors & AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME) {
+                glm::quat rotation = getHead()->getCameraOrientation();
-                    // rotate _motorVelocity into world frame
+                targetVelocity = rotation * _motorVelocity;
-                    glm::quat rotation = getHead()->getCameraOrientation();
-                    targetVelocity = rotation * _motorVelocity;
-                }
                 if (_wasPushing && glm::dot(targetVelocity, totalPenetration) > EPSILON) {
                     // we're puhing into the edge, so we want to lift
     
@@ -1786,25 +1766,34 @@ void MyAvatar::resetSize() {
     qDebug("Reseted scale to %f", _targetScale);
 }
 
-void MyAvatar::goToLocation(const glm::vec3& newPosition, bool hasOrientation, const glm::vec3& newOrientation) {    
+void MyAvatar::goToLocation(const glm::vec3& newPosition,
-    glm::quat quatOrientation = getOrientation();
+                            bool hasOrientation, const glm::quat& newOrientation,
+                            bool shouldFaceLocation) {
     
     qDebug().nospace() << "MyAvatar goToLocation - moving to " << newPosition.x << ", "
         << newPosition.y << ", " << newPosition.z;
     
+    glm::vec3 shiftedPosition = newPosition;
+    
     if (hasOrientation) {
         qDebug().nospace() << "MyAvatar goToLocation - new orientation is "
-            << newOrientation.x << ", " << newOrientation.y << ", " << newOrientation.z;
+            << newOrientation.x << ", " << newOrientation.y << ", " << newOrientation.z << ", " << newOrientation.w;
         
         // orient the user to face the target
-        glm::quat quatOrientation = glm::quat(glm::radians(newOrientation))
+        glm::quat quatOrientation = newOrientation;
-            * glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f));
+        
+        if (shouldFaceLocation) {
+            
+            quatOrientation = newOrientation * glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f));
+            
+            // move the user a couple units away
+            const float DISTANCE_TO_USER = 2.0f;
+            shiftedPosition = newPosition - quatOrientation * IDENTITY_FRONT * DISTANCE_TO_USER;
+        }
+        
         setOrientation(quatOrientation);
     }
 
-    // move the user a couple units away
-    const float DISTANCE_TO_USER = 2.0f;
-    glm::vec3 shiftedPosition = newPosition - quatOrientation * IDENTITY_FRONT * DISTANCE_TO_USER;
     slamPosition(shiftedPosition);
     emit transformChanged();
 }
@@ -1834,6 +1823,17 @@ void MyAvatar::updateMotionBehaviorsFromMenu() {
     }
 }
 
+void MyAvatar::onToggleRagdoll() {
+    Ragdoll* ragdoll = _skeletonModel.getRagdoll();
+    if (ragdoll) {
+        if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
+            _physicsSimulation.setRagdoll(ragdoll);
+        } else {
+            _physicsSimulation.setRagdoll(NULL);
+        }
+    }
+}
+
 void MyAvatar::renderAttachments(RenderMode renderMode) {
     if (Application::getInstance()->getCamera()->getMode() != CAMERA_MODE_FIRST_PERSON || renderMode == MIRROR_RENDER_MODE) {
         Avatar::renderAttachments(renderMode);

interface/src/avatar/MyAvatar.h

@@ -154,7 +154,9 @@ public slots:
     void decreaseSize();
     void resetSize();
     
-    void goToLocation(const glm::vec3& newPosition, bool hasOrientation = false, const glm::vec3& newOrientation = glm::vec3());
+    void goToLocation(const glm::vec3& newPosition,
+                      bool hasOrientation = false, const glm::quat& newOrientation = glm::quat(),
+                      bool shouldFaceLocation = false);
 
     //  Set/Get update the thrust that will move the avatar around
     void addThrust(glm::vec3 newThrust) { _thrust += newThrust; };
@@ -164,6 +166,7 @@ public slots:
     void setVelocity(const glm::vec3 velocity) { _velocity = velocity; }
 
     void updateMotionBehaviorsFromMenu();
+    void onToggleRagdoll();
     
     glm::vec3 getLeftPalmPosition();
     glm::vec3 getRightPalmPosition();
@@ -206,7 +209,6 @@ private:
     float _maxMotorSpeed;
     quint32 _motionBehaviors;
 
-    glm::vec3 _lastFloorContactPoint;
     QWeakPointer<AvatarData> _lookAtTargetAvatar;
     glm::vec3 _targetAvatarPosition;
     bool _shouldRender;
@@ -220,7 +222,6 @@ private:
     RecorderPointer _recorder;
     
 	// private methods
-    float computeDistanceToFloor(const glm::vec3& startPoint);
     void updateOrientation(float deltaTime);
     void updatePosition(float deltaTime);
     float computeMotorTimescale(const glm::vec3& velocity);

interface/src/avatar/VoxelShapeManager.cpp

@@ -17,7 +17,7 @@
 
 #include "VoxelShapeManager.h"
 
-VoxelShapeManager::VoxelShapeManager() : PhysicsEntity(), _lastSimulationTranslation(0.0f) {
+VoxelShapeManager::VoxelShapeManager() : PhysicsEntity(), _updateExpiry(0), _lastSimulationTranslation(0.0f) {
 }
 
 VoxelShapeManager::~VoxelShapeManager() {
@@ -57,7 +57,9 @@ void VoxelShapeManager::clearShapes() {
     _voxels.clear();
 }
 
-void VoxelShapeManager::updateVoxels(CubeList& cubes) {
+void VoxelShapeManager::updateVoxels(const quint64& now, CubeList& cubes) {
+    const quint64 VOXEL_UPDATE_PERIOD = 100000; // usec
+    _updateExpiry = now + VOXEL_UPDATE_PERIOD;
     PhysicsSimulation* simulation = getSimulation();
     if (!simulation) {
         return;

interface/src/avatar/VoxelShapeManager.h

@@ -28,7 +28,7 @@ public:
     AACubeShape* _shape;
 };
 
-typedef QHash<quint64, VoxelInfo> VoxelPool;
+typedef QHash<uint, VoxelInfo> VoxelPool;
 
 class VoxelShapeManager : public PhysicsEntity {
 public:
@@ -39,11 +39,14 @@ public:
     void buildShapes();
     void clearShapes();
 
+    bool needsUpdate(const quint64& now) const { return _updateExpiry < now; }
+
     /// \param cubes list of AACubes representing all of the voxels that should be in this VoxelShapeManager
-    void updateVoxels(CubeList& cubes);
+    void updateVoxels(const quint64& now, CubeList& cubes);
 
 
 private:
+    quint64 _updateExpiry;
     glm::vec3 _lastSimulationTranslation;
     VoxelPool _voxels;
 };

libraries/audio/src/AudioInjectorOptions.h

@@ -24,9 +24,11 @@
 class AudioInjectorOptions : public QObject {
     Q_OBJECT
     
+    Q_PROPERTY(glm::quat orientation READ getOrientation WRITE setOrientation)
     Q_PROPERTY(glm::vec3 position READ getPosition WRITE setPosition)
     Q_PROPERTY(float volume READ getVolume WRITE setVolume)
     Q_PROPERTY(bool loop READ getLoop WRITE setLoop)
+    Q_PROPERTY(bool isStereo READ isStereo WRITE setIsStereo)
 public:
     AudioInjectorOptions(QObject* parent = 0);
     AudioInjectorOptions(const AudioInjectorOptions& other);

libraries/audio/src/AudioSourceTone.cpp

@@ -17,4 +17,43 @@
 #include "AudioBuffer.h"
 #include "AudioSourceTone.h"
 
-uint32_t AudioSourceTone::_frameOffset = 0;
+AudioSourceTone::AudioSourceTone() {
+    initialize();
+}
+
+AudioSourceTone::~AudioSourceTone() {
+    finalize();
+}
+
+void AudioSourceTone::finalize() {
+}
+
+void AudioSourceTone::reset() {
+}
+
+void AudioSourceTone::updateCoefficients() {
+    _omega = _frequency / _sampleRate * TWO_PI;
+    _epsilon = 2.0f * sinf(_omega / 2.0f);
+    _yq1 = cosf(-1.0f * _omega);
+    _y1 = sinf(+1.0f * _omega);   
+}
+
+void AudioSourceTone::initialize() {
+    const float32_t FREQUENCY_220_HZ = 220.0f;
+    const float32_t GAIN_MINUS_3DB = 0.708f;
+    setParameters(SAMPLE_RATE, FREQUENCY_220_HZ, GAIN_MINUS_3DB);
+}
+
+void AudioSourceTone::setParameters(const float32_t sampleRate, const float32_t frequency,  const float32_t amplitude) {
+    _sampleRate = std::max(sampleRate, 1.0f);
+    _frequency = std::max(frequency, 1.0f);
+    _amplitude = std::max(amplitude, 1.0f);
+    updateCoefficients();
+}
+
+void AudioSourceTone::getParameters(float32_t& sampleRate, float32_t& frequency, float32_t& amplitude) {
+    sampleRate = _sampleRate;
+    frequency = _frequency;
+    amplitude = _amplitude;
+}
+

libraries/audio/src/AudioSourceTone.h

@@ -12,61 +12,52 @@
 #ifndef hifi_AudioSourceTone_h
 #define hifi_AudioSourceTone_h
 
+// Implements a two-pole Gordon-Smith oscillator
 class AudioSourceTone
 {
-    static uint32_t _frameOffset;
     float32_t _frequency;
     float32_t _amplitude;
     float32_t _sampleRate;
     float32_t _omega;
+    float32_t _epsilon;
+    float32_t _yq1;  
+    float32_t _y1;
+    
+    void updateCoefficients();
     
 public:
-    AudioSourceTone() {
+    AudioSourceTone();
-        initialize();
+    ~AudioSourceTone();
-    }
     
-    ~AudioSourceTone() {
+    void initialize();
-        finalize();
+    void finalize();
-    }
+    void reset();
-    
-    void initialize() {
-        _frameOffset = 0;
-        setParameters(SAMPLE_RATE, 220.0f, 0.9f);
-    }
-    
-    void finalize() {
-    }
-    
-    void reset() {
-        _frameOffset = 0;
-    }
 
-    void setParameters(const float32_t sampleRate, const float32_t frequency,  const float32_t amplitude) {
+    void setParameters(const float32_t sampleRate, const float32_t frequency,  const float32_t amplitude);    
-        _sampleRate = std::max(sampleRate, 1.0f);
+    void getParameters(float32_t& sampleRate, float32_t& frequency, float32_t& amplitude);
-        _frequency = std::max(frequency, 1.0f);
-        _amplitude = std::max(amplitude, 1.0f);
-        _omega = _frequency / _sampleRate * TWO_PI;
-    }
     
-    void getParameters(float32_t& sampleRate, float32_t& frequency, float32_t& amplitude) {
+    void render(AudioBufferFloat32& frameBuffer);
-        sampleRate = _sampleRate;
-        frequency = _frequency;
-        amplitude = _amplitude;
-    }
-
-    void render(AudioBufferFloat32& frameBuffer) {
-        
-        // note: this is a placeholder implementation. final version will not include any transcendental ops in our render loop
-        
-        float32_t** samples = frameBuffer.getFrameData();
-        for (uint16_t i = 0; i < frameBuffer.getFrameCount(); ++i) {
-            for (uint16_t j = 0; j < frameBuffer.getChannelCount(); ++j) {
-                samples[j][i] = sinf((i + _frameOffset) * _omega);
-            }
-        }
-        _frameOffset += frameBuffer.getFrameCount();
-    }
 };
 
+
+inline void AudioSourceTone::render(AudioBufferFloat32& frameBuffer) {
+    float32_t** samples = frameBuffer.getFrameData();
+    float32_t yq;
+    float32_t y;
+    for (uint16_t i = 0; i < frameBuffer.getFrameCount(); ++i) {
+        
+        yq = _yq1 - (_epsilon * _y1);
+        y = _y1 + (_epsilon * yq);
+        
+        // update delays
+        _yq1 = yq;
+        _y1 = y;
+        
+        for (uint16_t j = 0; j < frameBuffer.getChannelCount(); ++j) {
+            samples[j][i] = _amplitude * y;
+        }
+    }
+}
+
 #endif 
 

libraries/avatars/src/AvatarData.h

@@ -55,20 +55,14 @@ typedef unsigned long long quint64;
 #include "HandData.h"
 
 // avatar motion behaviors
-const quint32 AVATAR_MOTION_MOTOR_ENABLED = 1U << 0;
+const quint32 AVATAR_MOTION_MOTOR_KEYBOARD_ENABLED = 1U << 0;
-const quint32 AVATAR_MOTION_MOTOR_KEYBOARD_ENABLED = 1U << 1;
-const quint32 AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME = 1U << 2;
-const quint32 AVATAR_MOTION_MOTOR_COLLISION_SURFACE_ONLY = 1U << 3;
 
-const quint32 AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY = 1U << 4;
+const quint32 AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY = 1U << 1;
-const quint32 AVATAR_MOTION_OBEY_LOCAL_GRAVITY = 1U << 5;
+const quint32 AVATAR_MOTION_OBEY_LOCAL_GRAVITY = 1U << 2;
-
+const quint32 AVATAR_MOTION_STAND_ON_NEARBY_FLOORS = 1U << 3;
-const quint32 AVATAR_MOTION_STAND_ON_NEARBY_FLOORS = 1U << 6;
 
 const quint32 AVATAR_MOTION_DEFAULTS = 
-        AVATAR_MOTION_MOTOR_ENABLED |
         AVATAR_MOTION_MOTOR_KEYBOARD_ENABLED |
-        AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME |
         AVATAR_MOTION_STAND_ON_NEARBY_FLOORS;
 
 // these bits will be expanded as features are exposed

libraries/entities/src/EntityItemProperties.cpp

@@ -592,9 +592,9 @@ void EntityItemProperties::markAllChanged() {
     _glowLevelChanged = true;
 }
 
-AACube EntityItemProperties::getMaximumAACubeInTreeUnits() const { 
+AACube EntityItemProperties::getMaximumAACubeInTreeUnits() const {
     AACube maxCube = getMaximumAACubeInMeters();
-    maxCube.scale(1 / (float)TREE_SCALE);
+    maxCube.scale(1.0f / (float)TREE_SCALE);
     return maxCube;
 }
 
@@ -611,7 +611,7 @@ AACube EntityItemProperties::getMaximumAACubeInMeters() const {
     glm::vec3 registrationPoint = (_dimensions * _registrationPoint);
     glm::vec3 registrationRemainder = (_dimensions * (glm::vec3(1.0f, 1.0f, 1.0f) - _registrationPoint));
     glm::vec3 furthestExtentFromRegistration = glm::max(registrationPoint, registrationRemainder);
-    
+
     // * we know that if you rotate in any direction you would create a sphere
     //   that has a radius of the length of furthest extent from registration point
     float radius = glm::length(furthestExtentFromRegistration);

libraries/entities/src/EntityItemPropertiesMacros.h

@@ -180,21 +180,25 @@
         }                                           \
     }
 
-#define COPY_PROPERTY_FROM_QSCRIPTVALUE_VEC3(P, S)      \
+#define COPY_PROPERTY_FROM_QSCRIPTVALUE_VEC3(P, S)        \
-    QScriptValue P = object.property(#P);               \
+    QScriptValue P = object.property(#P);                 \
-    if (P.isValid()) {                                  \
+    if (P.isValid()) {                                    \
-        QScriptValue x = P.property("x");               \
+        QScriptValue x = P.property("x");                 \
-        QScriptValue y = P.property("y");               \
+        QScriptValue y = P.property("y");                 \
-        QScriptValue z = P.property("z");               \
+        QScriptValue z = P.property("z");                 \
-        if (x.isValid() && y.isValid() && z.isValid()) {\
+        if (x.isValid() && y.isValid() && z.isValid()) {  \
-            glm::vec3 newValue;                         \
+            glm::vec3 newValue;                           \
-            newValue.x = x.toVariant().toFloat();       \
+            newValue.x = x.toVariant().toFloat();         \
-            newValue.y = y.toVariant().toFloat();       \
+            newValue.y = y.toVariant().toFloat();         \
-            newValue.z = z.toVariant().toFloat();       \
+            newValue.z = z.toVariant().toFloat();         \
-            if (_defaultSettings || newValue != _##P) { \
+            bool isValid = !glm::isnan(newValue.x) &&     \
-                S(newValue);                            \
+                         !glm::isnan(newValue.y) &&       \
-            }                                           \
+                         !glm::isnan(newValue.z);         \
-        }                                               \
+            if (isValid &&                                \
+                (_defaultSettings || newValue != _##P)) { \
+                S(newValue);                              \
+            }                                             \
+        }                                                 \
     }
     
 #define COPY_PROPERTY_FROM_QSCRIPTVALUE_QUAT(P, S)                      \
@@ -210,7 +214,12 @@
             newValue.y = y.toVariant().toFloat();                       \
             newValue.z = z.toVariant().toFloat();                       \
             newValue.w = w.toVariant().toFloat();                       \
-            if (_defaultSettings || newValue != _##P) {                 \
+            bool isValid = !glm::isnan(newValue.x) &&                   \
+                           !glm::isnan(newValue.y) &&                   \
+                           !glm::isnan(newValue.z) &&                   \
+                           !glm::isnan(newValue.w);                     \
+            if (isValid &&                                              \
+                (_defaultSettings || newValue != _##P)) {               \
                 S(newValue);                                            \
             }                                                           \
         }                                                               \

libraries/entities/src/EntityTree.cpp

@@ -501,7 +501,6 @@ int EntityTree::processEditPacketData(PacketType packetType, const unsigned char
             processedBytes = 0;
             break;
     }
-    
     return processedBytes;
 }
 

libraries/entities/src/MovingEntitiesOperator.cpp

@@ -60,8 +60,19 @@ void MovingEntitiesOperator::addEntityToMoveList(EntityItem* entity, const AACub
         qDebug() << "    newCube:" << newCube;
         qDebug() << "    oldCubeClamped:" << oldCubeClamped;
         qDebug() << "    newCubeClamped:" << newCubeClamped;
-        qDebug() << "    oldContainingElement:" << oldContainingElement->getAACube();
+        if (oldContainingElement) {
-        qDebug() << "    oldContainingElement->bestFitBounds(newCubeClamped):" << oldContainingElement->bestFitBounds(newCubeClamped);
+            qDebug() << "    oldContainingElement:" << oldContainingElement->getAACube();
+            qDebug() << "    oldContainingElement->bestFitBounds(newCubeClamped):" 
+                            << oldContainingElement->bestFitBounds(newCubeClamped);
+        } else {
+            qDebug() << "    WARNING NO OLD CONTAINING ELEMENT!!!";
+        }
+    }
+    
+    if (!oldContainingElement) {
+            qDebug() << "UNEXPECTED!!!! attempting to move entity "<< entity->getEntityItemID() 
+                            << "that has no containing element. ";
+        return; // bail without adding.
     }
 
     // If the original containing element is the best fit for the requested newCube locations then

libraries/entities/src/UpdateEntityOperator.cpp

@@ -36,6 +36,10 @@ UpdateEntityOperator::UpdateEntityOperator(EntityTree* tree,
 {
     // caller must have verified existence of containingElement and oldEntity
     assert(_containingElement && _existingEntity);
+    
+    if (_wantDebug) {
+        qDebug() << "UpdateEntityOperator::UpdateEntityOperator() -----------------------------";
+    }    
 
     // Here we have a choice to make, do we want to "tight fit" the actual minimum for the
     // entity into the the element, or do we want to use the entities "relaxed" bounds
@@ -50,10 +54,19 @@ UpdateEntityOperator::UpdateEntityOperator(EntityTree* tree,
     if (_properties.containsPositionChange() && !_properties.containsDimensionsChange()) {
         glm::vec3 oldDimensionsInMeters = _existingEntity->getDimensions() * (float)TREE_SCALE;
         _properties.setDimensions(oldDimensionsInMeters);
+
+        if (_wantDebug) {
+            qDebug() << "    ** setting properties dimensions - had position change, no dimension change **";
+        }
+
     }
     if (!_properties.containsPositionChange() && _properties.containsDimensionsChange()) {
         glm::vec3 oldPositionInMeters = _existingEntity->getPosition() * (float)TREE_SCALE;
         _properties.setPosition(oldPositionInMeters);
+
+        if (_wantDebug) {
+            qDebug() << "    ** setting properties position - had dimensions change, no position change **";
+        }
     }
 
     // If our new properties don't have bounds details (no change to position, etc) or if this containing element would 
@@ -64,6 +77,11 @@ UpdateEntityOperator::UpdateEntityOperator(EntityTree* tree,
     // if we don't have bounds properties, then use our old clamped box to determine best fit
     if (!_properties.containsBoundsProperties()) {
         oldElementBestFit = _containingElement->bestFitBounds(_oldEntityBox);
+
+        if (_wantDebug) {
+            qDebug() << "    ** old Element best fit - no dimensions change, no position change **";
+        }
+
     }
     
     // For some reason we've seen a case where the original containing element isn't a best fit for the old properties
@@ -71,20 +89,36 @@ UpdateEntityOperator::UpdateEntityOperator(EntityTree* tree,
     if (!_properties.containsBoundsProperties() && !oldElementBestFit) {
         _newEntityCube = _oldEntityCube;
         _removeOld = true; // our properties are going to move us, so remember this for later processing
+
+        if (_wantDebug) {
+            qDebug() << "    **** UNUSUAL CASE ****  no changes, but not best fit... consider it a move.... **";
+        }
+
+
     } else if (!_properties.containsBoundsProperties() || oldElementBestFit) {
         _foundOld = true;
         _newEntityCube = _oldEntityCube;
         _dontMove = true;
+
+        if (_wantDebug) {
+            qDebug() << "    **** TYPICAL NO MOVE CASE ****";
+            qDebug() << "        _properties.containsBoundsProperties():" << _properties.containsBoundsProperties();
+            qDebug() << "                             oldElementBestFit:" << oldElementBestFit;
+        }
+
     } else {
         _newEntityCube = _properties.getMaximumAACubeInTreeUnits();
         _removeOld = true; // our properties are going to move us, so remember this for later processing
+
+        if (_wantDebug) {
+            qDebug() << "    **** TYPICAL MOVE CASE ****";
+        }
     }
 
     _newEntityBox = _newEntityCube.clamp(0.0f, 1.0f); // clamp to domain bounds
 
 
     if (_wantDebug) {
-        qDebug() << "UpdateEntityOperator::UpdateEntityOperator() -----------------------------";
         qDebug() << "    _entityItemID:" << _entityItemID;
         qDebug() << "    _containingElementCube:" << _containingElementCube;
         qDebug() << "    _oldEntityCube:" << _oldEntityCube;
@@ -108,31 +142,30 @@ bool UpdateEntityOperator::subTreeContainsOldEntity(OctreeElement* element) {
     // so when we're searching the tree for the old element, we use the known cube for the known containing element
     bool elementContainsOldBox = element->getAACube().contains(_containingElementCube);
 
-    /*
+    if (_wantDebug) {
-    bool elementContainsOldCube = element->getAACube().contains(_oldEntityCube);
+        bool elementContainsOldCube = element->getAACube().contains(_oldEntityCube);
-    qDebug() << "UpdateEntityOperator::subTreeContainsOldEntity()....";
+        qDebug() << "UpdateEntityOperator::subTreeContainsOldEntity()....";
-    qDebug() << "    element->getAACube()=" << element->getAACube();
+        qDebug() << "    element->getAACube()=" << element->getAACube();
-    qDebug() << "    _oldEntityCube=" << _oldEntityCube;
+        qDebug() << "    _oldEntityCube=" << _oldEntityCube;
-    qDebug() << "    _oldEntityBox=" << _oldEntityBox;
+        qDebug() << "    _oldEntityBox=" << _oldEntityBox;
-    qDebug() << "    elementContainsOldCube=" << elementContainsOldCube;
+        qDebug() << "    elementContainsOldCube=" << elementContainsOldCube;
-    qDebug() << "    elementContainsOldBox=" << elementContainsOldBox;
+        qDebug() << "    elementContainsOldBox=" << elementContainsOldBox;
-    */
+    }
-
     return elementContainsOldBox;
 }
 
 bool UpdateEntityOperator::subTreeContainsNewEntity(OctreeElement* element) {
     bool elementContainsNewBox = element->getAACube().contains(_newEntityBox);
 
-    /*
+    if (_wantDebug) {
-    bool elementContainsNewCube = element->getAACube().contains(_newEntityCube);
+        bool elementContainsNewCube = element->getAACube().contains(_newEntityCube);
-    qDebug() << "UpdateEntityOperator::subTreeContainsNewEntity()....";
+        qDebug() << "UpdateEntityOperator::subTreeContainsNewEntity()....";
-    qDebug() << "    element->getAACube()=" << element->getAACube();
+        qDebug() << "    element->getAACube()=" << element->getAACube();
-    qDebug() << "    _newEntityCube=" << _newEntityCube;
+        qDebug() << "    _newEntityCube=" << _newEntityCube;
-    qDebug() << "    _newEntityBox=" << _newEntityBox;
+        qDebug() << "    _newEntityBox=" << _newEntityBox;
-    qDebug() << "    elementContainsNewCube=" << elementContainsNewCube;
+        qDebug() << "    elementContainsNewCube=" << elementContainsNewCube;
-    qDebug() << "    elementContainsNewBox=" << elementContainsNewBox;
+        qDebug() << "    elementContainsNewBox=" << elementContainsNewBox;
-    */
+    }
 
     return elementContainsNewBox;
 }
@@ -155,18 +188,42 @@ bool UpdateEntityOperator::preRecursion(OctreeElement* element) {
     bool subtreeContainsOld = subTreeContainsOldEntity(element);
     bool subtreeContainsNew = subTreeContainsNewEntity(element);
 
+    if (_wantDebug) {
+        qDebug() << "---- UpdateEntityOperator::preRecursion().... ----";
+        qDebug() << "    element=" << element->getAACube();
+        qDebug() << "    subtreeContainsOld=" << subtreeContainsOld;
+        qDebug() << "    subtreeContainsNew=" << subtreeContainsNew;
+        qDebug() << "    _foundOld=" << _foundOld;
+        qDebug() << "    _foundNew=" << _foundNew;
+    }
+
     // If we haven't yet found the old entity, and this subTreeContains our old
     // entity, then we need to keep searching.
     if (!_foundOld && subtreeContainsOld) {
-    
+
+        if (_wantDebug) {
+            qDebug() << "    OLD TREE CASE....";
+            qDebug() << "    entityTreeElement=" << entityTreeElement;
+            qDebug() << "    _containingElement=" << _containingElement;
+        }
+
         // If this is the element we're looking for, then ask it to remove the old entity
         // and we can stop searching.
         if (entityTreeElement == _containingElement) {
 
+            if (_wantDebug) {
+                qDebug() << "    *** it's the OLD ELEMENT! ***";
+            }
+
             // If the containgElement IS NOT the best fit for the new entity properties
             // then we need to remove it, and the updateEntity below will store it in the
             // correct element.
             if (_removeOld) {
+
+                if (_wantDebug) {
+                    qDebug() << "    *** REMOVING from ELEMENT ***";
+                }
+
                 entityTreeElement->removeEntityItem(_existingEntity); // NOTE: only removes the entity, doesn't delete it
 
                 // If we haven't yet found the new location, then we need to 
@@ -174,6 +231,11 @@ bool UpdateEntityOperator::preRecursion(OctreeElement* element) {
                 // now we're not in that map
                 if (!_foundNew) {
                     _tree->setContainingElement(_entityItemID, NULL);
+
+                    if (_wantDebug) {
+                        qDebug() << "    *** REMOVING from MAP ***";
+                    }
+
                 }
             }
             _foundOld = true;
@@ -187,11 +249,27 @@ bool UpdateEntityOperator::preRecursion(OctreeElement* element) {
     // entity, then we need to keep searching.
     if (!_foundNew && subtreeContainsNew) {
 
+        if (_wantDebug) {
+            qDebug() << "    NEW TREE CASE....";
+            qDebug() << "    entityTreeElement=" << entityTreeElement;
+            qDebug() << "    _containingElement=" << _containingElement;
+            qDebug() << "    entityTreeElement->bestFitBounds(_newEntityBox)=" << entityTreeElement->bestFitBounds(_newEntityBox);
+        }
+
+
         // If this element is the best fit for the new entity properties, then add/or update it
         if (entityTreeElement->bestFitBounds(_newEntityBox)) {
 
+            if (_wantDebug) {
+                qDebug() << "    *** THIS ELEMENT IS BEST FIT ***";
+            }
+
             // if we are the existing containing element, then we can just do the update of the entity properties
             if (entityTreeElement == _containingElement) {
+
+                if (_wantDebug) {
+                    qDebug() << "    *** This is the same OLD ELEMENT ***";
+                }
             
                 // TODO: We shouldn't be in a remove old case and also be the new best fit. This indicates that
                 // we have some kind of a logic error in this operator. But, it can handle it properly by setting
@@ -201,21 +279,43 @@ bool UpdateEntityOperator::preRecursion(OctreeElement* element) {
                     qDebug() << "UNEXPECTED - UpdateEntityOperator - "
                                 "we thought we needed to removeOld, but the old entity is our best fit.";
                     _removeOld = false;
+                    
+                    // if we thought we were supposed to remove the old item, and we already did, then we need
+                    // to repair this case.
+                    if (_foundOld) {
+                        if (_wantDebug) {
+                            qDebug() << "    *** REPAIRING PREVIOUS REMOVAL from ELEMENT and MAP ***";
+                        }
+                        entityTreeElement->addEntityItem(_existingEntity);
+                        _tree->setContainingElement(_entityItemID, entityTreeElement);
+                    }
                 }
 
                 // set the entity properties and mark our element as changed.
                 _existingEntity->setProperties(_properties);
+                if (_wantDebug) {
+                    qDebug() << "    *** set properties ***";
+                }
             } else {
                 // otherwise, this is an add case.
                 entityTreeElement->addEntityItem(_existingEntity);
                 _existingEntity->setProperties(_properties); // still need to update the properties!
                 _tree->setContainingElement(_entityItemID, entityTreeElement);
+                if (_wantDebug) {
+                    qDebug() << "    *** ADDING ENTITY to ELEMENT and MAP and SETTING PROPERTIES ***";
+                }
             }
             _foundNew = true; // we found the new item!
         } else {
             keepSearching = true;
         }
     }
+
+    if (_wantDebug) {
+        qDebug() << "    FINAL --- keepSearching=" << keepSearching;
+        qDebug() << "--------------------------------------------------";
+    }
+
     
     return keepSearching; // if we haven't yet found it, keep looking
 }

libraries/networking/src/AddressManager.cpp

@@ -28,7 +28,7 @@ QString AddressManager::pathForPositionAndOrientation(const glm::vec3& position,
     QString pathString = "/" + createByteArray(position);
     
     if (hasOrientation) {
-        QString orientationString = createByteArray(glm::degrees(safeEulerAngles(orientation)));
+        QString orientationString = createByteArray(orientation);
         pathString += "/" + orientationString;
     }
     
@@ -61,25 +61,25 @@ bool AddressManager::handleUrl(const QUrl& lookupUrl) {
         // 3. location string (posX,posY,posZ/eulerX,eulerY,eulerZ)
         // 4. domain network address (IP or dns resolvable hostname)
         
-        if (lookupUrl.isRelative()) {
+        // use our regex'ed helpers to figure out what we're supposed to do with this
-            // if this is a relative path then handle it as a relative viewpoint
+        if (!handleUsername(lookupUrl.authority())) {
-            handleRelativeViewpoint(lookupUrl.path());
+            // we're assuming this is either a network address or global place name
-        } else {
+            // check if it is a network address first
-            // use our regex'ed helpers to figure out what we're supposed to do with this
+            if (!handleNetworkAddress(lookupUrl.host())) {
-            if (!handleUsername(lookupUrl.authority())) {
+                // wasn't an address - lookup the place name
-                // we're assuming this is either a network address or global place name
+                attemptPlaceNameLookup(lookupUrl.host());
-                // check if it is a network address first
-                if (!handleNetworkAddress(lookupUrl.host())) {
-                    // wasn't an address - lookup the place name
-                    attemptPlaceNameLookup(lookupUrl.host());
-                }
-                
-                // we may have a path that defines a relative viewpoint - if so we should jump to that now
-                handleRelativeViewpoint(lookupUrl.path());
             }
+            
+            // we may have a path that defines a relative viewpoint - if so we should jump to that now
+            handleRelativeViewpoint(lookupUrl.path());
         }
         
         return true;
+    } else if (lookupUrl.toString().startsWith('/')) {
+        qDebug() << "Going to relative path" << lookupUrl.path();
+        
+        // if this is a relative path then handle it as a relative viewpoint
+        handleRelativeViewpoint(lookupUrl.path());
     }
     
     return false;
@@ -89,10 +89,18 @@ void AddressManager::handleLookupString(const QString& lookupString) {
     if (!lookupString.isEmpty()) {
         // make this a valid hifi URL and handle it off to handleUrl
         QString sanitizedString = lookupString;
-        const QRegExp HIFI_SCHEME_REGEX = QRegExp(HIFI_URL_SCHEME + ":\\/{1,2}", Qt::CaseInsensitive);
+        QUrl lookupURL;
-        sanitizedString = sanitizedString.remove(HIFI_SCHEME_REGEX);
         
-        handleUrl(QUrl(HIFI_URL_SCHEME + "://" + sanitizedString));
+        if (!lookupString.startsWith('/')) {
+            const QRegExp HIFI_SCHEME_REGEX = QRegExp(HIFI_URL_SCHEME + ":\\/{1,2}", Qt::CaseInsensitive);
+            sanitizedString = sanitizedString.remove(HIFI_SCHEME_REGEX);
+            
+            lookupURL = QUrl(HIFI_URL_SCHEME + "://" + sanitizedString);
+        } else {
+            lookupURL = QUrl(lookupString);
+        }
+        
+        handleUrl(lookupURL);
     }
 }
 
@@ -124,9 +132,11 @@ void AddressManager::handleAPIResponse(const QJsonObject &jsonObject) {
                 returnedPath = domainObject[LOCATION_KEY].toObject()[LOCATION_PATH_KEY].toString();
             }
             
+            bool shouldFaceViewpoint = dataObject.contains(ADDRESS_API_ONLINE_KEY);
+            
             if (!returnedPath.isEmpty()) {
                 // try to parse this returned path as a viewpoint, that's the only thing it could be for now
-                if (!handleRelativeViewpoint(returnedPath)) {
+                if (!handleRelativeViewpoint(returnedPath, shouldFaceViewpoint)) {
                     qDebug() << "Received a location path that was could not be handled as a viewpoint -" << returnedPath;
                 }
             }
@@ -183,38 +193,47 @@ bool AddressManager::handleNetworkAddress(const QString& lookupString) {
     return false;
 }
 
-bool AddressManager::handleRelativeViewpoint(const QString& lookupString) {
+bool AddressManager::handleRelativeViewpoint(const QString& lookupString, bool shouldFace) {
     const QString FLOAT_REGEX_STRING = "([-+]?[0-9]*\\.?[0-9]+(?:[eE][-+]?[0-9]+)?)";
-    const QString TRIPLE_FLOAT_REGEX_STRING = QString("\\/") + FLOAT_REGEX_STRING + "\\s*,\\s*" +
+    const QString SPACED_COMMA_REGEX_STRING = "\\s*,\\s*";
-    FLOAT_REGEX_STRING + "\\s*,\\s*" + FLOAT_REGEX_STRING + "\\s*(?:$|\\/)";
+    const QString POSITION_REGEX_STRING = QString("\\/") + FLOAT_REGEX_STRING + SPACED_COMMA_REGEX_STRING +
+        FLOAT_REGEX_STRING + SPACED_COMMA_REGEX_STRING + FLOAT_REGEX_STRING + "\\s*(?:$|\\/)";
+    const QString QUAT_REGEX_STRING = QString("\\/") + FLOAT_REGEX_STRING + SPACED_COMMA_REGEX_STRING +
+        FLOAT_REGEX_STRING + SPACED_COMMA_REGEX_STRING + FLOAT_REGEX_STRING + SPACED_COMMA_REGEX_STRING +
+        FLOAT_REGEX_STRING + "\\s*$";
     
-    QRegExp tripleFloatRegex(TRIPLE_FLOAT_REGEX_STRING);
+    QRegExp positionRegex(POSITION_REGEX_STRING);
     
-    if (tripleFloatRegex.indexIn(lookupString) != -1) {
+    if (positionRegex.indexIn(lookupString) != -1) {
         // we have at least a position, so emit our signal to say we need to change position
-        glm::vec3 newPosition(tripleFloatRegex.cap(1).toFloat(),
+        glm::vec3 newPosition(positionRegex.cap(1).toFloat(),
-                              tripleFloatRegex.cap(2).toFloat(),
+                              positionRegex.cap(2).toFloat(),
-                              tripleFloatRegex.cap(3).toFloat());
+                              positionRegex.cap(3).toFloat());
         
         if (!isNaN(newPosition.x) && !isNaN(newPosition.y) && !isNaN(newPosition.z)) {
-            glm::vec3 newOrientation;
+            glm::quat newOrientation;
+            
+            QRegExp orientationRegex(QUAT_REGEX_STRING);
+            
             // we may also have an orientation
-            if (lookupString[tripleFloatRegex.matchedLength() - 1] == QChar('/')
+            if (lookupString[positionRegex.matchedLength() - 1] == QChar('/')
-                && tripleFloatRegex.indexIn(lookupString, tripleFloatRegex.matchedLength() - 1) != -1) {
+                && orientationRegex.indexIn(lookupString, positionRegex.matchedLength() - 1) != -1) {
                 
-                glm::vec3 newOrientation(tripleFloatRegex.cap(1).toFloat(),
+                glm::quat newOrientation = glm::normalize(glm::quat(orientationRegex.cap(4).toFloat(),
-                                         tripleFloatRegex.cap(2).toFloat(),
+                                                                    orientationRegex.cap(1).toFloat(),
-                                         tripleFloatRegex.cap(3).toFloat());
+                                                                    orientationRegex.cap(2).toFloat(),
+                                                                    orientationRegex.cap(3).toFloat()));
                 
-                if (!isNaN(newOrientation.x) && !isNaN(newOrientation.y) && !isNaN(newOrientation.z)) {
+                if (!isNaN(newOrientation.x) && !isNaN(newOrientation.y) && !isNaN(newOrientation.z)
-                    emit locationChangeRequired(newPosition, true, newOrientation);
+                    && !isNaN(newOrientation.w)) {
+                    emit locationChangeRequired(newPosition, true, newOrientation, shouldFace);
                     return true;
                 } else {
                     qDebug() << "Orientation parsed from lookup string is invalid. Will not use for location change.";
                 }
             }
             
-            emit locationChangeRequired(newPosition, false, newOrientation);
+            emit locationChangeRequired(newPosition, false, newOrientation, shouldFace);
             
         } else {
             qDebug() << "Could not jump to position from lookup string because it has an invalid value.";

libraries/networking/src/AddressManager.h

@@ -40,14 +40,16 @@ signals:
     void lookupResultIsOffline();
     void lookupResultIsNotFound();
     void possibleDomainChangeRequired(const QString& newHostname);
-    void locationChangeRequired(const glm::vec3& newPosition, bool hasOrientationChange, const glm::vec3& newOrientation);
+    void locationChangeRequired(const glm::vec3& newPosition,
+                                bool hasOrientationChange, const glm::quat& newOrientation,
+                                bool shouldFaceLocation);
 private:
     const JSONCallbackParameters& apiCallbackParameters();
     
     bool handleUrl(const QUrl& lookupUrl);
     
     bool handleNetworkAddress(const QString& lookupString);
-    bool handleRelativeViewpoint(const QString& pathSubsection);
+    bool handleRelativeViewpoint(const QString& pathSubsection, bool shouldFace = false);
     bool handleUsername(const QString& lookupString);
 };
 

libraries/octree/src/Octree.cpp

@@ -851,7 +851,7 @@ bool findShapeCollisionsOp(OctreeElement* element, void* extraData) {
     return false;
 }
 
-quint64 cubeListHashKey(const glm::vec3& point) {
+uint qHash(const glm::vec3& point) {
     // NOTE: TREE_SCALE = 16384 (15 bits) and multiplier is 1024 (11 bits), 
     // so each component (26 bits) uses more than its alloted 21 bits.
     // however we don't expect to span huge cubes so it is ok if we wrap
@@ -859,9 +859,9 @@ quint64 cubeListHashKey(const glm::vec3& point) {
     const uint BITS_PER_COMPONENT = 21;
     const quint64 MAX_SCALED_COMPONENT = 2097152; // 2^21
     const float RESOLUTION_PER_METER = 1024.0f; // 2^10
-    return (quint64)(point.x * RESOLUTION_PER_METER) % MAX_SCALED_COMPONENT + 
+    return qHash((quint64)(point.x * RESOLUTION_PER_METER) % MAX_SCALED_COMPONENT + 
         (((quint64)(point.y * RESOLUTION_PER_METER)) % MAX_SCALED_COMPONENT << BITS_PER_COMPONENT) +
-        (((quint64)(point.z * RESOLUTION_PER_METER)) % MAX_SCALED_COMPONENT << 2 * BITS_PER_COMPONENT);
+        (((quint64)(point.z * RESOLUTION_PER_METER)) % MAX_SCALED_COMPONENT << 2 * BITS_PER_COMPONENT));
 }
 
 bool findContentInCubeOp(OctreeElement* element, void* extraData) {
@@ -877,8 +877,9 @@ bool findContentInCubeOp(OctreeElement* element, void* extraData) {
         return true; // recurse on children
     }
     if (element->hasContent()) {
-        // NOTE: the voxel's center is unique so we use it as the input for the key
+        // NOTE: the voxel's center is unique so we use it as the input for the key.
-        args->cubes->insert(cubeListHashKey(cube.calcCenter()), cube);
+        // We use the qHash(glm::vec()) as the key as an optimization for the code that uses CubeLists.
+        args->cubes->insert(qHash(cube.calcCenter()), cube);
         return true;
     }
     return false;

libraries/octree/src/Octree.h

@@ -48,7 +48,7 @@ public:
 // Callback function, for recuseTreeWithOperation
 typedef bool (*RecurseOctreeOperation)(OctreeElement* element, void* extraData);
 typedef enum {GRADIENT, RANDOM, NATURAL} creationMode;
-typedef QHash<quint64, AACube> CubeList;
+typedef QHash<uint, AACube> CubeList;
 
 const bool NO_EXISTS_BITS         = false;
 const bool WANT_EXISTS_BITS       = true;

libraries/shared/src/AACubeShape.cpp

@@ -9,8 +9,68 @@
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
-#include "AACubeShape.h"
+#include <glm/glm.hpp>
+#include <glm/gtx/norm.hpp>
 
-bool AACubeShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const {
+#include "AACubeShape.h"
-    return false;
+#include "SharedUtil.h" // for SQUARE_ROOT_OF_3 
+
+glm::vec3 faceNormals[3] = { glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 1.0f) };
+
+bool AACubeShape::findRayIntersection(RayIntersectionInfo& intersection) const {
+    // A = ray point
+    // B = cube center
+    glm::vec3 BA = _translation - intersection._rayStart;
+
+    // check for ray intersection with cube's bounding sphere
+    // a = distance along line to closest approach to B
+    float a = glm::dot(intersection._rayDirection, BA);
+    // b2 = squared distance from cube center to point of closest approach
+    float b2 = glm::length2(a * intersection._rayDirection - BA);
+    // r = bounding radius of cube
+    float halfSide = 0.5f * _scale;
+    const float r = SQUARE_ROOT_OF_3 * halfSide;
+    if (b2 > r * r) {
+        // line doesn't hit cube's bounding sphere
+        return false;
+    }
+
+    // check for tuncated/short ray
+    // maxLength = maximum possible distance between rayStart and center of cube
+    const float maxLength = glm::min(intersection._rayLength, intersection._hitDistance) + r;
+    if (a * a + b2 > maxLength * maxLength) {
+        // ray is not long enough to reach cube's bounding sphere
+        // NOTE: we don't fall in here when ray's length if FLT_MAX because maxLength^2 will be inf or nan
+        return false;
+    }
+
+    // the trivial checks have been exhausted, so must trace to each face
+    bool hit = false;
+    for (int i = 0; i < 3; ++i) {
+        for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
+            glm::vec3 faceNormal = sign * faceNormals[i];
+            float rayDotPlane = glm::dot(intersection._rayDirection, faceNormal);
+            if (glm::abs(rayDotPlane) > EPSILON) {
+                float distanceToFace = (halfSide + glm::dot(BA, faceNormal)) / rayDotPlane;
+                if (distanceToFace >= 0.0f) {
+                    glm::vec3 point = distanceToFace * intersection._rayDirection - BA;
+                    int j = (i + 1) % 3;
+                    int k = (i + 2) % 3;
+                    glm::vec3 secondNormal = faceNormals[j];
+                    glm::vec3 thirdNormal = faceNormals[k];
+                    if (glm::abs(glm::dot(point, secondNormal)) > halfSide ||
+                            glm::abs(glm::dot(point, thirdNormal)) > halfSide) {
+                        continue;
+                    }
+                    if (distanceToFace < intersection._hitDistance && distanceToFace < intersection._rayLength) {
+                        intersection._hitDistance = distanceToFace;
+                        intersection._hitNormal = faceNormal;
+                        intersection._hitShape = const_cast<AACubeShape*>(this);
+                        hit = true;
+                    }
+                }
+            }
+        }
+    }
+    return hit;
 }

libraries/shared/src/AACubeShape.h

@@ -25,7 +25,7 @@ public:
     float getScale() const { return _scale; }
     void setScale(float scale) { _scale = scale; }
 
-    bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const;
+    bool findRayIntersection(RayIntersectionInfo& intersection) const;
 
     float getVolume() const { return _scale * _scale * _scale; }
 

libraries/shared/src/CapsuleShape.cpp

@@ -78,13 +78,135 @@ void CapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& en
     updateBoundingRadius();
 }
 
-bool CapsuleShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const {
+// helper
-        glm::vec3 capsuleStart, capsuleEnd;
+bool findRayIntersectionWithCap(const glm::vec3& sphereCenter, float sphereRadius, 
-        getStartPoint(capsuleStart);
+        const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) {
-        getEndPoint(capsuleEnd);
+    float r2 = sphereRadius * sphereRadius;
-        // NOTE: findRayCapsuleIntersection returns 'true' with distance = 0 when rayStart is inside capsule.
+
-        // TODO: implement the raycast to return inside surface intersection for the internal rayStart.
+    // compute closest approach (CA)
-        return findRayCapsuleIntersection(rayStart, rayDirection, capsuleStart, capsuleEnd, _radius, distance);
+    float a = glm::dot(sphereCenter - intersection._rayStart, intersection._rayDirection); // a = distance from ray-start to CA
+    float b2 = glm::distance2(sphereCenter, intersection._rayStart + a * intersection._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 intersection._rayDirection
+    float d2 = glm::distance2(intersection._rayStart, sphereCenter); // d2 = squared distance from sphere-center to ray-start
+    float distance = FLT_MAX;
+    if (a < 0.0f) {
+        // ray points away from sphere-center
+        if (d2 > r2) {
+            // ray starts outside sphere
+            return false;
+        }
+        // ray starts inside sphere
+        distance = c + a;
+    } else if (d2 > r2) {
+        // ray starts outside sphere
+        distance = a - c;
+    } else {
+        // ray starts inside sphere
+        distance = a + c;
+    }
+    if (distance > 0.0f && distance < intersection._rayLength && distance < intersection._hitDistance) {
+        glm::vec3 sphereCenterToHitPoint = intersection._rayStart + distance * intersection._rayDirection - sphereCenter;
+        if (glm::dot(sphereCenterToHitPoint, sphereCenter - capsuleCenter) >= 0.0f) {
+            intersection._hitDistance = distance;
+            intersection._hitNormal = glm::normalize(sphereCenterToHitPoint);
+            return true;
+        }
+    } 
+    return false;
+}
+
+bool CapsuleShape::findRayIntersectionWithCaps(const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) const {
+    glm::vec3 capCenter;
+    getStartPoint(capCenter);
+    bool hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection);
+    getEndPoint(capCenter);
+    hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection) || hit;
+    if (hit) {
+        intersection._hitShape = const_cast<CapsuleShape*>(this);
+    }
+    return hit;
+}
+
+bool CapsuleShape::findRayIntersection(RayIntersectionInfo& intersection) const {
+    // ray is U, capsule is V
+    glm::vec3 axisV;
+    computeNormalizedAxis(axisV);
+    glm::vec3 centerV = getTranslation();
+
+    // first handle parallel case
+    float uDotV = glm::dot(axisV, intersection._rayDirection);
+    glm::vec3 UV = intersection._rayStart - centerV;
+    if (glm::abs(1.0f - glm::abs(uDotV)) < EPSILON) {
+        // line and cylinder are parallel
+        float distanceV = glm::dot(UV, intersection._rayDirection);
+        if (glm::length2(UV - distanceV * intersection._rayDirection) <= _radius * _radius) {
+            // ray is inside cylinder's radius and might intersect caps
+            return findRayIntersectionWithCaps(centerV, intersection);
+        }
+        return false;
+    }
+    
+    // Given a line with point 'U' and normalized direction 'u' and 
+    // a cylinder with axial point 'V', radius 'r', and normalized direction 'v'
+    // the intersection of the two is on the line at distance 't' from 'U'.
+    //
+    // Determining the values of t reduces to solving a quadratic equation: At^2 + Bt + C = 0 
+    //
+    // where:
+    //
+    // UV = U-V
+    // w = u-(u.v)v
+    // Q = UV-(UV.v)v
+    //       
+    // A = w^2
+    // B = 2(w.Q)
+    // C = Q^2 - r^2
+
+    glm::vec3 w = intersection._rayDirection - uDotV * axisV;
+    glm::vec3 Q = UV - glm::dot(UV, axisV) * axisV;
+
+    // we save a few multiplies by storing 2*A rather than just A
+    float A2 = 2.0f * glm::dot(w, w);
+    float B = 2.0f * glm::dot(w, Q);
+
+    // since C is only ever used once (in the determinant) we compute it inline
+    float determinant = B * B - 2.0f * A2 * (glm::dot(Q, Q) - _radius * _radius);
+    if (determinant < 0.0f) {
+        return false;
+    }
+    float hitLow  = (-B - sqrtf(determinant)) / A2;
+    float hitHigh = -(hitLow + 2.0f * B / A2);
+
+    if (hitLow > hitHigh) {
+        // re-arrange so hitLow is always the smaller value
+        float temp = hitHigh;
+        hitHigh = hitLow;
+        hitLow = temp;
+    }
+    if (hitLow < 0.0f) {
+        if (hitHigh < 0.0f) {
+            // capsule is completely behind rayStart
+            return false;
+        }
+        hitLow = hitHigh;
+    }
+
+    glm::vec3 p = intersection._rayStart + hitLow * intersection._rayDirection;
+    float d = glm::dot(p - centerV, axisV);
+    if (glm::abs(d) <= getHalfHeight()) {
+        // we definitely hit the cylinder wall
+        intersection._hitDistance = hitLow;
+        intersection._hitNormal = glm::normalize(p - centerV - d * axisV);
+        intersection._hitShape = const_cast<CapsuleShape*>(this);
+        return true;
+    }
+
+    // ray still might hit the caps
+    return findRayIntersectionWithCaps(centerV, intersection);
 }
 
 // static

libraries/shared/src/CapsuleShape.h

@@ -47,11 +47,12 @@ public:
     /// 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;
+    bool findRayIntersection(RayIntersectionInfo& intersection) const;
 
     virtual float getVolume() const { return (PI * _radius * _radius) * (1.3333333333f * _radius + getHalfHeight()); }
 
 protected:
+    bool findRayIntersectionWithCaps(const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) const;
     virtual void updateBoundingRadius() { _boundingRadius = _radius + getHalfHeight(); }
     static glm::quat computeNewRotation(const glm::vec3& newAxis);
 

libraries/shared/src/GLMHelpers.cpp

@@ -284,6 +284,11 @@ QByteArray createByteArray(const glm::vec3& vector) {
     return QByteArray::number(vector.x) + ',' + QByteArray::number(vector.y) + ',' + QByteArray::number(vector.z);
 }
 
+QByteArray createByteArray(const glm::quat& quat) {
+    return QByteArray::number(quat.x) + ',' + QByteArray::number(quat.y) + "," + QByteArray::number(quat.z) + ","
+        + QByteArray::number(quat.w);
+}
+
 bool isSimilarOrientation(const glm::quat& orientionA, const glm::quat& orientionB, float similarEnough) {
     // Compute the angular distance between the two orientations
     float angleOrientation = orientionA == orientionB ? 0.0f : glm::degrees(glm::angle(orientionA * glm::inverse(orientionB)));

libraries/shared/src/GLMHelpers.h

@@ -77,6 +77,7 @@ float extractUniformScale(const glm::mat4& matrix);
 float extractUniformScale(const glm::vec3& scale);
 
 QByteArray createByteArray(const glm::vec3& vector);
+QByteArray createByteArray(const glm::quat& quat);
 
 /// \return bool are two orientations similar to each other
 const float ORIENTATION_SIMILAR_ENOUGH = 5.0f; // 10 degrees in any direction

libraries/shared/src/PhysicsEntity.cpp

@@ -76,23 +76,8 @@ void PhysicsEntity::clearShapes() {
     _shapes.clear();
 }
 
-bool PhysicsEntity::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const {
+bool PhysicsEntity::findRayIntersection(RayIntersectionInfo& intersection) const {
-    int numShapes = _shapes.size();
+    return ShapeCollider::findRayIntersection(_shapes, intersection);
-    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<const Shape*> shapes, CollisionList& collisions) {

libraries/shared/src/PhysicsEntity.h

@@ -19,6 +19,7 @@
 #include <glm/gtc/quaternion.hpp>
 
 #include "CollisionInfo.h"
+#include "RayIntersectionInfo.h"
 
 class Shape;
 class PhysicsSimulation;
@@ -52,7 +53,7 @@ public:
 
     PhysicsSimulation* getSimulation() const { return _simulation; }
 
-    bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const;
+    bool findRayIntersection(RayIntersectionInfo& intersection) const;
     bool findCollisions(const QVector<const Shape*> shapes, CollisionList& collisions);
     bool findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadius, CollisionList& collisions);
     bool findPlaneCollisions(const glm::vec4& plane, CollisionList& collisions);

libraries/shared/src/PhysicsSimulation.cpp

@@ -207,9 +207,6 @@ void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
 
 void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) {
     ++_frameCount;
-    if (!_ragdoll) {
-        return;
-    }
     quint64 now = usecTimestampNow();
     quint64 startTime = now;
     quint64 expiry = startTime + maxUsec;
@@ -219,7 +216,9 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
     int numDolls = _otherRagdolls.size();
     {
         PerformanceTimer perfTimer("enforce");
-        _ragdoll->enforceConstraints();
+        if (_ragdoll) {
+            _ragdoll->enforceConstraints();
+        }
         for (int i = 0; i < numDolls; ++i) {
             _otherRagdolls[i]->enforceConstraints();
         }
@@ -235,7 +234,9 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
 
         { // enforce constraints
             PerformanceTimer perfTimer("enforce");
-            error = _ragdoll->enforceConstraints();
+            if (_ragdoll) {
+                error = _ragdoll->enforceConstraints();
+            }
             for (int i = 0; i < numDolls; ++i) {
                 error = glm::max(error, _otherRagdolls[i]->enforceConstraints());
             }
@@ -246,9 +247,12 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
         now = usecTimestampNow();
     } while (_collisions.size() != 0 && (iterations < maxIterations) && (error > minError) && (now < expiry));
 
-    // the collisions may have moved the main ragdoll from the simulation center
+    if (_ragdoll) {
-    // so we remove this offset (potentially storing it as movement of the Ragdoll owner)
+        // This is why _ragdoll is special and is not in the list of other ragdolls:
-    _ragdoll->removeRootOffset(collidedWithOtherRagdoll);
+        // The collisions may have moved the main ragdoll from the simulation center
+        // so we remove this offset (potentially storing it as movement of the Ragdoll owner)
+        _ragdoll->removeRootOffset(collidedWithOtherRagdoll);
+    }
 
     // also remove any offsets from the other ragdolls
     for (int i = 0; i < numDolls; ++i) {
@@ -257,13 +261,41 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
     pruneContacts();
 }
 
+bool PhysicsSimulation::findFloorRayIntersection(RayIntersectionInfo& intersection) const {
+    // only casts against otherEntities
+    bool hit = false;
+    int numEntities = _otherEntities.size();
+    for (int i = 0; i < numEntities; ++i) {
+        const QVector<Shape*> otherShapes = _otherEntities.at(i)->getShapes();
+        if (ShapeCollider::findRayIntersection(otherShapes, intersection)) {
+            hit = true;
+        }
+    }
+    return hit;
+}
+
+
+bool PhysicsSimulation::getShapeCollisions(const Shape* shape, CollisionList& collisions) const {
+    bool hit = false;
+    int numEntities = _otherEntities.size();
+    for (int i = 0; i < numEntities; ++i) {
+        const QVector<Shape*> otherShapes = _otherEntities.at(i)->getShapes();
+        if (ShapeCollider::collideShapeWithShapes(shape, otherShapes, 0, collisions)) {
+            hit = true;
+        }
+    }
+    return hit;
+}
+
 void PhysicsSimulation::integrate(float deltaTime) {
     PerformanceTimer perfTimer("integrate");
     int numEntities = _otherEntities.size();
     for (int i = 0; i < numEntities; ++i) {
         _otherEntities[i]->stepForward(deltaTime);
     }
-    _ragdoll->stepForward(deltaTime);
+    if (_ragdoll) {
+        _ragdoll->stepForward(deltaTime);
+    }
     int numDolls = _otherRagdolls.size();
     for (int i = 0; i < numDolls; ++i) {
         _otherRagdolls[i]->stepForward(deltaTime);

libraries/shared/src/PhysicsSimulation.h

@@ -9,8 +9,8 @@
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
-#ifndef hifi_PhysicsSimulation
+#ifndef hifi_PhysicsSimulation_h
-#define hifi_PhysicsSimulation
+#define hifi_PhysicsSimulation_h
 
 #include <QtGlobal>
 #include <QMap>
@@ -18,6 +18,7 @@
 
 #include "CollisionInfo.h"
 #include "ContactPoint.h"
+#include "RayIntersectionInfo.h"
 
 class PhysicsEntity;
 class Ragdoll;
@@ -54,6 +55,12 @@ public:
     /// \return distance of largest movement
     void stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec);
 
+    /// \param intersection collision info about ray hit
+    /// \return true if ray hits any shape that doesn't belong to the main ragdoll/entity
+    bool findFloorRayIntersection(RayIntersectionInfo& hit) const;
+
+    bool getShapeCollisions(const Shape* shape, CollisionList& collisions) const;
+
 protected:
     void integrate(float deltaTime);
 
@@ -80,4 +87,4 @@ private:
     QMap<quint64, ContactPoint> _contacts;
 };
 
-#endif // hifi_PhysicsSimulation
+#endif // hifi_PhysicsSimulation_h

libraries/shared/src/PlaneShape.cpp

@@ -11,6 +11,7 @@
 
 #include "PlaneShape.h"
 #include "SharedUtil.h"
+#include "GLMHelpers.h"
 
 const glm::vec3 UNROTATED_NORMAL(0.0f, 1.0f, 0.0f);
 
@@ -34,22 +35,42 @@ glm::vec3 PlaneShape::getNormal() const {
     return _rotation * UNROTATED_NORMAL;
 }
 
+void PlaneShape::setNormal(const glm::vec3& direction) {
+    glm::vec3 oldTranslation = _translation;
+    _rotation = rotationBetween(UNROTATED_NORMAL, direction);
+    glm::vec3 normal = getNormal();
+    _translation = glm::dot(oldTranslation, normal) * normal;
+}
+
+void PlaneShape::setPoint(const glm::vec3& point) {
+    glm::vec3 normal = getNormal();
+    _translation = glm::dot(point, normal) * normal;
+}
+
 glm::vec4 PlaneShape::getCoefficients() const {
     glm::vec3 normal = _rotation * UNROTATED_NORMAL;
     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 {
+bool PlaneShape::findRayIntersection(RayIntersectionInfo& intersection) const {
     glm::vec3 n = getNormal();
-    float denominator = glm::dot(n, rayDirection);
+    float denominator = glm::dot(n, intersection._rayDirection);
     if (fabsf(denominator) < EPSILON) {
         // line is parallel to plane
-        return glm::dot(_translation - rayStart, n) < EPSILON;
+        if (glm::dot(_translation - intersection._rayStart, n) < EPSILON) {
+            // ray starts on the plane
+            intersection._hitDistance = 0.0f;
+            intersection._hitNormal = n;
+            intersection._hitShape = const_cast<PlaneShape*>(this);
+            return true;
+        }
     } else {
-        float d = glm::dot(_translation - rayStart, n) / denominator;
+        float d = glm::dot(_translation - intersection._rayStart, n) / denominator;
-        if (d > 0.0f) {
+        if (d > 0.0f && d < intersection._rayLength && d < intersection._hitDistance) {
             // ray points toward plane
-            distance = d;
+            intersection._hitDistance = d;
+            intersection._hitNormal = n;
+            intersection._hitShape = const_cast<PlaneShape*>(this);
             return true;
         }
     }

libraries/shared/src/PlaneShape.h

@@ -21,7 +21,10 @@ public:
     glm::vec3 getNormal() const;
     glm::vec4 getCoefficients() const;
 
-    bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const;
+    void setNormal(const glm::vec3& normal);
+    void setPoint(const glm::vec3& point);
+
+    bool findRayIntersection(RayIntersectionInfo& intersection) const;
 };
 
 #endif // hifi_PlaneShape_h

libraries/shared/src/RayIntersectionInfo.h

@@ -0,0 +1,37 @@
+//
+//  RayIntersectionInfo.h
+//  interface/src/avatar
+//
+//  Created by Andrew Meadows 2014.09.09
+//  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_RayIntersectionInfo_h
+#define hifi_RayIntersectionInfo_h
+
+#include <glm/glm.hpp>
+
+class Shape;
+
+class RayIntersectionInfo {
+public:
+    RayIntersectionInfo() : _rayStart(0.0f), _rayDirection(1.0f, 0.0f, 0.0f), _rayLength(FLT_MAX), 
+            _hitDistance(FLT_MAX), _hitNormal(1.0f, 0.0f, 0.0f), _hitShape(NULL) { }
+
+    glm::vec3 getIntersectionPoint() const { return _rayStart + _hitDistance * _rayDirection; }
+
+    // input
+    glm::vec3 _rayStart;
+    glm::vec3 _rayDirection;
+    float _rayLength;
+
+    // output
+    float _hitDistance;
+    glm::vec3 _hitNormal;
+    Shape* _hitShape;
+};
+
+#endif // hifi_RayIntersectionInfo_h

libraries/shared/src/Shape.h

@@ -17,6 +17,8 @@
 #include <QtGlobal>
 #include <QVector>
 
+#include "RayIntersectionInfo.h"
+
 class PhysicsEntity;
 class VerletPoint;
 
@@ -59,7 +61,7 @@ public:
     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;
+    virtual bool findRayIntersection(RayIntersectionInfo& intersection) const = 0;
 
     /// \param penetration of collision
     /// \param contactPoint of collision

libraries/shared/src/ShapeCollider.cpp

@@ -1087,24 +1087,18 @@ bool capsuleVsAACubeLegacy(const CapsuleShape* capsuleA, const glm::vec3& cubeCe
     return sphereVsAACubeLegacy(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions);
 }
 
-bool findRayIntersectionWithShapes(const QVector<Shape*> shapes, const glm::vec3& rayStart, const glm::vec3& rayDirection, float& minDistance) {
+bool findRayIntersection(const QVector<Shape*>& shapes, RayIntersectionInfo& intersection) {
-    float hitDistance = FLT_MAX;
     int numShapes = shapes.size();
+    bool hit = false;
     for (int i = 0; i < numShapes; ++i) {
         Shape* shape = shapes.at(i);
         if (shape) {
-            float distance;
+            if (shape->findRayIntersection(intersection)) {
-            if (shape->findRayIntersection(rayStart, rayDirection, distance)) {
+                hit = true;
-                if (distance < hitDistance) {
-                    hitDistance = distance;
-                }
             }
         }
     }
-    if (hitDistance < FLT_MAX) {
+    return hit;
-        minDistance = hitDistance;
-    }
-    return false;
 }
 
 }   // namespace ShapeCollider

libraries/shared/src/ShapeCollider.h

@@ -15,6 +15,7 @@
 #include <QVector>
 
 #include "CollisionInfo.h"
+#include "RayIntersectionInfo.h"
 #include "SharedUtil.h" 
 
 class Shape;
@@ -145,11 +146,9 @@ namespace ShapeCollider {
     bool capsuleVsAACubeLegacy(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions);
 
     /// \param shapes list of pointers to shapes (shape pointers may be NULL)
-    /// \param startPoint beginning of ray
+    /// \param intersection[out] struct with info about Ray and hit
-    /// \param direction direction of ray
-    /// \param minDistance[out] shortest distance to intersection of ray with a shapes
     /// \return true if ray hits any shape in shapes
-    bool findRayIntersectionWithShapes(const QVector<Shape*> shapes, const glm::vec3& startPoint, const glm::vec3& direction, float& minDistance);
+    bool findRayIntersection(const QVector<Shape*>& shapes, RayIntersectionInfo& intersection);
 
 }   // namespace ShapeCollider
 

libraries/shared/src/SphereShape.cpp

@@ -13,18 +13,19 @@
 
 #include "SphereShape.h"
 
-bool SphereShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const {
+bool SphereShape::findRayIntersection(RayIntersectionInfo& intersection) const {
     float r2 = _boundingRadius * _boundingRadius;
 
     // compute closest approach (CA)
-    float a = glm::dot(_translation - rayStart, rayDirection); // a = distance from ray-start to CA
+    float a = glm::dot(_translation - intersection._rayStart, intersection._rayDirection); // a = distance from ray-start to CA
-    float b2 = glm::distance2(_translation, rayStart + a * rayDirection); // b2 = squared distance from sphere-center to CA
+    float b2 = glm::distance2(_translation, intersection._rayStart + a * intersection._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 c = sqrtf(r2 - b2); // c = distance from CA to sphere surface along intersection._rayDirection
-    float d2 = glm::distance2(rayStart, _translation); // d2 = squared distance from sphere-center to ray-start
+    float d2 = glm::distance2(intersection._rayStart, _translation); // d2 = squared distance from sphere-center to ray-start
+    float distance = FLT_MAX;
     if (a < 0.0f) {
         // ray points away from sphere-center
         if (d2 > r2) {
@@ -40,5 +41,11 @@ bool SphereShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3
         // ray starts inside sphere
         distance = a + c;
     }
-    return true;
+    if (distance > 0.0f && distance < intersection._rayLength && distance < intersection._hitDistance) {
+        intersection._hitDistance = distance;
+        intersection._hitNormal = glm::normalize(intersection._rayStart + distance * intersection._rayDirection - _translation);
+        intersection._hitShape = const_cast<SphereShape*>(this);
+        return true;
+    }
+    return false;
 }

libraries/shared/src/SphereShape.h

@@ -34,7 +34,7 @@ public:
 
     void setRadius(float radius) { _boundingRadius = radius; }
 
-    bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const;
+    bool findRayIntersection(RayIntersectionInfo& intersection) const;
 
     float getVolume() const { return 1.3333333333f * PI * _boundingRadius * _boundingRadius * _boundingRadius; }
 };

tests/physics/src/ShapeColliderTests.cpp

@@ -1803,40 +1803,44 @@ void ShapeColliderTests::capsuleTouchesAACube() {
 
 void ShapeColliderTests::rayHitsSphere() {
     float startDistance = 3.0f;
-    glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
-    glm::vec3 rayDirection(1.0f, 0.0f, 0.0f);
 
     float radius = 1.0f;
     glm::vec3 center(0.0f);
-
     SphereShape sphere(radius, center);
 
     // very simple ray along xAxis
     {
-        float distance = FLT_MAX;
+        RayIntersectionInfo intersection;
-        if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) {
+        intersection._rayStart = -startDistance * xAxis;
+        intersection._rayDirection = xAxis;
+
+        if (!sphere.findRayIntersection(intersection)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl;
         }
     
         float expectedDistance = startDistance - radius;
-        float relativeError = fabsf(distance - expectedDistance) / startDistance;
+        float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
         if (relativeError > EPSILON) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = " << relativeError << std::endl;
         }
+        if (intersection._hitShape != &sphere) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should point at sphere" 
+                << std::endl;
+        }
     }
 
     // ray along a diagonal axis
     {
-        rayStart = glm::vec3(startDistance, startDistance, 0.0f);
+        RayIntersectionInfo intersection;
-        rayDirection = - glm::normalize(rayStart);
+        intersection._rayStart = glm::vec3(startDistance, startDistance, 0.0f);
+        intersection._rayDirection = - glm::normalize(intersection._rayStart);
     
-        float distance = FLT_MAX;
+        if (!sphere.findRayIntersection(intersection)) {
-        if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl;
         }
     
         float expectedDistance = SQUARE_ROOT_OF_2 * startDistance - radius;
-        float relativeError = fabsf(distance - expectedDistance) / startDistance;
+        float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
         if (relativeError > EPSILON) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = " << relativeError << std::endl;
         }
@@ -1851,22 +1855,22 @@ void ShapeColliderTests::rayHitsSphere() {
         glm::quat rotation = glm::angleAxis(0.987654321f, axis);
         glm::vec3 translation(35.7f, 2.46f, -1.97f);
 
-        glm::vec3 unrotatedRayDirection(-1.0f, 0.0f, 0.0f);
+        glm::vec3 unrotatedRayDirection = -xAxis;
-        glm::vec3 untransformedRayStart(startDistance, 0.0f, 0.0f);
+        glm::vec3 untransformedRayStart = startDistance * xAxis;
 
-        rayStart = rotation * (untransformedRayStart + translation);
+        RayIntersectionInfo intersection;
-        rayDirection = rotation * unrotatedRayDirection;
+        intersection._rayStart = rotation * (untransformedRayStart + translation);
+        intersection._rayDirection = rotation * unrotatedRayDirection;
 
         sphere.setRadius(radius);
         sphere.setTranslation(rotation * translation);
     
-        float distance = FLT_MAX;
+        if (!sphere.findRayIntersection(intersection)) {
-        if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl;
         }
     
         float expectedDistance = startDistance - radius;
-        float relativeError = fabsf(distance - expectedDistance) / startDistance;
+        float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
         if (relativeError > EPSILON) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = " 
                 << relativeError << std::endl;
@@ -1879,31 +1883,40 @@ void ShapeColliderTests::rayBarelyHitsSphere() {
     glm::vec3 center(0.0f);
     float delta = 2.0f * EPSILON;
 
-    float startDistance = 3.0f;
-    glm::vec3 rayStart(-startDistance, radius - delta, 0.0f);
-    glm::vec3 rayDirection(1.0f, 0.0f, 0.0f);
-
     SphereShape sphere(radius, center);
+    float startDistance = 3.0f;
 
-    // very simple ray along xAxis
+    {
-    float distance = FLT_MAX;
+        RayIntersectionInfo intersection;
-    if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) {
+        intersection._rayStart = glm::vec3(-startDistance, radius - delta, 0.0f);
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl;
+        intersection._rayDirection = xAxis;
+    
+        // very simple ray along xAxis
+        if (!sphere.findRayIntersection(intersection)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl;
+        }
+        if (intersection._hitShape != &sphere) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should point at sphere" 
+                << std::endl;
+        }
     }
 
-    // translate and rotate the whole system... 
+    {
-    glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
+        // translate and rotate the whole system... 
-    glm::quat rotation = glm::angleAxis(0.987654321f, axis);
+        glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
-    glm::vec3 translation(35.7f, 2.46f, -1.97f);
+        glm::quat rotation = glm::angleAxis(0.987654321f, axis);
-
+        glm::vec3 translation(35.7f, 0.46f, -1.97f);
-    rayStart = rotation * (rayStart + translation);
+    
-    rayDirection = rotation * rayDirection;
+        RayIntersectionInfo intersection;
-    sphere.setTranslation(rotation * translation);
+        intersection._rayStart = rotation * (intersection._rayStart + translation);
-
+        intersection._rayDirection = rotation * intersection._rayDirection;
-    // ...and test again
+    
-    distance = FLT_MAX;
+        sphere.setTranslation(rotation * translation);
-    if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) {
+    
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl;
+        // ...and test again
+        if (!sphere.findRayIntersection(intersection)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl;
+        }
     }
 }
 
@@ -1914,39 +1927,47 @@ void ShapeColliderTests::rayBarelyMissesSphere() {
     glm::vec3 center(0.0f);
     float delta = 2.0f * EPSILON;
 
-    float startDistance = 3.0f;
-    glm::vec3 rayStart(-startDistance, radius + delta, 0.0f);
-    glm::vec3 rayDirection(1.0f, 0.0f, 0.0f);
-
     SphereShape sphere(radius, center);
+    float startDistance = 3.0f;
 
-    // very simple ray along xAxis
+    {
-    float distance = FLT_MAX;
+        RayIntersectionInfo intersection;
-    if (sphere.findRayIntersection(rayStart, rayDirection, distance)) {
+        intersection._rayStart = glm::vec3(-startDistance, radius + delta, 0.0f);
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl;
+        intersection._rayDirection = xAxis;
-    }
+    
-    if (distance != FLT_MAX) {
+        // very simple ray along xAxis
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" 
+        if (sphere.findRayIntersection(intersection)) {
-            << std::endl;
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl;
+        }
+        if (intersection._hitDistance != FLT_MAX) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" 
+                << std::endl;
+        }
     }
 
-    // translate and rotate the whole system... 
+    {
-    glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
+        // translate and rotate the whole system... 
-    glm::quat rotation = glm::angleAxis(0.987654321f, axis);
+        float angle = 0.987654321f;
-    glm::vec3 translation(35.7f, 2.46f, -1.97f);
+        glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
+        glm::quat rotation = glm::angleAxis(angle, axis);
+        glm::vec3 translation(35.7f, 2.46f, -1.97f);
 
-    rayStart = rotation * (rayStart + translation);
+        RayIntersectionInfo intersection;
-    rayDirection = rotation * rayDirection;
+        intersection._rayStart = rotation * (glm::vec3(-startDistance, radius + delta, 0.0f) + translation);
-    sphere.setTranslation(rotation * translation);
+        intersection._rayDirection = rotation * xAxis;
-
+        sphere.setTranslation(rotation * translation);
-    // ...and test again
+    
-    distance = FLT_MAX;
+        // ...and test again
-    if (sphere.findRayIntersection(rayStart, rayDirection, distance)) {
+        if (sphere.findRayIntersection(intersection)) {
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl;
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl;
-    }
+        }
-    if (distance != FLT_MAX) {
+        if (intersection._hitDistance != FLT_MAX) {
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" 
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" 
-            << std::endl;
+                << std::endl;
+        }
+        if (intersection._hitShape != NULL) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should be NULL" << std::endl;
+        }
     }
 }
 
@@ -1957,85 +1978,99 @@ void ShapeColliderTests::rayHitsCapsule() {
     glm::vec3 center(0.0f);
     CapsuleShape capsule(radius, halfHeight);
 
-    { // simple test along xAxis 
+    // simple tests along xAxis 
-        // toward capsule center
+    { // toward capsule center
-        glm::vec3 rayStart(startDistance, 0.0f, 0.0f);
+        RayIntersectionInfo intersection;
-        glm::vec3 rayDirection(-1.0f, 0.0f, 0.0f);
+        intersection._rayStart = glm::vec3(startDistance, 0.0f, 0.0f);
-        float distance = FLT_MAX;
+        intersection._rayDirection = - xAxis;
-        if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) {
+        if (!capsule.findRayIntersection(intersection)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
         }
         float expectedDistance = startDistance - radius;
-        float relativeError = fabsf(distance - expectedDistance) / startDistance;
+        float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
         if (relativeError > EPSILON) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " 
                 << relativeError << std::endl;
         }
+        if (intersection._hitShape != &capsule) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should point at capsule" 
+                << std::endl;
+        }
+    }
 
-        // toward top of cylindrical wall
+    { // toward top of cylindrical wall
-        rayStart.y = halfHeight;
+        RayIntersectionInfo intersection;
-        distance = FLT_MAX;
+        intersection._rayStart = glm::vec3(startDistance, halfHeight, 0.0f);
-        if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) {
+        intersection._rayDirection = - xAxis;
+        if (!capsule.findRayIntersection(intersection)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
         }
-        relativeError = fabsf(distance - expectedDistance) / startDistance;
+        float expectedDistance = startDistance - radius;
+        float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
         if (relativeError > EPSILON) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " 
                 << relativeError << std::endl;
         }
+    }
 
-        // toward top cap
+    float delta = 2.0f * EPSILON;
-        float delta = 2.0f * EPSILON;
+    { // toward top cap
-        rayStart.y = halfHeight + delta;
+        RayIntersectionInfo intersection;
-        distance = FLT_MAX;
+        intersection._rayStart = glm::vec3(startDistance, halfHeight + delta, 0.0f);
-        if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) {
+        intersection._rayDirection = - xAxis;
+        if (!capsule.findRayIntersection(intersection)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
         }
-        relativeError = fabsf(distance - expectedDistance) / startDistance;
+        float expectedDistance = startDistance - radius;
+        float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
         if (relativeError > EPSILON) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " 
                 << relativeError << std::endl;
         }
+    }
 
-        const float EDGE_CASE_SLOP_FACTOR = 20.0f;
+    const float EDGE_CASE_SLOP_FACTOR = 20.0f;
-
+    { // toward tip of top cap
-        // toward tip of top cap
+        RayIntersectionInfo intersection;
-        rayStart.y = halfHeight + radius - delta;
+        intersection._rayStart = glm::vec3(startDistance, halfHeight + radius - delta, 0.0f);
-        distance = FLT_MAX;
+        intersection._rayDirection = - xAxis;
-        if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) {
+        if (!capsule.findRayIntersection(intersection)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
         }
-        expectedDistance = startDistance - radius * sqrtf(2.0f * delta);    // using small angle approximation of cosine
+        float expectedDistance = startDistance - radius * sqrtf(2.0f * delta);    // using small angle approximation of cosine
-        relativeError = fabsf(distance - expectedDistance) / startDistance;
+        float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
         // for edge cases we allow a LOT of error
         if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " 
                 << relativeError << std::endl;
         }
+    }
 
-        // toward tip of bottom cap
+    { // toward tip of bottom cap
-        rayStart.y = - halfHeight - radius + delta;
+        RayIntersectionInfo intersection;
-        distance = FLT_MAX;
+        intersection._rayStart = glm::vec3(startDistance, - halfHeight - radius + delta, 0.0f);
-        if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) {
+        intersection._rayDirection = - xAxis;
+        if (!capsule.findRayIntersection(intersection)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
         }
-        expectedDistance = startDistance - radius * sqrtf(2.0f * delta);    // using small angle approximation of cosine
+        float expectedDistance = startDistance - radius * sqrtf(2.0f * delta);    // using small angle approximation of cosine
-        relativeError = fabsf(distance - expectedDistance) / startDistance;
+        float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
         // for edge cases we allow a LOT of error
         if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " 
                 << relativeError << std::endl;
         }
+    }
 
-        // toward edge of capsule cylindrical face
+    { // toward edge of capsule cylindrical face
-        rayStart.y = 0.0f;
+        RayIntersectionInfo intersection;
-        rayStart.z = radius - delta;
+        intersection._rayStart = glm::vec3(startDistance, 0.0f, radius - delta);
-        distance = FLT_MAX;
+        intersection._rayDirection = - xAxis;
-        if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) {
+        if (!capsule.findRayIntersection(intersection)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
         }
-        expectedDistance = startDistance - radius * sqrtf(2.0f * delta);    // using small angle approximation of cosine
+        float expectedDistance = startDistance - radius * sqrtf(2.0f * delta);    // using small angle approximation of cosine
-        relativeError = fabsf(distance - expectedDistance) / startDistance;
+        float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
         // for edge cases we allow a LOT of error
         if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = " 
@@ -2055,43 +2090,47 @@ void ShapeColliderTests::rayMissesCapsule() {
 
     { // simple test along xAxis 
         // toward capsule center
-        glm::vec3 rayStart(startDistance, 0.0f, 0.0f);
+        RayIntersectionInfo intersection;
-        glm::vec3 rayDirection(-1.0f, 0.0f, 0.0f);
+        intersection._rayStart = glm::vec3(startDistance, 0.0f, 0.0f);
+        intersection._rayDirection = -xAxis;
         float delta = 2.0f * EPSILON;
 
         // over top cap
-        rayStart.y = halfHeight + radius + delta;
+        intersection._rayStart.y = halfHeight + radius + delta;
-        float distance = FLT_MAX;
+        intersection._hitDistance = FLT_MAX;
-        if (capsule.findRayIntersection(rayStart, rayDirection, distance)) {
+        if (capsule.findRayIntersection(intersection)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl;
         }
-        if (distance != FLT_MAX) {
+        if (intersection._hitDistance != FLT_MAX) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" 
                 << std::endl;
         }
 
         // below bottom cap
-        rayStart.y = - halfHeight - radius - delta;
+        intersection._rayStart.y = - halfHeight - radius - delta;
-        distance = FLT_MAX;
+        intersection._hitDistance = FLT_MAX;
-        if (capsule.findRayIntersection(rayStart, rayDirection, distance)) {
+        if (capsule.findRayIntersection(intersection)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl;
         }
-        if (distance != FLT_MAX) {
+        if (intersection._hitDistance != FLT_MAX) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" 
                 << std::endl;
         }
 
         // past edge of capsule cylindrical face
-        rayStart.y = 0.0f;
+        intersection._rayStart.y = 0.0f;
-        rayStart.z = radius + delta;
+        intersection._rayStart.z = radius + delta;
-        distance = FLT_MAX;
+        intersection._hitDistance = FLT_MAX;
-        if (capsule.findRayIntersection(rayStart, rayDirection, distance)) {
+        if (capsule.findRayIntersection(intersection)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl;
         }
-        if (distance != FLT_MAX) {
+        if (intersection._hitDistance != FLT_MAX) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" 
                 << std::endl;
         }
+        if (intersection._hitShape != NULL) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should be NULL" << std::endl;
+        }
     }
     // TODO: test at steep angles near edge
 }
@@ -2101,45 +2140,53 @@ void ShapeColliderTests::rayHitsPlane() {
     float planeDistanceFromOrigin = 3.579f;
     glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f);
     PlaneShape plane;
-    plane.setTranslation(planePosition);
+    plane.setPoint(planePosition);
+    plane.setNormal(yAxis);
 
     // make a simple ray
     float startDistance = 1.234f;
-    glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
+    {
-    glm::vec3 rayDirection = glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f));
+        RayIntersectionInfo intersection;
-
+        intersection._rayStart = -startDistance * xAxis;
-    float distance = FLT_MAX;
+        intersection._rayDirection = glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f));
-    if (!plane.findRayIntersection(rayStart, rayDirection, distance)) {
+    
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl;
+        if (!plane.findRayIntersection(intersection)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl;
+        }
+    
+        float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin;
+        float relativeError = fabsf(intersection._hitDistance - expectedDistance) / planeDistanceFromOrigin;
+        if (relativeError > EPSILON) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = " 
+                << relativeError << std::endl;
+        }
+        if (intersection._hitShape != &plane) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should point at plane" 
+                << std::endl;
+        }
     }
 
-    float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin;
+    { // rotate the whole system and try again
-    float relativeError = fabsf(distance - expectedDistance) / planeDistanceFromOrigin;
+        float angle = 37.8f;
-    if (relativeError > EPSILON) {
+        glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) );
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = " 
+        glm::quat rotation = glm::angleAxis(angle, axis);
-            << relativeError << std::endl;
+    
-    }
+        plane.setNormal(rotation * yAxis);
-
+        plane.setPoint(rotation * planePosition);
-    // rotate the whole system and try again
+        RayIntersectionInfo intersection;
-    float angle = 37.8f;
+        intersection._rayStart = rotation * (-startDistance * xAxis);
-    glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) );
+        intersection._rayDirection = rotation * glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f));
-    glm::quat rotation = glm::angleAxis(angle, axis);
+    
-
+        if (!plane.findRayIntersection(intersection)) {
-    plane.setTranslation(rotation * planePosition);
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl;
-    plane.setRotation(rotation);
+        }
-    rayStart = rotation * rayStart;
+    
-    rayDirection = rotation * rayDirection;
+        float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin;
-
+        float relativeError = fabsf(intersection._hitDistance - expectedDistance) / planeDistanceFromOrigin;
-    distance = FLT_MAX;
+        if (relativeError > EPSILON) {
-    if (!plane.findRayIntersection(rayStart, rayDirection, distance)) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = " 
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl;
+                << relativeError << std::endl;
-    }
+        }
-
-    expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin;
-    relativeError = fabsf(distance - expectedDistance) / planeDistanceFromOrigin;
-    if (relativeError > EPSILON) {
-        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = " 
-            << relativeError << std::endl;
     }
 }
 
@@ -2152,14 +2199,14 @@ void ShapeColliderTests::rayMissesPlane() {
 
     { // parallel rays should miss
         float startDistance = 1.234f;
-        glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
+        RayIntersectionInfo intersection;
-        glm::vec3 rayDirection = glm::normalize(glm::vec3(-1.0f, 0.0f, -1.0f));
+        intersection._rayStart = glm::vec3(-startDistance, 0.0f, 0.0f);
+        intersection._rayDirection = glm::normalize(glm::vec3(-1.0f, 0.0f, -1.0f));
     
-        float distance = FLT_MAX;
+        if (plane.findRayIntersection(intersection)) {
-        if (plane.findRayIntersection(rayStart, rayDirection, distance)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
         }
-        if (distance != FLT_MAX) {
+        if (intersection._hitDistance != FLT_MAX) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" 
                 << std::endl;
         }
@@ -2171,29 +2218,35 @@ void ShapeColliderTests::rayMissesPlane() {
     
         plane.setTranslation(rotation * planePosition);
         plane.setRotation(rotation);
-        rayStart = rotation * rayStart;
+
-        rayDirection = rotation * rayDirection;
+        intersection._rayStart = rotation * intersection._rayStart;
-    
+        intersection._rayDirection = rotation * intersection._rayDirection;
-        distance = FLT_MAX;
+        intersection._hitDistance = FLT_MAX;
-        if (plane.findRayIntersection(rayStart, rayDirection, distance)) {
+
+        if (plane.findRayIntersection(intersection)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
         }
-        if (distance != FLT_MAX) {
+        if (intersection._hitDistance != FLT_MAX) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" 
                 << std::endl;
         }
+        if (intersection._hitShape != NULL) {
+            std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should be NULL" << std::endl;
+        }
     }
 
     { // make a simple ray that points away from plane
         float startDistance = 1.234f;
-        glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
+
-        glm::vec3 rayDirection = glm::normalize(glm::vec3(-1.0f, -1.0f, -1.0f));
+        RayIntersectionInfo intersection;
+        intersection._rayStart = glm::vec3(-startDistance, 0.0f, 0.0f);
+        intersection._rayDirection = glm::normalize(glm::vec3(-1.0f, -1.0f, -1.0f));
+        intersection._hitDistance = FLT_MAX;
     
-        float distance = FLT_MAX;
+        if (plane.findRayIntersection(intersection)) {
-        if (plane.findRayIntersection(rayStart, rayDirection, distance)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
         }
-        if (distance != FLT_MAX) {
+        if (intersection._hitDistance != FLT_MAX) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" 
                 << std::endl;
         }
@@ -2205,20 +2258,225 @@ void ShapeColliderTests::rayMissesPlane() {
     
         plane.setTranslation(rotation * planePosition);
         plane.setRotation(rotation);
-        rayStart = rotation * rayStart;
+
-        rayDirection = rotation * rayDirection;
+        intersection._rayStart = rotation * intersection._rayStart;
+        intersection._rayDirection = rotation * intersection._rayDirection;
+        intersection._hitDistance = FLT_MAX;
     
-        distance = FLT_MAX;
+        if (plane.findRayIntersection(intersection)) {
-        if (plane.findRayIntersection(rayStart, rayDirection, distance)) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
         }
-        if (distance != FLT_MAX) {
+        if (intersection._hitDistance != FLT_MAX) {
             std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss" 
                 << std::endl;
         }
     }
 }
 
+void ShapeColliderTests::rayHitsAACube() {
+    glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f);
+    float cubeSide = 2.127f;
+    AACubeShape cube(cubeSide, cubeCenter);
+
+    float rayOffset = 3.796f;
+
+    glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis};
+    int numDirections = 3;
+    int numRayCasts = 5;
+
+    for (int i = 0; i < numDirections; ++i) {
+        for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
+            glm::vec3 faceNormal = sign * faceNormals[i];
+            glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections];
+            glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections];
+
+            // pick a random point somewhere above the face
+            glm::vec3 rayStart = cubeCenter + 
+                (cubeSide + rayOffset) * faceNormal + 
+                (cubeSide * (randFloat() - 0.5f)) * secondNormal +
+                (cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+
+            // cast multiple rays toward the face
+            for (int j = 0; j < numRayCasts; ++j) {
+                // pick a random point on the face
+                glm::vec3 facePoint = cubeCenter + 
+                    0.5f * cubeSide * faceNormal + 
+                    (cubeSide * (randFloat() - 0.5f)) * secondNormal +
+                    (cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+
+                // construct a ray from first point through second point
+                RayIntersectionInfo intersection;
+                intersection._rayStart = rayStart;
+                intersection._rayDirection = glm::normalize(facePoint - rayStart);
+                intersection._rayLength = 1.0001f * glm::distance(rayStart, facePoint);
+
+                // cast the ray
+                bool hit = cube.findRayIntersection(intersection);
+
+                // validate
+                if (!hit) {
+                    std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit cube face" << std::endl;
+                    break;
+                }
+                if (glm::abs(1.0f - glm::dot(faceNormal, intersection._hitNormal)) > EPSILON) {
+                    std::cout << __FILE__ << ":" << __LINE__ 
+                        << " ERROR: ray should hit cube face with normal " << faceNormal
+                        << " but found different normal " << intersection._hitNormal << std::endl;
+                }
+                if (glm::distance(facePoint, intersection.getIntersectionPoint()) > EPSILON) {
+                    std::cout << __FILE__ << ":" << __LINE__ 
+                        << " ERROR: ray should hit cube face at " << facePoint
+                        << " but actually hit at " << intersection.getIntersectionPoint()
+                        << std::endl;
+                }
+                if (intersection._hitShape != &cube) {
+                    std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should point at cube" 
+                        << std::endl;
+                }
+            }
+        }
+    }
+}
+
+void ShapeColliderTests::rayMissesAACube() {
+    //glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f);
+    //float cubeSide = 2.127f;
+    glm::vec3 cubeCenter(0.0f);
+    float cubeSide = 2.f;
+    AACubeShape cube(cubeSide, cubeCenter);
+
+    float rayOffset = 3.796f;
+
+    glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis};
+    int numDirections = 3;
+    int numRayCasts = 5;
+
+    const float SOME_SMALL_NUMBER = 0.0001f;
+
+    { // ray misses cube for being too short
+        for (int i = 0; i < numDirections; ++i) {
+            for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
+                glm::vec3 faceNormal = sign * faceNormals[i];
+                glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections];
+                glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections];
+    
+                // pick a random point somewhere above the face
+                glm::vec3 rayStart = cubeCenter + 
+                    (cubeSide + rayOffset) * faceNormal + 
+                    (cubeSide * (randFloat() - 0.5f)) * secondNormal +
+                    (cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+    
+                // cast multiple rays toward the face
+                for (int j = 0; j < numRayCasts; ++j) {
+                    // pick a random point on the face
+                    glm::vec3 facePoint = cubeCenter + 
+                        0.5f * cubeSide * faceNormal + 
+                        (cubeSide * (randFloat() - 0.5f)) * secondNormal +
+                        (cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+    
+                    // construct a ray from first point to almost second point
+                    RayIntersectionInfo intersection;
+                    intersection._rayStart = rayStart;
+                    intersection._rayDirection = glm::normalize(facePoint - rayStart);
+                    intersection._rayLength = (1.0f - SOME_SMALL_NUMBER) * glm::distance(rayStart, facePoint);
+    
+                    // cast the ray
+                    if (cube.findRayIntersection(intersection)) {
+                        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should NOT hit cube face " 
+                            << faceNormal << std::endl;
+                    }
+                }
+            }
+        }
+    }
+    { // long ray misses cube
+        for (int i = 0; i < numDirections; ++i) {
+            for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
+                glm::vec3 faceNormal = sign * faceNormals[i];
+                glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections];
+                glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections];
+    
+                // pick a random point somewhere above the face
+                glm::vec3 rayStart = cubeCenter + 
+                    (cubeSide + rayOffset) * faceNormal + 
+                    (cubeSide * (randFloat() - 0.5f)) * secondNormal +
+                    (cubeSide * (randFloat() - 0.5f)) * thirdNormal;
+    
+                // cast multiple rays that miss the face
+                for (int j = 0; j < numRayCasts; ++j) {
+                    // pick a random point just outside of face
+                    float inside = (cubeSide * (randFloat() - 0.5f));
+                    float outside = 0.5f * cubeSide + SOME_SMALL_NUMBER * randFloat();
+                    if (randFloat() - 0.5f < 0.0f) {
+                        outside *= -1.0f;
+                    }
+                    glm::vec3 sidePoint = cubeCenter + 0.5f * cubeSide * faceNormal;
+                    if (randFloat() - 0.5f < 0.0f) {
+                        sidePoint += outside * secondNormal + inside * thirdNormal;
+                    } else {
+                        sidePoint += inside * secondNormal + outside * thirdNormal;
+                    }
+
+                    // construct a ray from first point through second point
+                    RayIntersectionInfo intersection;
+                    intersection._rayStart = rayStart;
+                    intersection._rayDirection = glm::normalize(sidePoint - rayStart);
+    
+                    // cast the ray
+                    if (cube.findRayIntersection(intersection)) {
+                        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should NOT hit cube face " 
+                            << faceNormal << std::endl;
+                    }
+                }
+            }
+        }
+    }
+    { // ray parallel to face barely misses cube
+        for (int i = 0; i < numDirections; ++i) {
+            for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
+                glm::vec3 faceNormal = sign * faceNormals[i];
+                glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections];
+                glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections];
+
+                // cast multiple rays that miss the face
+                for (int j = 0; j < numRayCasts; ++j) {
+                    // rayStart is above the face
+                    glm::vec3 rayStart = cubeCenter + (0.5f + SOME_SMALL_NUMBER) * cubeSide * faceNormal;
+
+                    // move rayStart to some random edge and choose the ray direction to point across the face
+                    float inside = (cubeSide * (randFloat() - 0.5f));
+                    float outside = 0.5f * cubeSide + SOME_SMALL_NUMBER * randFloat();
+                    if (randFloat() - 0.5f < 0.0f) {
+                        outside *= -1.0f;
+                    }
+                    glm::vec3 rayDirection = secondNormal;
+                    if (randFloat() - 0.5f < 0.0f) {
+                        rayStart += outside * secondNormal + inside * thirdNormal;
+                    } else {
+                        rayStart += inside * secondNormal + outside * thirdNormal;
+                        rayDirection = thirdNormal;
+                    }
+                    if (outside > 0.0f) {
+                        rayDirection *= -1.0f;
+                    }
+
+                    // construct a ray from first point through second point
+                    RayIntersectionInfo intersection;
+                    intersection._rayStart = rayStart;
+                    intersection._rayDirection = rayDirection;
+    
+                    // cast the ray
+                    if (cube.findRayIntersection(intersection)) {
+                        std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should NOT hit cube face " 
+                            << faceNormal << std::endl;
+                    }
+                }
+            }
+        }
+    }
+
+}
+
 void ShapeColliderTests::measureTimeOfCollisionDispatch() {
     /* KEEP for future manual testing
     // create two non-colliding spheres
@@ -2278,4 +2536,7 @@ void ShapeColliderTests::runAllTests() {
     rayMissesCapsule();
     rayHitsPlane();
     rayMissesPlane();
+
+    rayHitsAACube();
+    rayMissesAACube();
 }

tests/physics/src/ShapeColliderTests.h

@@ -38,6 +38,8 @@ namespace ShapeColliderTests {
     void rayMissesCapsule();
     void rayHitsPlane();
     void rayMissesPlane();
+    void rayHitsAACube();
+    void rayMissesAACube();
 
     void measureTimeOfCollisionDispatch();