Merge remote-tracking branch 'upstream/master' into scriptvec3

android/app/src/main/java/io/highfidelity/hifiinterface/fragment/FriendsFragment.java

@@ -98,13 +98,17 @@ public class FriendsFragment extends Fragment {
 
         mUsersAdapter.setListener(new UserListAdapter.AdapterListener() {
             @Override
-            public void onEmptyAdapter() {
-                mSwipeRefreshLayout.setRefreshing(false);
+            public void onEmptyAdapter(boolean shouldStopRefreshing) {
+                if (shouldStopRefreshing) {
+                    mSwipeRefreshLayout.setRefreshing(false);
+                }
             }
 
             @Override
-            public void onNonEmptyAdapter() {
-                mSwipeRefreshLayout.setRefreshing(false);
+            public void onNonEmptyAdapter(boolean shouldStopRefreshing) {
+                if (shouldStopRefreshing) {
+                    mSwipeRefreshLayout.setRefreshing(false);
+                }
             }
 
             @Override
@@ -115,6 +119,8 @@ public class FriendsFragment extends Fragment {
 
         mUsersView.setAdapter(mUsersAdapter);
 
+        mUsersAdapter.startLoad();
+
         mSlidingUpPanelLayout.setFadeOnClickListener(new View.OnClickListener() {
             @Override
             public void onClick(View view) {

android/app/src/main/java/io/highfidelity/hifiinterface/fragment/HomeFragment.java

@@ -76,18 +76,22 @@ public class HomeFragment extends Fragment {
         });
         mDomainAdapter.setListener(new DomainAdapter.AdapterListener() {
             @Override
-            public void onEmptyAdapter() {
+            public void onEmptyAdapter(boolean shouldStopRefreshing) {
                 searchNoResultsView.setText(R.string.search_no_results);
                 searchNoResultsView.setVisibility(View.VISIBLE);
                 mDomainsView.setVisibility(View.GONE);
-                mSwipeRefreshLayout.setRefreshing(false);
+                if (shouldStopRefreshing) {
+                    mSwipeRefreshLayout.setRefreshing(false);
+                }
             }
 
             @Override
-            public void onNonEmptyAdapter() {
+            public void onNonEmptyAdapter(boolean shouldStopRefreshing) {
                 searchNoResultsView.setVisibility(View.GONE);
                 mDomainsView.setVisibility(View.VISIBLE);
-                mSwipeRefreshLayout.setRefreshing(false);
+                if (shouldStopRefreshing) {
+                    mSwipeRefreshLayout.setRefreshing(false);
+                }
             }
 
             @Override
@@ -96,11 +100,20 @@ public class HomeFragment extends Fragment {
             }
         });
         mDomainsView.setAdapter(mDomainAdapter);
+        mDomainAdapter.startLoad();
 
         mSearchView = rootView.findViewById(R.id.searchView);
         mSearchIconView = rootView.findViewById(R.id.search_mag_icon);
         mClearSearch = rootView.findViewById(R.id.search_clear);
 
+        getActivity().getWindow().setSoftInputMode(WindowManager.LayoutParams.SOFT_INPUT_STATE_ALWAYS_HIDDEN);
+
+        return rootView;
+    }
+
+    @Override
+    public void onStart() {
+        super.onStart();
         mSearchView.addTextChangedListener(new TextWatcher() {
             @Override
             public void beforeTextChanged(CharSequence charSequence, int i, int i1, int i2) {}
@@ -142,10 +155,6 @@ public class HomeFragment extends Fragment {
                 mDomainAdapter.loadDomains(mSearchView.getText().toString(), true);
             }
         });
-
-        getActivity().getWindow().setSoftInputMode(WindowManager.LayoutParams.SOFT_INPUT_STATE_ALWAYS_HIDDEN);
-
-        return rootView;
     }
 
     @Override

android/app/src/main/java/io/highfidelity/hifiinterface/view/DomainAdapter.java

@@ -12,6 +12,7 @@ import android.widget.TextView;
 
 import com.squareup.picasso.Picasso;
 
+import java.util.Arrays;
 import java.util.List;
 
 import io.highfidelity.hifiinterface.R;
@@ -36,19 +37,41 @@ public class DomainAdapter extends RecyclerView.Adapter<DomainAdapter.ViewHolder
     // references to our domains
     private Domain[] mDomains = {};
 
+    private static Domain[] DOMAINS_TMP_CACHE = {};
+
     public DomainAdapter(Context c, String protocol, String lastLocation) {
         mContext = c;
         this.mInflater = LayoutInflater.from(mContext);
         mProtocol = protocol;
         mLastLocation = lastLocation;
         domainProvider = new UserStoryDomainProvider(mProtocol);
-        loadDomains("", true);
     }
 
     public void setListener(AdapterListener adapterListener) {
         mAdapterListener = adapterListener;
     }
 
+    public void startLoad() {
+        useTmpCachedDomains();
+        loadDomains("", true);
+    }
+
+    private void useTmpCachedDomains() {
+        synchronized (this) {
+            if (DOMAINS_TMP_CACHE != null && DOMAINS_TMP_CACHE.length > 0) {
+                mDomains = Arrays.copyOf(DOMAINS_TMP_CACHE, DOMAINS_TMP_CACHE.length);
+                notifyDataSetChanged();
+                if (mAdapterListener != null) {
+                    if (mDomains.length == 0) {
+                        mAdapterListener.onEmptyAdapter(false);
+                    } else {
+                        mAdapterListener.onNonEmptyAdapter(false);
+                    }
+                }
+            }
+        }
+    }
+
     public void loadDomains(String filterText, boolean forceRefresh) {
         domainProvider.retrieve(filterText, new DomainProvider.DomainCallback() {
             @Override
@@ -60,13 +83,18 @@ public class DomainAdapter extends RecyclerView.Adapter<DomainAdapter.ViewHolder
                 overrideDefaultThumbnails(domain);
 
                 mDomains = new Domain[domain.size()];
-                mDomains = domain.toArray(mDomains);
-                notifyDataSetChanged();
-                if (mAdapterListener != null) {
-                    if (mDomains.length == 0) {
-                        mAdapterListener.onEmptyAdapter();
-                    } else {
-                        mAdapterListener.onNonEmptyAdapter();
+                synchronized (this) {
+                    domain.toArray(mDomains);
+                    if (filterText.isEmpty()) {
+                        DOMAINS_TMP_CACHE = Arrays.copyOf(mDomains, mDomains.length);
+                    }
+                    notifyDataSetChanged();
+                    if (mAdapterListener != null) {
+                        if (mDomains.length == 0) {
+                            mAdapterListener.onEmptyAdapter(true);
+                        } else {
+                            mAdapterListener.onNonEmptyAdapter(true);
+                        }
                     }
                 }
             }
@@ -112,8 +140,6 @@ public class DomainAdapter extends RecyclerView.Adapter<DomainAdapter.ViewHolder
 
     @Override
     public void onBindViewHolder(ViewHolder holder, int position) {
-        // TODO
-        //holder.thumbnail.setImageResource(mDomains[position].thumbnail);
         Domain domain = mDomains[position];
         holder.mDomainName.setText(domain.name);
         Uri uri = Uri.parse(domain.thumbnail);
@@ -164,8 +190,8 @@ public class DomainAdapter extends RecyclerView.Adapter<DomainAdapter.ViewHolder
     }
 
     public interface AdapterListener {
-        void onEmptyAdapter();
-        void onNonEmptyAdapter();
+        void onEmptyAdapter(boolean shouldStopRefreshing);
+        void onNonEmptyAdapter(boolean shouldStopRefreshing);
         void onError(Exception e, String message);
     }
 }

android/app/src/main/java/io/highfidelity/hifiinterface/view/UserListAdapter.java

@@ -37,28 +37,57 @@ public class UserListAdapter extends RecyclerView.Adapter<UserListAdapter.ViewHo
     private ItemClickListener mClickListener;
     private AdapterListener mAdapterListener;
 
+    private static List<User> USERS_TMP_CACHE;
+
     public UserListAdapter(Context c, UsersProvider usersProvider) {
         mContext = c;
         mInflater = LayoutInflater.from(mContext);
         mProvider = usersProvider;
-        loadUsers();
     }
 
     public void setListener(AdapterListener adapterListener) {
         mAdapterListener = adapterListener;
     }
 
+    public void startLoad() {
+        useTmpCachedUsers();
+        loadUsers();
+    }
+
+    private void useTmpCachedUsers() {
+        synchronized (this) {
+            if (USERS_TMP_CACHE != null && USERS_TMP_CACHE.size() > 0) {
+                mUsers = new ArrayList<>(USERS_TMP_CACHE.size());
+                mUsers.addAll(USERS_TMP_CACHE);
+                notifyDataSetChanged();
+                if (mAdapterListener != null) {
+                    if (mUsers.isEmpty()) {
+                        mAdapterListener.onEmptyAdapter(false);
+                    } else {
+                        mAdapterListener.onNonEmptyAdapter(false);
+                    }
+                }
+            }
+        }
+    }
+
     public void loadUsers() {
         mProvider.retrieve(new UsersProvider.UsersCallback() {
             @Override
             public void retrieveOk(List<User> users) {
                 mUsers = new ArrayList<>(users);
                 notifyDataSetChanged();
-                if (mAdapterListener != null) {
-                    if (mUsers.isEmpty()) {
-                        mAdapterListener.onEmptyAdapter();
-                    } else {
-                        mAdapterListener.onNonEmptyAdapter();
+
+                synchronized (this) {
+                    USERS_TMP_CACHE = new ArrayList<>(mUsers.size());
+                    USERS_TMP_CACHE.addAll(mUsers);
+
+                    if (mAdapterListener != null) {
+                        if (mUsers.isEmpty()) {
+                            mAdapterListener.onEmptyAdapter(true);
+                        } else {
+                            mAdapterListener.onNonEmptyAdapter(true);
+                        }
                     }
                 }
             }
@@ -240,8 +269,9 @@ public class UserListAdapter extends RecyclerView.Adapter<UserListAdapter.ViewHo
     }
 
     public interface AdapterListener {
-        void onEmptyAdapter();
-        void onNonEmptyAdapter();
+        void onEmptyAdapter(boolean shouldStopRefreshing);
+        void onNonEmptyAdapter(boolean shouldStopRefreshing);
         void onError(Exception e, String message);
     }
+
 }

domain-server/src/DomainGatekeeper.cpp

@@ -463,19 +463,15 @@ SharedNodePointer DomainGatekeeper::processAgentConnectRequest(const NodeConnect
     limitedNodeList->eachNodeBreakable([nodeConnection, username, &existingNodeID](const SharedNodePointer& node){
 
         if (node->getPublicSocket() == nodeConnection.publicSockAddr && node->getLocalSocket() == nodeConnection.localSockAddr) {
-            // we have a node that already has these exact sockets - this can occur if a node
-            // is failing to connect to the domain
-
-            // we'll re-use the existing node ID
-            // as long as the user hasn't changed their username (by logging in or logging out)
-            auto existingNodeData = static_cast<DomainServerNodeData*>(node->getLinkedData());
-
-            if (existingNodeData->getUsername() == username) {
-                qDebug() << "Deleting existing connection from same sockaddr: " << node->getUUID();
-                existingNodeID = node->getUUID();
-                return false;
-            }
+            // we have a node that already has these exact sockets
+            // this can occur if a node is failing to connect to the domain
+            
+            // remove the old node before adding the new node
+            qDebug() << "Deleting existing connection from same sockaddr: " << node->getUUID();
+            existingNodeID = node->getUUID();
+            return false;
         }
+
         return true;
     });
 

interface/resources/qml/Stats.qml

@@ -119,6 +119,22 @@ Item {
                         visible: root.expanded
                         text: "Avatars NOT Updated: " + root.notUpdatedAvatarCount
                     }
+                    StatText {
+                        visible: root.expanded
+                        text: "Total picks:\n    " +
+                                    root.stylusPicksCount + " styluses\n    " +
+                                    root.rayPicksCount + " rays\n    " +
+                                    root.parabolaPicksCount + " parabolas\n    " +
+                                    root.collisionPicksCount + " colliders"
+                    }
+                    StatText {
+                        visible: root.expanded
+                        text: "Intersection calls: Entities/Overlays/Avatars/HUD\n    " +
+                                    "Styluses:\t" + root.stylusPicksUpdated.x + "/" + root.stylusPicksUpdated.y + "/" + root.stylusPicksUpdated.z + "/" + root.stylusPicksUpdated.w + "\n    " +
+                                    "Rays:\t" + root.rayPicksUpdated.x + "/" + root.rayPicksUpdated.y + "/" + root.rayPicksUpdated.z + "/" + root.rayPicksUpdated.w + "\n    " +
+                                    "Parabolas:\t" + root.parabolaPicksUpdated.x + "/" + root.parabolaPicksUpdated.y + "/" + root.parabolaPicksUpdated.z + "/" + root.parabolaPicksUpdated.w + "\n    " +
+                                    "Colliders:\t" + root.collisionPicksUpdated.x + "/" + root.collisionPicksUpdated.y + "/" + root.collisionPicksUpdated.z + "/" + root.collisionPicksUpdated.w
+                    }
                 }
             }
 

interface/resources/qml/controls/TabletWebView.qml

@@ -52,12 +52,18 @@ Item {
                 id: back
                 enabledColor: hifi.colors.darkGray
                 disabledColor: hifi.colors.lightGrayText
-                enabled: historyIndex > 0
+                enabled: true
                 text: "BACK"
 
                 MouseArea {
                     anchors.fill: parent
-                    onClicked: goBack()
+                    onClicked: {
+                        if (historyIndex > 0) {
+                            goBack();
+                        } else {
+                            closeWebEngine();
+                        }
+                    }
                 }
             }
 

interface/src/Application.cpp

@@ -726,6 +726,9 @@ static const QString STATE_SNAP_TURN = "SnapTurn";
 static const QString STATE_ADVANCED_MOVEMENT_CONTROLS = "AdvancedMovement";
 static const QString STATE_GROUNDED = "Grounded";
 static const QString STATE_NAV_FOCUSED = "NavigationFocused";
+static const QString STATE_PLATFORM_WINDOWS = "PlatformWindows";
+static const QString STATE_PLATFORM_MAC = "PlatformMac";
+static const QString STATE_PLATFORM_ANDROID = "PlatformAndroid";
 
 // Statically provided display and input plugins
 extern DisplayPluginList getDisplayPlugins();
@@ -909,7 +912,8 @@ bool setupEssentials(int& argc, char** argv, bool runningMarkerExisted) {
     DependencyManager::set<MessagesClient>();
     controller::StateController::setStateVariables({ { STATE_IN_HMD, STATE_CAMERA_FULL_SCREEN_MIRROR,
                     STATE_CAMERA_FIRST_PERSON, STATE_CAMERA_THIRD_PERSON, STATE_CAMERA_ENTITY, STATE_CAMERA_INDEPENDENT,
-                    STATE_SNAP_TURN, STATE_ADVANCED_MOVEMENT_CONTROLS, STATE_GROUNDED, STATE_NAV_FOCUSED } });
+                    STATE_SNAP_TURN, STATE_ADVANCED_MOVEMENT_CONTROLS, STATE_GROUNDED, STATE_NAV_FOCUSED,
+                    STATE_PLATFORM_WINDOWS, STATE_PLATFORM_MAC, STATE_PLATFORM_ANDROID } });
     DependencyManager::set<UserInputMapper>();
     DependencyManager::set<controller::ScriptingInterface, ControllerScriptingInterface>();
     DependencyManager::set<InterfaceParentFinder>();
@@ -1683,6 +1687,27 @@ Application::Application(int& argc, char** argv, QElapsedTimer& startupTimer, bo
     _applicationStateDevice->setInputVariant(STATE_NAV_FOCUSED, []() -> float {
         return DependencyManager::get<OffscreenUi>()->navigationFocused() ? 1 : 0;
     });
+    _applicationStateDevice->setInputVariant(STATE_PLATFORM_WINDOWS, []() -> float {
+#if defined(Q_OS_WIN) 
+        return 1;
+#else
+        return 0;
+#endif
+    });
+    _applicationStateDevice->setInputVariant(STATE_PLATFORM_MAC, []() -> float {
+#if defined(Q_OS_MAC) 
+        return 1;
+#else
+        return 0;
+#endif
+    });
+    _applicationStateDevice->setInputVariant(STATE_PLATFORM_ANDROID, []() -> float {
+#if defined(Q_OS_ANDROID) 
+        return 1;
+#else
+        return 0;
+#endif
+    });
 
     // Setup the _keyboardMouseDevice, _touchscreenDevice, _touchscreenVirtualPadDevice and the user input mapper with the default bindings
     userInputMapper->registerDevice(_keyboardMouseDevice->getInputDevice());
@@ -1735,11 +1760,6 @@ Application::Application(int& argc, char** argv, QElapsedTimer& startupTimer, bo
     QTimer* settingsTimer = new QTimer();
     moveToNewNamedThread(settingsTimer, "Settings Thread", [this, settingsTimer]{
         connect(qApp, &Application::beforeAboutToQuit, [this, settingsTimer]{
-            bool autoLogout = Setting::Handle<bool>(AUTO_LOGOUT_SETTING_NAME, false).get();
-            if (autoLogout) {
-                auto accountManager = DependencyManager::get<AccountManager>();
-                accountManager->logout();
-            }
             // Disconnect the signal from the save settings
             QObject::disconnect(settingsTimer, &QTimer::timeout, this, &Application::saveSettings);
             // Stop the settings timer
@@ -1841,6 +1861,10 @@ Application::Application(int& argc, char** argv, QElapsedTimer& startupTimer, bo
     });
 
     EntityTree::setAddMaterialToEntityOperator([this](const QUuid& entityID, graphics::MaterialLayer material, const std::string& parentMaterialName) {
+        if (_aboutToQuit) {
+            return false;
+        }
+
         // try to find the renderable
         auto renderable = getEntities()->renderableForEntityId(entityID);
         if (renderable) {
@@ -1856,6 +1880,10 @@ Application::Application(int& argc, char** argv, QElapsedTimer& startupTimer, bo
         return false;
     });
     EntityTree::setRemoveMaterialFromEntityOperator([this](const QUuid& entityID, graphics::MaterialPointer material, const std::string& parentMaterialName) {
+        if (_aboutToQuit) {
+            return false;
+        }
+
         // try to find the renderable
         auto renderable = getEntities()->renderableForEntityId(entityID);
         if (renderable) {
@@ -2483,6 +2511,11 @@ void Application::cleanupBeforeQuit() {
     }
     DependencyManager::destroy<ScriptEngines>();
 
+    bool autoLogout = Setting::Handle<bool>(AUTO_LOGOUT_SETTING_NAME, false).get();
+    if (autoLogout) {
+        DependencyManager::get<AccountManager>()->removeAccountFromFile();
+    }
+
     _displayPlugin.reset();
     PluginManager::getInstance()->shutdown();
 
@@ -5787,15 +5820,13 @@ void Application::update(float deltaTime) {
                     auto t5 = std::chrono::high_resolution_clock::now();
 
                     workload::Timings timings(6);
-                    timings[0] = (t4 - t0);
-                    timings[1] = (t5 - t4);
-                    timings[2] = (t4 - t3);
-                    timings[3] = (t3 - t2);
-                    timings[4] = (t2 - t1);
-                    timings[5] = (t1 - t0);
-
+                    timings[0] = t1 - t0; // prePhysics entities
+                    timings[1] = t2 - t1; // prePhysics avatars
+                    timings[2] = t3 - t2; // stepPhysics
+                    timings[3] = t4 - t3; // postPhysics
+                    timings[4] = t5 - t4; // non-physical kinematics
+                    timings[5] = workload::Timing_ns((int32_t)(NSECS_PER_SECOND * deltaTime)); // game loop duration
                     _gameWorkload.updateSimulationTimings(timings);
-
                 }
             }
         } else {

interface/src/Application.h

@@ -312,6 +312,9 @@ public:
 
     Q_INVOKABLE void copyToClipboard(const QString& text);
 
+    int getOtherAvatarsReplicaCount() { return DependencyManager::get<AvatarHashMap>()->getReplicaCount(); }
+    void setOtherAvatarsReplicaCount(int count) { DependencyManager::get<AvatarHashMap>()->setReplicaCount(count); }
+
 #if defined(Q_OS_ANDROID)
     void beforeEnterBackground();
     void enterBackground();

interface/src/AvatarBookmarks.cpp

@@ -145,20 +145,9 @@ void AvatarBookmarks::removeBookmark(const QString& bookmarkName) {
     emit bookmarkDeleted(bookmarkName);
 }
 
-bool isWearableEntity(const EntityItemPointer& entity) {
-    return entity->isVisible() && (entity->getParentJointIndex() != INVALID_JOINT_INDEX || (entity->getType() == EntityTypes::Model && (std::static_pointer_cast<ModelEntityItem>(entity))->getRelayParentJoints()))
-        && (entity->getParentID() == DependencyManager::get<NodeList>()->getSessionUUID() || entity->getParentID() == DependencyManager::get<AvatarManager>()->getMyAvatar()->getSelfID());
-}
-
 void AvatarBookmarks::updateAvatarEntities(const QVariantList &avatarEntities) {
     auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
-    auto treeRenderer = DependencyManager::get<EntityTreeRenderer>();
-    EntityTreePointer entityTree = treeRenderer ? treeRenderer->getTree() : nullptr;
-    myAvatar->removeAvatarEntities([&](const QUuid& entityID) {
-        auto entity = entityTree->findEntityByID(entityID);
-        return entity && isWearableEntity(entity);
-    });
-
+    myAvatar->removeWearableAvatarEntities();
     addAvatarEntities(avatarEntities);
 }
 
@@ -183,10 +172,7 @@ void AvatarBookmarks::loadBookmark(const QString& bookmarkName) {
             auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
             auto treeRenderer = DependencyManager::get<EntityTreeRenderer>();
             EntityTreePointer entityTree = treeRenderer ? treeRenderer->getTree() : nullptr;
-            myAvatar->removeAvatarEntities([&](const QUuid& entityID) {
-                auto entity = entityTree->findEntityByID(entityID);
-                return entity && isWearableEntity(entity);
-            });
+            myAvatar->removeWearableAvatarEntities();
             const QString& avatarUrl = bookmark.value(ENTRY_AVATAR_URL, "").toString();
             myAvatar->useFullAvatarURL(avatarUrl);
             qCDebug(interfaceapp) << "Avatar On " << avatarUrl;

interface/src/Menu.cpp

@@ -46,6 +46,7 @@
 #include "InterfaceLogging.h"
 #include "LocationBookmarks.h"
 #include "DeferredLightingEffect.h"
+#include "PickManager.h"
 
 #include "AmbientOcclusionEffect.h"
 #include "RenderShadowTask.h"
@@ -691,6 +692,11 @@ Menu::Menu() {
     addCheckableActionToQMenuAndActionHash(physicsOptionsMenu, MenuOption::PhysicsShowBulletConstraints, 0, false, qApp, SLOT(setShowBulletConstraints(bool)));
     addCheckableActionToQMenuAndActionHash(physicsOptionsMenu, MenuOption::PhysicsShowBulletConstraintLimits, 0, false, qApp, SLOT(setShowBulletConstraintLimits(bool)));
 
+    // Developer > Picking >>>
+    MenuWrapper* pickingOptionsMenu = developerMenu->addMenu("Picking");
+    addCheckableActionToQMenuAndActionHash(pickingOptionsMenu, MenuOption::ForceCoarsePicking, 0, false,
+        DependencyManager::get<PickManager>().data(), SLOT(setForceCoarsePicking(bool)));
+
     // Developer > Display Crash Options
     addCheckableActionToQMenuAndActionHash(developerMenu, MenuOption::DisplayCrashOptions, 0, true);
     // Developer > Crash >>>

interface/src/Menu.h

@@ -221,6 +221,7 @@ namespace MenuOption {
     const QString NotificationSounds = "play_notification_sounds";
     const QString NotificationSoundsSnapshot = "play_notification_sounds_snapshot";
     const QString NotificationSoundsTablet = "play_notification_sounds_tablet";
+    const QString ForceCoarsePicking = "Force Coarse Picking";
     const QString ComputeBlendshapes = "Compute Blendshapes";
 }
 

interface/src/avatar/AvatarManager.cpp

@@ -583,8 +583,14 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
         return result;
     }
 
-    glm::vec3 normDirection = glm::normalize(ray.direction);
+    // It's better to intersect the ray against the avatar's actual mesh, but this is currently difficult to
+    // do, because the transformed mesh data only exists over in GPU-land.  As a compromise, this code
+    // intersects against the avatars capsule and then against the (T-pose) mesh.  The end effect is that picking
+    // against the avatar is sort-of right, but you likely wont be able to pick against the arms.
 
+    // TODO -- find a way to extract transformed avatar mesh data from the rendering engine.
+
+    std::vector<SortedAvatar> sortedAvatars;
     auto avatarHashCopy = getHashCopy();
     for (auto avatarData : avatarHashCopy) {
         auto avatar = std::static_pointer_cast<Avatar>(avatarData);
@@ -593,52 +599,65 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
             continue;
         }
 
-        float distance;
-        BoxFace face;
-        glm::vec3 surfaceNormal;
-
-        SkeletonModelPointer avatarModel = avatar->getSkeletonModel();
-
-        // It's better to intersect the ray against the avatar's actual mesh, but this is currently difficult to
-        // do, because the transformed mesh data only exists over in GPU-land.  As a compromise, this code
-        // intersects against the avatars capsule and then against the (T-pose) mesh.  The end effect is that picking
-        // against the avatar is sort-of right, but you likely wont be able to pick against the arms.
-
-        // TODO -- find a way to extract transformed avatar mesh data from the rendering engine.
-
+        float distance = FLT_MAX;
+#if 0
         // if we weren't picking against the capsule, we would want to pick against the avatarBounds...
-        // AABox avatarBounds = avatarModel->getRenderableMeshBound();
-        // if (!avatarBounds.findRayIntersection(ray.origin, normDirection, distance, face, surfaceNormal)) {
-        //     // ray doesn't intersect avatar's bounding-box
-        //     continue;
-        // }
-
+        SkeletonModelPointer avatarModel = avatar->getSkeletonModel();
+        AABox avatarBounds = avatarModel->getRenderableMeshBound();
+        if (avatarBounds.contains(ray.origin)) {
+            distance = 0.0f;
+        } else {
+            float boundDistance = FLT_MAX;
+            BoxFace face;
+            glm::vec3 surfaceNormal;
+            if (avatarBounds.findRayIntersection(ray.origin, ray.direction, boundDistance, face, surfaceNormal)) {
+                distance = boundDistance;
+            }
+        }
+#else
         glm::vec3 start;
         glm::vec3 end;
         float radius;
         avatar->getCapsule(start, end, radius);
-        bool intersects = findRayCapsuleIntersection(ray.origin, normDirection, start, end, radius, distance);
-        if (!intersects) {
-            // ray doesn't intersect avatar's capsule
-            continue;
+        findRayCapsuleIntersection(ray.origin, ray.direction, start, end, radius, distance);
+#endif
+
+        if (distance < FLT_MAX) {
+            sortedAvatars.emplace_back(distance, avatar);
+        }
+    }
+
+    if (sortedAvatars.size() > 1) {
+        static auto comparator = [](const SortedAvatar& left, const SortedAvatar& right) { return left.first < right.first; };
+        std::sort(sortedAvatars.begin(), sortedAvatars.end(), comparator);
+    }
+
+    for (auto it = sortedAvatars.begin(); it != sortedAvatars.end(); ++it) {
+        const SortedAvatar& sortedAvatar = *it;
+        // We can exit once avatarCapsuleDistance > bestDistance
+        if (sortedAvatar.first > result.distance) {
+            break;
         }
 
+        float distance = FLT_MAX;
+        BoxFace face;
+        glm::vec3 surfaceNormal;
         QVariantMap extraInfo;
-        intersects = avatarModel->findRayIntersectionAgainstSubMeshes(ray.origin, normDirection,
-                                                                      distance, face, surfaceNormal, extraInfo, true);
-
-        if (intersects && (!result.intersects || distance < result.distance)) {
-            result.intersects = true;
-            result.avatarID = avatar->getID();
-            result.distance = distance;
-            result.face = face;
-            result.surfaceNormal = surfaceNormal;
-            result.extraInfo = extraInfo;
+        SkeletonModelPointer avatarModel = sortedAvatar.second->getSkeletonModel();
+        if (avatarModel->findRayIntersectionAgainstSubMeshes(ray.origin, ray.direction, distance, face, surfaceNormal, extraInfo, true)) {
+            if (distance < result.distance) {
+                result.intersects = true;
+                result.avatarID = sortedAvatar.second->getID();
+                result.distance = distance;
+                result.face = face;
+                result.surfaceNormal = surfaceNormal;
+                result.extraInfo = extraInfo;
+            }
         }
     }
 
     if (result.intersects) {
-        result.intersection = ray.origin + normDirection * result.distance;
+        result.intersection = ray.origin + ray.direction * result.distance;
     }
 
     return result;
@@ -657,6 +676,14 @@ ParabolaToAvatarIntersectionResult AvatarManager::findParabolaIntersectionVector
         return result;
     }
 
+    // It's better to intersect the ray against the avatar's actual mesh, but this is currently difficult to
+    // do, because the transformed mesh data only exists over in GPU-land.  As a compromise, this code
+    // intersects against the avatars capsule and then against the (T-pose) mesh.  The end effect is that picking
+    // against the avatar is sort-of right, but you likely wont be able to pick against the arms.
+
+    // TODO -- find a way to extract transformed avatar mesh data from the rendering engine.
+
+    std::vector<SortedAvatar> sortedAvatars;
     auto avatarHashCopy = getHashCopy();
     for (auto avatarData : avatarHashCopy) {
         auto avatar = std::static_pointer_cast<Avatar>(avatarData);
@@ -665,47 +692,60 @@ ParabolaToAvatarIntersectionResult AvatarManager::findParabolaIntersectionVector
             continue;
         }
 
-        float parabolicDistance;
-        BoxFace face;
-        glm::vec3 surfaceNormal;
-
-        SkeletonModelPointer avatarModel = avatar->getSkeletonModel();
-
-        // It's better to intersect the parabola against the avatar's actual mesh, but this is currently difficult to
-        // do, because the transformed mesh data only exists over in GPU-land.  As a compromise, this code
-        // intersects against the avatars capsule and then against the (T-pose) mesh.  The end effect is that picking
-        // against the avatar is sort-of right, but you likely wont be able to pick against the arms.
-
-        // TODO -- find a way to extract transformed avatar mesh data from the rendering engine.
-
+        float distance = FLT_MAX;
+#if 0
         // if we weren't picking against the capsule, we would want to pick against the avatarBounds...
-        // AABox avatarBounds = avatarModel->getRenderableMeshBound();
-        // if (!avatarBounds.findParabolaIntersection(pick.origin, pick.velocity, pick.acceleration, parabolicDistance, face, surfaceNormal)) {
-        //     // parabola doesn't intersect avatar's bounding-box
-        //     continue;
-        // }
-
+        SkeletonModelPointer avatarModel = avatar->getSkeletonModel();
+        AABox avatarBounds = avatarModel->getRenderableMeshBound();
+        if (avatarBounds.contains(pick.origin)) {
+            distance = 0.0f;
+        } else {
+            float boundDistance = FLT_MAX;
+            BoxFace face;
+            glm::vec3 surfaceNormal;
+            if (avatarBounds.findParabolaIntersection(pick.origin, pick.velocity, pick.acceleration, boundDistance, face, surfaceNormal)) {
+                distance = boundDistance;
+            }
+        }
+#else
         glm::vec3 start;
         glm::vec3 end;
         float radius;
         avatar->getCapsule(start, end, radius);
-        bool intersects = findParabolaCapsuleIntersection(pick.origin, pick.velocity, pick.acceleration, start, end, radius, avatar->getWorldOrientation(), parabolicDistance);
-        if (!intersects) {
-            // ray doesn't intersect avatar's capsule
-            continue;
+        findParabolaCapsuleIntersection(pick.origin, pick.velocity, pick.acceleration, start, end, radius, avatar->getWorldOrientation(), distance);
+#endif
+
+        if (distance < FLT_MAX) {
+            sortedAvatars.emplace_back(distance, avatar);
+        }
+    }
+
+    if (sortedAvatars.size() > 1) {
+        static auto comparator = [](const SortedAvatar& left, const SortedAvatar& right) { return left.first < right.first; };
+        std::sort(sortedAvatars.begin(), sortedAvatars.end(), comparator);
+    }
+
+    for (auto it = sortedAvatars.begin(); it != sortedAvatars.end(); ++it) {
+        const SortedAvatar& sortedAvatar = *it;
+        // We can exit once avatarCapsuleDistance > bestDistance
+        if (sortedAvatar.first > result.parabolicDistance) {
+            break;
         }
 
+        float parabolicDistance = FLT_MAX;
+        BoxFace face;
+        glm::vec3 surfaceNormal;
         QVariantMap extraInfo;
-        intersects = avatarModel->findParabolaIntersectionAgainstSubMeshes(pick.origin, pick.velocity, pick.acceleration,
-                                                                           parabolicDistance, face, surfaceNormal, extraInfo, true);
-
-        if (intersects && (!result.intersects || parabolicDistance < result.parabolicDistance)) {
-            result.intersects = true;
-            result.avatarID = avatar->getID();
-            result.parabolicDistance = parabolicDistance;
-            result.face = face;
-            result.surfaceNormal = surfaceNormal;
-            result.extraInfo = extraInfo;
+        SkeletonModelPointer avatarModel = sortedAvatar.second->getSkeletonModel();
+        if (avatarModel->findParabolaIntersectionAgainstSubMeshes(pick.origin, pick.velocity, pick.acceleration, parabolicDistance, face, surfaceNormal, extraInfo, true)) {
+            if (parabolicDistance < result.parabolicDistance) {
+                result.intersects = true;
+                result.avatarID = sortedAvatar.second->getID();
+                result.parabolicDistance = parabolicDistance;
+                result.face = face;
+                result.surfaceNormal = surfaceNormal;
+                result.extraInfo = extraInfo;
+            }
         }
     }
 

interface/src/avatar/AvatarManager.h

@@ -31,6 +31,8 @@
 #include "MyAvatar.h"
 #include "OtherAvatar.h"
 
+using SortedAvatar = std::pair<float, std::shared_ptr<Avatar>>;
+
 /**jsdoc 
  * The AvatarManager API has properties and methods which manage Avatars within the same domain.
  *

interface/src/avatar/MyAvatar.cpp

@@ -91,8 +91,6 @@ const float MIN_SCALE_CHANGED_DELTA = 0.001f;
 const int MODE_READINGS_RING_BUFFER_SIZE = 500;
 const float CENTIMETERS_PER_METER = 100.0f;
 
-//#define DEBUG_DRAW_HMD_MOVING_AVERAGE
-
 MyAvatar::MyAvatar(QThread* thread) :
     Avatar(thread),
     _yawSpeed(YAW_SPEED_DEFAULT),
@@ -447,9 +445,27 @@ void MyAvatar::reset(bool andRecenter, bool andReload, bool andHead) {
 void MyAvatar::update(float deltaTime) {
     // update moving average of HMD facing in xz plane.
     const float HMD_FACING_TIMESCALE = getRotationRecenterFilterLength();
+    const float PERCENTAGE_WEIGHT_HEAD_VS_SHOULDERS_AZIMUTH = 0.0f; // 100 percent shoulders
 
     float tau = deltaTime / HMD_FACING_TIMESCALE;
-    _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, _headControllerFacing, tau);
+    setHipToHandController(computeHandAzimuth());
+
+    // put the average hand azimuth into sensor space.
+    // then mix it with head facing direction to determine rotation recenter
+    if (getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() && getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid()) {
+        glm::vec3 handHipAzimuthWorldSpace = transformVectorFast(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y));
+        glm::mat4 sensorToWorldMat = getSensorToWorldMatrix();
+        glm::mat4 worldToSensorMat = glm::inverse(sensorToWorldMat);
+        glm::vec3 handHipAzimuthSensorSpace = transformVectorFast(worldToSensorMat, handHipAzimuthWorldSpace);
+        glm::vec2 normedHandHipAzimuthSensorSpace(0.0f, 1.0f);
+        if (glm::length(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z)) > 0.0f) {
+            normedHandHipAzimuthSensorSpace = glm::normalize(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z));
+        }
+        glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHipAzimuthSensorSpace, _headControllerFacing, PERCENTAGE_WEIGHT_HEAD_VS_SHOULDERS_AZIMUTH);
+        _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, headFacingPlusHandHipAzimuthMix, tau);
+    } else {
+        _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, _headControllerFacing, tau);
+    }
 
     if (_smoothOrientationTimer < SMOOTH_TIME_ORIENTATION) {
         _rotationChanged = usecTimestampNow();
@@ -462,19 +478,23 @@ void MyAvatar::update(float deltaTime) {
     setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD)));
     setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD)));
 
-#ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE
-    auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
-    glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation());
-    glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y));
-    glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y));
-    DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f));
-    DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f));
-#endif
+   if (_drawAverageFacingEnabled) {
+        auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
+        glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation());
+        glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y));
+        glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y));
+        DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f));
+        DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f));
+
+        // draw hand azimuth vector
+        glm::vec3 handAzimuthMidpoint = transformPoint(getTransform().getMatrix(), glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y));
+        DebugDraw::getInstance().drawRay(getWorldPosition(), handAzimuthMidpoint, glm::vec4(0.0f, 1.0f, 1.0f, 1.0f));
+    }
 
     if (_goToPending) {
         setWorldPosition(_goToPosition);
         setWorldOrientation(_goToOrientation);
-        _headControllerFacingMovingAverage = _headControllerFacing;  // reset moving average
+        _headControllerFacingMovingAverage = _headControllerFacing; // reset moving average
         _goToPending = false;
         // updateFromHMDSensorMatrix (called from paintGL) expects that the sensorToWorldMatrix is updated for any position changes
         // that happen between render and Application::update (which calls updateSensorToWorldMatrix to do so).
@@ -796,6 +816,47 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
     }
 }
 
+// Find the vector halfway between the hip to hand azimuth vectors
+// This midpoint hand azimuth is in Avatar space
+glm::vec2 MyAvatar::computeHandAzimuth() const {
+    controller::Pose leftHandPoseAvatarSpace = getLeftHandPose();
+    controller::Pose rightHandPoseAvatarSpace = getRightHandPose();
+    controller::Pose headPoseAvatarSpace = getControllerPoseInAvatarFrame(controller::Action::HEAD);
+    const float HALFWAY = 0.50f;
+    glm::vec2 latestHipToHandController = _hipToHandController;
+
+    if (leftHandPoseAvatarSpace.isValid() && rightHandPoseAvatarSpace.isValid() && headPoseAvatarSpace.isValid()) {
+        // we need the old azimuth reading to prevent flipping the facing direction 180
+        // in the case where the hands go from being slightly less than 180 apart to slightly more than 180 apart.
+        glm::vec2 oldAzimuthReading = _hipToHandController;
+        if ((glm::length(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)) > 0.0f) && (glm::length(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)) > 0.0f)) {
+            latestHipToHandController = lerp(glm::normalize(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)), glm::normalize(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)), HALFWAY);
+        } else {
+            latestHipToHandController = glm::vec2(0.0f, -1.0f);
+        }
+
+        glm::vec3 headLookAtAvatarSpace = transformVectorFast(headPoseAvatarSpace.getMatrix(), glm::vec3(0.0f, 0.0f, 1.0f));
+        glm::vec2 headAzimuthAvatarSpace = glm::vec2(headLookAtAvatarSpace.x, headLookAtAvatarSpace.z);
+        if (glm::length(headAzimuthAvatarSpace) > 0.0f) {
+            headAzimuthAvatarSpace = glm::normalize(headAzimuthAvatarSpace);
+        } else {
+            headAzimuthAvatarSpace = -latestHipToHandController;
+        }
+
+        // check the angular distance from forward and back
+        float cosForwardAngle = glm::dot(latestHipToHandController, oldAzimuthReading);
+        float cosHeadShoulder = glm::dot(-latestHipToHandController, headAzimuthAvatarSpace);
+        // if we are now closer to the 180 flip of the previous chest forward
+        // then we negate our computed latestHipToHandController to keep the chest from flipping.
+        // also check the head to shoulder azimuth difference if we negate.
+        // don't negate the chest azimuth if this is greater than 100 degrees.
+        if ((cosForwardAngle < 0.0f) && !(cosHeadShoulder < -0.2f)) {
+            latestHipToHandController = -latestHipToHandController;
+        }
+    }
+    return latestHipToHandController;
+}
+
 void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeValueCache<glm::mat4>& matrixCache) {
     assert(QThread::currentThread() == thread());
     auto userInputMapper = DependencyManager::get<UserInputMapper>();
@@ -1703,18 +1764,50 @@ void MyAvatar::setSkeletonModelURL(const QUrl& skeletonModelURL) {
     emit skeletonChanged();
 }
 
-void MyAvatar::removeAvatarEntities(const std::function<bool(const QUuid& entityID)>& condition) {
+bool isWearableEntity(const EntityItemPointer& entity) {
+    return entity->isVisible()
+        && (entity->getParentJointIndex() != INVALID_JOINT_INDEX
+            || (entity->getType() == EntityTypes::Model && (std::static_pointer_cast<ModelEntityItem>(entity))->getRelayParentJoints()))
+        && (entity->getParentID() == DependencyManager::get<NodeList>()->getSessionUUID()
+            || entity->getParentID() == AVATAR_SELF_ID);
+}
+
+void MyAvatar::clearAvatarEntities() {
     auto treeRenderer = DependencyManager::get<EntityTreeRenderer>();
     EntityTreePointer entityTree = treeRenderer ? treeRenderer->getTree() : nullptr;
-    if (entityTree) {
-        entityTree->withWriteLock([&] {
-            AvatarEntityMap avatarEntities = getAvatarEntityData();
-            for (auto entityID : avatarEntities.keys()) {
-                if (!condition || condition(entityID)) {
-                    entityTree->deleteEntity(entityID, true, true);
-                }
-            }
+
+    AvatarEntityMap avatarEntities = getAvatarEntityData();
+    for (auto entityID : avatarEntities.keys()) {
+        entityTree->withWriteLock([&entityID, &entityTree] {
+            // remove this entity first from the entity tree
+            entityTree->deleteEntity(entityID, true, true);
         });
+
+        // remove the avatar entity from our internal list
+        // (but indicate it doesn't need to be pulled from the tree)
+        clearAvatarEntity(entityID, false);
+    }
+}
+
+void MyAvatar::removeWearableAvatarEntities() {
+    auto treeRenderer = DependencyManager::get<EntityTreeRenderer>();
+    EntityTreePointer entityTree = treeRenderer ? treeRenderer->getTree() : nullptr;
+    
+    if (entityTree) {
+        AvatarEntityMap avatarEntities = getAvatarEntityData();
+        for (auto entityID : avatarEntities.keys()) {
+            auto entity = entityTree->findEntityByID(entityID);
+            if (entity && isWearableEntity(entity)) {
+                entityTree->withWriteLock([&entityID, &entityTree] {
+                    // remove this entity first from the entity tree
+                    entityTree->deleteEntity(entityID, true, true);
+                });
+
+                // remove the avatar entity from our internal list
+                // (but indicate it doesn't need to be pulled from the tree)
+                clearAvatarEntity(entityID, false);
+            }
+        }
     }
 }
 
@@ -2116,7 +2209,7 @@ void MyAvatar::setAttachmentData(const QVector<AttachmentData>& attachmentData)
     }
 
     // clear any existing avatar entities
-    setAvatarEntityData(AvatarEntityMap());
+    clearAvatarEntities();
 
     for (auto& properties : newEntitiesProperties) {
         DependencyManager::get<EntityScriptingInterface>()->addEntity(properties, true);
@@ -2212,7 +2305,11 @@ void MyAvatar::initHeadBones() {
         neckJointIndex = _skeletonModel->getFBXGeometry().neckJointIndex;
     }
     if (neckJointIndex == -1) {
-        return;
+        neckJointIndex = (_skeletonModel->getFBXGeometry().headJointIndex - 1);
+        if (neckJointIndex < 0) {
+            // return if the head is not even there. can't cauterize!!
+            return;
+        }
     }
     _headBoneSet.clear();
     std::queue<int> q;
@@ -3321,6 +3418,24 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
     return createMatFromQuatAndPos(headOrientationYawOnly, bodyPos);
 }
 
+glm::mat4 MyAvatar::getSpine2RotationRigSpace() const {
+
+    // static const glm::quat RIG_CHANGE_OF_BASIS = Quaternions::Y_180;
+    // RIG_CHANGE_OF_BASIS * AVATAR_TO_RIG_ROTATION * inverse(RIG_CHANGE_OF_BASIS) = Quaternions::Y_180; //avatar Space;
+    const glm::quat AVATAR_TO_RIG_ROTATION = Quaternions::Y_180;
+    glm::vec3 hipToHandRigSpace = AVATAR_TO_RIG_ROTATION * glm::vec3(_hipToHandController.x, 0.0f, _hipToHandController.y);
+
+    glm::vec3 u, v, w;
+    if (glm::length(hipToHandRigSpace) > 0.0f) {
+        hipToHandRigSpace = glm::normalize(hipToHandRigSpace);
+    } else {
+        hipToHandRigSpace = glm::vec3(0.0f, 0.0f, 1.0f);
+    }
+    generateBasisVectors(glm::vec3(0.0f,1.0f,0.0f), hipToHandRigSpace, u, v, w);
+    glm::mat4 spine2RigSpace(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f));
+    return spine2RigSpace;
+}
+
 // ease in function for dampening cg movement
 static float slope(float num) {
     const float CURVE_CONSTANT = 1.0f;
@@ -3904,7 +4019,7 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontalCG(MyAvatar& myAvatar) cons
         glm::vec3 currentHeadPosition = currentHeadPose.getTranslation();
         float anatomicalHeadToHipsDistance = glm::length(defaultHeadPosition - defaultHipsPosition);
         if (!isActive(Horizontal) &&
-            (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + DEFAULT_AVATAR_SPINE_STRETCH_LIMIT))) {
+            (glm::length(currentHeadPosition - defaultHipsPosition) > (anatomicalHeadToHipsDistance + (DEFAULT_AVATAR_SPINE_STRETCH_LIMIT * anatomicalHeadToHipsDistance)))) {
             myAvatar.setResetMode(true);
             stepDetected = true;
         }
@@ -3928,25 +4043,32 @@ void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat
         qApp->getCamera().getMode() != CAMERA_MODE_MIRROR) {
         if (!isActive(Rotation) && (shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) {
             activate(Rotation);
-            myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing);
+            myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing());
         }
         if (myAvatar.getCenterOfGravityModelEnabled()) {
             if (!isActive(Horizontal) && (shouldActivateHorizontalCG(myAvatar) || hasDriveInput)) {
                 activate(Horizontal);
+                if (myAvatar.getEnableStepResetRotation()) {
+                    activate(Rotation);
+                    myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing());
+                }
             }
         } else {
             if (!isActive(Horizontal) && (shouldActivateHorizontal(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) {
                 activate(Horizontal);
+                if (myAvatar.getEnableStepResetRotation()) {
+                    activate(Rotation);
+                    myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing());
+                }
             }
         }
-
         if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) {
             activate(Vertical);
         }
     } else {
         if (!isActive(Rotation) && getForceActivateRotation()) {
             activate(Rotation);
-            myAvatar.setHeadControllerFacingMovingAverage(myAvatar._headControllerFacing);
+            myAvatar.setHeadControllerFacingMovingAverage(myAvatar.getHeadControllerFacing());
             setForceActivateRotation(false);
         }
         if (!isActive(Horizontal) && getForceActivateHorizontal()) {

interface/src/avatar/MyAvatar.h

@@ -201,6 +201,8 @@ class MyAvatar : public Avatar {
     Q_PROPERTY(bool hasAudioEnabledFaceMovement READ getHasAudioEnabledFaceMovement WRITE setHasAudioEnabledFaceMovement)
     Q_PROPERTY(float rotationRecenterFilterLength READ getRotationRecenterFilterLength WRITE setRotationRecenterFilterLength)
     Q_PROPERTY(float rotationThreshold READ getRotationThreshold WRITE setRotationThreshold)
+    Q_PROPERTY(bool enableStepResetRotation READ getEnableStepResetRotation WRITE setEnableStepResetRotation)
+    Q_PROPERTY(bool enableDrawAverageFacing READ getEnableDrawAverageFacing WRITE setEnableDrawAverageFacing)
     //TODO: make gravity feature work Q_PROPERTY(glm::vec3 gravity READ getGravity WRITE setGravity)
 
     Q_PROPERTY(glm::vec3 leftHandPosition READ getLeftHandPosition)
@@ -318,6 +320,9 @@ public:
     // as it moves through the world.
     void updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix);
 
+    // compute the hip to hand average azimuth.
+    glm::vec2 computeHandAzimuth() const;
+
     // read the location of a hand controller and save the transform
     void updateJointFromController(controller::Action poseKey, ThreadSafeValueCache<glm::mat4>& matrixCache);
 
@@ -909,6 +914,10 @@ public:
 
     virtual void rebuildCollisionShape() override;
 
+    const glm::vec2& getHipToHandController() const { return _hipToHandController; }
+    void setHipToHandController(glm::vec2 currentHipToHandController) { _hipToHandController = currentHipToHandController; }
+    const glm::vec2& getHeadControllerFacing() const { return _headControllerFacing; }
+    void setHeadControllerFacing(glm::vec2 currentHeadControllerFacing) { _headControllerFacing = currentHeadControllerFacing; }
     const glm::vec2& getHeadControllerFacingMovingAverage() const { return _headControllerFacingMovingAverage; }
     void setHeadControllerFacingMovingAverage(glm::vec2 currentHeadControllerFacing) { _headControllerFacingMovingAverage = currentHeadControllerFacing; }
     float getCurrentStandingHeight() const { return _currentStandingHeight; }
@@ -931,7 +940,8 @@ public:
     * @returns {object[]}
     */
     Q_INVOKABLE QVariantList getAvatarEntitiesVariant();
-    void removeAvatarEntities(const std::function<bool(const QUuid& entityID)>& condition = {});
+    void clearAvatarEntities();
+    void removeWearableAvatarEntities();
 
     /**jsdoc
      * @function MyAvatar.isFlying
@@ -1045,6 +1055,8 @@ public:
     // results are in sensor frame (-z forward)
     glm::mat4 deriveBodyFromHMDSensor() const;
 
+    glm::mat4 getSpine2RotationRigSpace() const;
+
     glm::vec3 computeCounterBalance();
 
     // derive avatar body position and orientation from using the current HMD Sensor location in relation to the previous
@@ -1515,6 +1527,10 @@ private:
     float getRotationRecenterFilterLength() const { return _rotationRecenterFilterLength; }
     void setRotationThreshold(float angleRadians);
     float getRotationThreshold() const { return _rotationThreshold; }
+    void setEnableStepResetRotation(bool stepReset) { _stepResetRotationEnabled = stepReset; }
+    bool getEnableStepResetRotation() const { return _stepResetRotationEnabled; }
+    void setEnableDrawAverageFacing(bool drawAverage) { _drawAverageFacingEnabled = drawAverage; }
+    bool getEnableDrawAverageFacing() const { return _drawAverageFacingEnabled; }
     bool isMyAvatar() const override { return true; }
     virtual int parseDataFromBuffer(const QByteArray& buffer) override;
     virtual glm::vec3 getSkeletonPosition() const override;
@@ -1637,6 +1653,8 @@ private:
     std::atomic<bool> _hasScriptedBlendShapes { false };
     std::atomic<float> _rotationRecenterFilterLength { 4.0f };
     std::atomic<float> _rotationThreshold { 0.5235f };  // 30 degrees in radians
+    std::atomic<bool> _stepResetRotationEnabled { true };
+    std::atomic<bool> _drawAverageFacingEnabled { false };
 
     // working copy -- see AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access
     glm::mat4 _sensorToWorldMatrix { glm::mat4() };
@@ -1650,6 +1668,8 @@ private:
     glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f };   // facing vector in xz plane (sensor space)
     glm::quat _averageHeadRotation { 0.0f, 0.0f, 0.0f, 1.0f };
 
+    glm::vec2 _hipToHandController { 0.0f, -1.0f };  // spine2 facing vector in xz plane (avatar space)
+
     float _currentStandingHeight { 0.0f };
     bool _resetMode { true };
     RingBufferHistory<int> _recentModeReadings;
@@ -1782,4 +1802,6 @@ void audioListenModeFromScriptValue(const QScriptValue& object, AudioListenerMod
 QScriptValue driveKeysToScriptValue(QScriptEngine* engine, const MyAvatar::DriveKeys& driveKeys);
 void driveKeysFromScriptValue(const QScriptValue& object, MyAvatar::DriveKeys& driveKeys);
 
+bool isWearableEntity(const EntityItemPointer& entity);
+
 #endif // hifi_MyAvatar_h

interface/src/avatar/MyAvatarHeadTransformNode.cpp

@@ -0,0 +1,23 @@
+//
+//  Created by Sabrina Shanman 8/14/2018
+//  Copyright 2018 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "MyAvatarHeadTransformNode.h"
+
+#include "DependencyManager.h"
+#include "AvatarManager.h"
+#include "MyAvatar.h"
+
+Transform MyAvatarHeadTransformNode::getTransform() {
+    auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
+
+    glm::vec3 pos = myAvatar->getHeadPosition();
+    glm::quat headOri = myAvatar->getHeadOrientation();
+    glm::quat ori = headOri * glm::angleAxis(-PI / 2.0f, Vectors::RIGHT);
+
+    return Transform(ori, glm::vec3(1.0f), pos);
+}

interface/src/avatar/MyAvatarHeadTransformNode.h

@@ -0,0 +1,19 @@
+//
+//  Created by Sabrina Shanman 8/14/2018
+//  Copyright 2018 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_MyAvatarHeadTransformNode_h
+#define hifi_MyAvatarHeadTransformNode_h
+
+#include "TransformNode.h"
+
+class MyAvatarHeadTransformNode : public TransformNode {
+public:
+    MyAvatarHeadTransformNode() { }
+    Transform getTransform() override;
+};
+
+#endif // hifi_MyAvatarHeadTransformNode_h

interface/src/avatar/MySkeletonModel.cpp

@@ -238,6 +238,35 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
         params.primaryControllerPoses[Rig::PrimaryControllerType_Hips] = sensorToRigPose * hips;
         params.primaryControllerFlags[Rig::PrimaryControllerType_Hips] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated;
 
+        // set spine2 if we have hand controllers
+        if (myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid() &&
+                myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() &&
+                !(params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] & (uint8_t)Rig::ControllerFlags::Enabled)) {
+
+            AnimPose currentSpine2Pose;
+            AnimPose currentHeadPose;
+            AnimPose currentHipsPose;
+            bool spine2Exists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Spine2"), currentSpine2Pose);
+            bool headExists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Head"), currentHeadPose);
+            bool hipsExists = _rig.getAbsoluteJointPoseInRigFrame(_rig.indexOfJoint("Hips"), currentHipsPose);
+            if (spine2Exists && headExists && hipsExists) {
+                AnimPose rigSpaceYaw(myAvatar->getSpine2RotationRigSpace());
+                glm::vec3 u, v, w;
+                glm::vec3 fwd = rigSpaceYaw.rot() * glm::vec3(0.0f, 0.0f, 1.0f);
+                glm::vec3 up = currentHeadPose.trans() - currentHipsPose.trans();
+                if (glm::length(up) > 0.0f) {
+                    up = glm::normalize(up);
+                } else {
+                    up = glm::vec3(0.0f, 1.0f, 0.0f);
+                }
+                generateBasisVectors(up, fwd, u, v, w);
+                AnimPose newSpinePose(glm::mat4(glm::vec4(w, 0.0f), glm::vec4(u, 0.0f), glm::vec4(v, 0.0f), glm::vec4(glm::vec3(0.0f, 0.0f, 0.0f), 1.0f)));
+                currentSpine2Pose.rot() = newSpinePose.rot();
+                params.primaryControllerPoses[Rig::PrimaryControllerType_Spine2] = currentSpine2Pose;
+                params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated;
+            }
+        }
+
     } else {
         _prevHipsValid = false;
     }

interface/src/commerce/QmlCommerce.cpp

@@ -40,11 +40,9 @@ QmlCommerce::QmlCommerce() {
     connect(ledger.data(), &Ledger::transferAssetToUsernameResult, this, &QmlCommerce::transferAssetToUsernameResult);
     connect(ledger.data(), &Ledger::availableUpdatesResult, this, &QmlCommerce::availableUpdatesResult);
     connect(ledger.data(), &Ledger::updateItemResult, this, &QmlCommerce::updateItemResult);
-    
+
     auto accountManager = DependencyManager::get<AccountManager>();
-    connect(accountManager.data(), &AccountManager::usernameChanged, this, [&]() {
-        setPassphrase("");
-    });
+    connect(accountManager.data(), &AccountManager::usernameChanged, this, [&]() { setPassphrase(""); });
 
     _appsPath = PathUtils::getAppDataPath() + "Apps/";
 }
@@ -105,7 +103,11 @@ void QmlCommerce::balance() {
     }
 }
 
-void QmlCommerce::inventory(const QString& editionFilter, const QString& typeFilter, const QString& titleFilter, const int& page, const int& perPage) {
+void QmlCommerce::inventory(const QString& editionFilter,
+                            const QString& typeFilter,
+                            const QString& titleFilter,
+                            const int& page,
+                            const int& perPage) {
     auto ledger = DependencyManager::get<Ledger>();
     auto wallet = DependencyManager::get<Wallet>();
     QStringList cachedPublicKeys = wallet->listPublicKeys();
@@ -166,24 +168,30 @@ void QmlCommerce::certificateInfo(const QString& certificateId) {
     ledger->certificateInfo(certificateId);
 }
 
-void QmlCommerce::transferAssetToNode(const QString& nodeID, const QString& certificateID, const int& amount, const QString& optionalMessage) {
+void QmlCommerce::transferAssetToNode(const QString& nodeID,
+                                      const QString& certificateID,
+                                      const int& amount,
+                                      const QString& optionalMessage) {
     auto ledger = DependencyManager::get<Ledger>();
     auto wallet = DependencyManager::get<Wallet>();
     QStringList keys = wallet->listPublicKeys();
     if (keys.count() == 0) {
-        QJsonObject result{ { "status", "fail" },{ "message", "Uninitialized Wallet." } };
+        QJsonObject result{ { "status", "fail" }, { "message", "Uninitialized Wallet." } };
         return emit transferAssetToNodeResult(result);
     }
     QString key = keys[0];
     ledger->transferAssetToNode(key, nodeID, certificateID, amount, optionalMessage);
 }
 
-void QmlCommerce::transferAssetToUsername(const QString& username, const QString& certificateID, const int& amount, const QString& optionalMessage) {
+void QmlCommerce::transferAssetToUsername(const QString& username,
+                                          const QString& certificateID,
+                                          const int& amount,
+                                          const QString& optionalMessage) {
     auto ledger = DependencyManager::get<Ledger>();
     auto wallet = DependencyManager::get<Wallet>();
     QStringList keys = wallet->listPublicKeys();
     if (keys.count() == 0) {
-        QJsonObject result{ { "status", "fail" },{ "message", "Uninitialized Wallet." } };
+        QJsonObject result{ { "status", "fail" }, { "message", "Uninitialized Wallet." } };
         return emit transferAssetToUsernameResult(result);
     }
     QString key = keys[0];
@@ -194,10 +202,7 @@ void QmlCommerce::replaceContentSet(const QString& itemHref, const QString& cert
     auto ledger = DependencyManager::get<Ledger>();
     ledger->updateLocation(certificateID, DependencyManager::get<AddressManager>()->getPlaceName(), true);
     qApp->replaceDomainContent(itemHref);
-    QJsonObject messageProperties = {
-        { "status", "SuccessfulRequestToReplaceContent" },
-        { "content_set_url", itemHref }
-    };
+    QJsonObject messageProperties = { { "status", "SuccessfulRequestToReplaceContent" }, { "content_set_url", itemHref } };
     UserActivityLogger::getInstance().logAction("replace_domain_content", messageProperties);
 
     emit contentSetChanged(itemHref);
@@ -214,10 +219,7 @@ QString QmlCommerce::getInstalledApps(const QString& justInstalledAppID) {
 
     QDir directory(_appsPath);
     QStringList apps = directory.entryList(QStringList("*.app.json"));
-    foreach(QString appFileName, apps) {
-        installedAppsFromMarketplace += appFileName;
-        installedAppsFromMarketplace += ",";
-
+    foreach (QString appFileName, apps) {
         // If we were supplied a "justInstalledAppID" argument, that means we're entering this function
         // to get the new list of installed apps immediately after installing an app.
         // In that case, the app we installed may not yet have its associated script running -
@@ -243,10 +245,12 @@ QString QmlCommerce::getInstalledApps(const QString& justInstalledAppID) {
             // delete the .app.json from the user's local disk.
             if (!runningScripts.contains(scriptURL)) {
                 if (!appFile.remove()) {
-                    qCWarning(commerce)
-                        << "Couldn't delete local .app.json file (app's script isn't running). App filename is:"
-                        << appFileName;
+                    qCWarning(commerce) << "Couldn't delete local .app.json file (app's script isn't running). App filename is:"
+                                        << appFileName;
                 }
+            } else {
+                installedAppsFromMarketplace += appFileName;
+                installedAppsFromMarketplace += ",";
             }
         } else {
             qCDebug(commerce) << "Couldn't open local .app.json file for reading.";
@@ -317,7 +321,9 @@ bool QmlCommerce::uninstallApp(const QString& itemHref) {
     // Read from the file to know what .js script to stop
     QFile appFile(_appsPath + "/" + appHref.fileName());
     if (!appFile.open(QIODevice::ReadOnly)) {
-        qCDebug(commerce) << "Couldn't open local .app.json file for deletion. Cannot continue with app uninstallation. App filename is:" << appHref.fileName();
+        qCDebug(commerce)
+            << "Couldn't open local .app.json file for deletion. Cannot continue with app uninstallation. App filename is:"
+            << appHref.fileName();
         return false;
     }
     QJsonDocument appFileJsonDocument = QJsonDocument::fromJson(appFile.readAll());
@@ -325,13 +331,15 @@ bool QmlCommerce::uninstallApp(const QString& itemHref) {
     QString scriptUrl = appFileJsonObject["scriptURL"].toString();
 
     if (!DependencyManager::get<ScriptEngines>()->stopScript(scriptUrl.trimmed(), false)) {
-        qCWarning(commerce) << "Couldn't stop script during app uninstall. Continuing anyway. ScriptURL is:" << scriptUrl.trimmed();
+        qCWarning(commerce) << "Couldn't stop script during app uninstall. Continuing anyway. ScriptURL is:"
+                            << scriptUrl.trimmed();
     }
 
     // Delete the .app.json from the filesystem
     // remove() closes the file first.
     if (!appFile.remove()) {
-        qCWarning(commerce) << "Couldn't delete local .app.json file during app uninstall. Continuing anyway. App filename is:" << appHref.fileName();
+        qCWarning(commerce) << "Couldn't delete local .app.json file during app uninstall. Continuing anyway. App filename is:"
+                            << appHref.fileName();
     }
 
     QFileInfo appFileInfo(appFile);
@@ -352,7 +360,8 @@ bool QmlCommerce::openApp(const QString& itemHref) {
     QJsonObject appFileJsonObject = appFileJsonDocument.object();
     QString homeUrl = appFileJsonObject["homeURL"].toString();
 
-    auto tablet = dynamic_cast<TabletProxy*>(DependencyManager::get<TabletScriptingInterface>()->getTablet("com.highfidelity.interface.tablet.system"));
+    auto tablet = dynamic_cast<TabletProxy*>(
+        DependencyManager::get<TabletScriptingInterface>()->getTablet("com.highfidelity.interface.tablet.system"));
     if (homeUrl.contains(".qml", Qt::CaseInsensitive)) {
         tablet->loadQMLSource(homeUrl);
     } else if (homeUrl.contains(".html", Qt::CaseInsensitive)) {
@@ -377,7 +386,7 @@ void QmlCommerce::updateItem(const QString& certificateId) {
     auto wallet = DependencyManager::get<Wallet>();
     QStringList keys = wallet->listPublicKeys();
     if (keys.count() == 0) {
-        QJsonObject result{ { "status", "fail" },{ "message", "Uninitialized Wallet." } };
+        QJsonObject result{ { "status", "fail" }, { "message", "Uninitialized Wallet." } };
         return emit updateItemResult(result);
     }
     QString key = keys[0];

interface/src/raypick/CollisionPick.cpp

@@ -32,9 +32,6 @@ PickResultPointer CollisionPickResult::compareAndProcessNewResult(const PickResu
     }
 
     intersects = entityIntersections.size() || avatarIntersections.size();
-    if (newCollisionResult->loadState == LOAD_STATE_NOT_LOADED || loadState == LOAD_STATE_UNKNOWN) {
-        loadState = (LoadState)newCollisionResult->loadState;
-    }
 
     return std::make_shared<CollisionPickResult>(*this);
 }
@@ -80,23 +77,42 @@ QVariantMap CollisionPickResult::toVariantMap() const {
     }
 
     variantMap["intersectingObjects"] = qIntersectingObjects;
-    variantMap["loaded"] = (loadState == LOAD_STATE_LOADED);
     variantMap["collisionRegion"] = pickVariant;
 
     return variantMap;
 }
 
-bool CollisionPick::isShapeInfoReady() {
+bool CollisionPick::isLoaded() const {
+    return !_mathPick.shouldComputeShapeInfo() || (_cachedResource && _cachedResource->isLoaded());
+}
+
+bool CollisionPick::getShapeInfoReady() {
     if (_mathPick.shouldComputeShapeInfo()) {
         if (_cachedResource && _cachedResource->isLoaded()) {
             computeShapeInfo(_mathPick, *_mathPick.shapeInfo, _cachedResource);
-            return true;
+            _mathPick.loaded = true;
+        } else {
+            _mathPick.loaded = false;
         }
-
-        return false;
+    } else {
+        computeShapeInfoDimensionsOnly(_mathPick, *_mathPick.shapeInfo, _cachedResource);
+        _mathPick.loaded = true;
     }
 
-    return true;
+    return _mathPick.loaded;
+}
+
+void CollisionPick::computeShapeInfoDimensionsOnly(CollisionRegion& pick, ShapeInfo& shapeInfo, QSharedPointer<GeometryResource> resource) {
+    ShapeType type = shapeInfo.getType();
+    glm::vec3 dimensions = pick.transform.getScale();
+    QString modelURL = (resource ? resource->getURL().toString() : "");
+    if (type == SHAPE_TYPE_COMPOUND) {
+        shapeInfo.setParams(type, dimensions, modelURL);
+    } else if (type >= SHAPE_TYPE_SIMPLE_HULL && type <= SHAPE_TYPE_STATIC_MESH) {
+        shapeInfo.setParams(type, 0.5f * dimensions, modelURL);
+    } else {
+        shapeInfo.setParams(type, 0.5f * dimensions, modelURL);
+    }
 }
 
 void CollisionPick::computeShapeInfo(CollisionRegion& pick, ShapeInfo& shapeInfo, QSharedPointer<GeometryResource> resource) {
@@ -328,8 +344,25 @@ void CollisionPick::computeShapeInfo(CollisionRegion& pick, ShapeInfo& shapeInfo
     }
 }
 
+CollisionPick::CollisionPick(const PickFilter& filter, float maxDistance, bool enabled, CollisionRegion collisionRegion, PhysicsEnginePointer physicsEngine) :
+    Pick(filter, maxDistance, enabled),
+    _mathPick(collisionRegion),
+    _physicsEngine(physicsEngine) {
+    if (collisionRegion.shouldComputeShapeInfo()) {
+        _cachedResource = DependencyManager::get<ModelCache>()->getCollisionGeometryResource(collisionRegion.modelURL);
+    }
+    _mathPick.loaded = isLoaded();
+}
+
 CollisionRegion CollisionPick::getMathematicalPick() const {
-    return _mathPick;
+    CollisionRegion mathPick = _mathPick;
+    mathPick.loaded = isLoaded();
+    if (!parentTransform) {
+        return mathPick;
+    } else {
+        mathPick.transform = parentTransform->getTransform().worldTransform(mathPick.transform);
+        return mathPick;
+    }
 }
 
 void CollisionPick::filterIntersections(std::vector<ContactTestResult>& intersections) const {
@@ -356,31 +389,37 @@ void CollisionPick::filterIntersections(std::vector<ContactTestResult>& intersec
 }
 
 PickResultPointer CollisionPick::getEntityIntersection(const CollisionRegion& pick) {
-    if (!isShapeInfoReady()) {
+    if (!pick.loaded) {
         // Cannot compute result
-        return std::make_shared<CollisionPickResult>(pick.toVariantMap(), CollisionPickResult::LOAD_STATE_NOT_LOADED, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
+        return std::make_shared<CollisionPickResult>(pick.toVariantMap(), std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
     }
+    getShapeInfoReady();
     
-    auto entityIntersections = _physicsEngine->contactTest(USER_COLLISION_MASK_ENTITIES, *pick.shapeInfo, pick.transform);
+    auto entityIntersections = _physicsEngine->contactTest(USER_COLLISION_MASK_ENTITIES, *pick.shapeInfo, pick.transform, USER_COLLISION_GROUP_DYNAMIC, pick.threshold);
     filterIntersections(entityIntersections);
-    return std::make_shared<CollisionPickResult>(pick, CollisionPickResult::LOAD_STATE_LOADED, entityIntersections, std::vector<ContactTestResult>());
+    return std::make_shared<CollisionPickResult>(pick, entityIntersections, std::vector<ContactTestResult>());
 }
 
 PickResultPointer CollisionPick::getOverlayIntersection(const CollisionRegion& pick) {
-    return std::make_shared<CollisionPickResult>(pick.toVariantMap(), isShapeInfoReady() ? CollisionPickResult::LOAD_STATE_LOADED : CollisionPickResult::LOAD_STATE_NOT_LOADED, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
+    return std::make_shared<CollisionPickResult>(pick, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
 }
 
 PickResultPointer CollisionPick::getAvatarIntersection(const CollisionRegion& pick) {
-    if (!isShapeInfoReady()) {
+    if (!pick.loaded) {
         // Cannot compute result
-        return std::make_shared<CollisionPickResult>(pick.toVariantMap(), CollisionPickResult::LOAD_STATE_NOT_LOADED, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
+        return std::make_shared<CollisionPickResult>(pick, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
     }
+    getShapeInfoReady();
     
-    auto avatarIntersections = _physicsEngine->contactTest(USER_COLLISION_MASK_AVATARS, *pick.shapeInfo, pick.transform);
+    auto avatarIntersections = _physicsEngine->contactTest(USER_COLLISION_MASK_AVATARS, *pick.shapeInfo, pick.transform, USER_COLLISION_GROUP_DYNAMIC, pick.threshold);
     filterIntersections(avatarIntersections);
-    return std::make_shared<CollisionPickResult>(pick, CollisionPickResult::LOAD_STATE_LOADED, std::vector<ContactTestResult>(), avatarIntersections);
+    return std::make_shared<CollisionPickResult>(pick, std::vector<ContactTestResult>(), avatarIntersections);
 }
 
 PickResultPointer CollisionPick::getHUDIntersection(const CollisionRegion& pick) {
-    return std::make_shared<CollisionPickResult>(pick.toVariantMap(), isShapeInfoReady() ? CollisionPickResult::LOAD_STATE_LOADED : CollisionPickResult::LOAD_STATE_NOT_LOADED, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
+    return std::make_shared<CollisionPickResult>(pick.toVariantMap(), std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
+}
+
+Transform CollisionPick::getResultTransform() const {
+    return Transform(getMathematicalPick().transform);
 }

interface/src/raypick/CollisionPick.h

@@ -11,35 +11,28 @@
 #include <PhysicsEngine.h>
 #include <model-networking/ModelCache.h>
 #include <RegisteredMetaTypes.h>
+#include <TransformNode.h>
 #include <Pick.h>
 
 class CollisionPickResult : public PickResult {
 public:
-    enum LoadState {
-        LOAD_STATE_UNKNOWN,
-        LOAD_STATE_NOT_LOADED,
-        LOAD_STATE_LOADED
-    };
-
     CollisionPickResult() {}
     CollisionPickResult(const QVariantMap& pickVariant) : PickResult(pickVariant) {}
 
-    CollisionPickResult(const CollisionRegion& searchRegion, LoadState loadState, const std::vector<ContactTestResult>& entityIntersections, const std::vector<ContactTestResult>& avatarIntersections) :
+    CollisionPickResult(const CollisionRegion& searchRegion, const std::vector<ContactTestResult>& entityIntersections, const std::vector<ContactTestResult>& avatarIntersections) :
         PickResult(searchRegion.toVariantMap()),
-        loadState(loadState),
         intersects(entityIntersections.size() || avatarIntersections.size()),
         entityIntersections(entityIntersections),
-        avatarIntersections(avatarIntersections) {
+        avatarIntersections(avatarIntersections)
+    {
     }
 
     CollisionPickResult(const CollisionPickResult& collisionPickResult) : PickResult(collisionPickResult.pickVariant) {
         avatarIntersections = collisionPickResult.avatarIntersections;
         entityIntersections = collisionPickResult.entityIntersections;
         intersects = collisionPickResult.intersects;
-        loadState = collisionPickResult.loadState;
     }
 
-    LoadState loadState { LOAD_STATE_UNKNOWN };
     bool intersects { false };
     std::vector<ContactTestResult> entityIntersections;
     std::vector<ContactTestResult> avatarIntersections;
@@ -54,28 +47,24 @@ public:
 
 class CollisionPick : public Pick<CollisionRegion> {
 public:
-    CollisionPick(const PickFilter& filter, float maxDistance, bool enabled, CollisionRegion collisionRegion, PhysicsEnginePointer physicsEngine) :
-        Pick(filter, maxDistance, enabled),
-        _mathPick(collisionRegion),
-        _physicsEngine(physicsEngine) {
-        if (collisionRegion.shouldComputeShapeInfo()) {
-            _cachedResource = DependencyManager::get<ModelCache>()->getCollisionGeometryResource(collisionRegion.modelURL);
-        }
-    }
+    CollisionPick(const PickFilter& filter, float maxDistance, bool enabled, CollisionRegion collisionRegion, PhysicsEnginePointer physicsEngine);
 
     CollisionRegion getMathematicalPick() const override;
     PickResultPointer getDefaultResult(const QVariantMap& pickVariant) const override {
-        return std::make_shared<CollisionPickResult>(pickVariant, CollisionPickResult::LOAD_STATE_UNKNOWN, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
+        return std::make_shared<CollisionPickResult>(pickVariant, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
     }
     PickResultPointer getEntityIntersection(const CollisionRegion& pick) override;
     PickResultPointer getOverlayIntersection(const CollisionRegion& pick) override;
     PickResultPointer getAvatarIntersection(const CollisionRegion& pick) override;
     PickResultPointer getHUDIntersection(const CollisionRegion& pick) override;
-
+    Transform getResultTransform() const override;
 protected:
-    // Returns true if pick.shapeInfo is valid. Otherwise, attempts to get the shapeInfo ready for use.
-    bool isShapeInfoReady();
+    // Returns true if the resource for _mathPick.shapeInfo is loaded or if a resource is not needed.
+    bool isLoaded() const;
+    // Returns true if _mathPick.shapeInfo is valid. Otherwise, attempts to get the _mathPick ready for use.
+    bool getShapeInfoReady();
     void computeShapeInfo(CollisionRegion& pick, ShapeInfo& shapeInfo, QSharedPointer<GeometryResource> resource);
+    void computeShapeInfoDimensionsOnly(CollisionRegion& pick, ShapeInfo& shapeInfo, QSharedPointer<GeometryResource> resource);
     void filterIntersections(std::vector<ContactTestResult>& intersections) const;
 
     CollisionRegion _mathPick;

interface/src/raypick/LaserPointer.cpp

@@ -35,6 +35,37 @@ void LaserPointer::editRenderStatePath(const std::string& state, const QVariant&
     }
 }
 
+QVariantMap LaserPointer::toVariantMap() const {
+    QVariantMap qVariantMap;
+
+    QVariantMap qRenderStates;
+    for (auto iter = _renderStates.cbegin(); iter != _renderStates.cend(); iter++) {
+        auto renderState = iter->second;
+        QVariantMap qRenderState;
+        qRenderState["start"] = renderState->getStartID();
+        qRenderState["path"] = std::static_pointer_cast<RenderState>(renderState)->getPathID();
+        qRenderState["end"] = renderState->getEndID();
+        qRenderStates[iter->first.c_str()] = qRenderState;
+    }
+    qVariantMap["renderStates"] = qRenderStates;
+
+    QVariantMap qDefaultRenderStates;
+    for (auto iter = _defaultRenderStates.cbegin(); iter != _defaultRenderStates.cend(); iter++) {
+        float distance = iter->second.first;
+        auto defaultRenderState = iter->second.second;
+        QVariantMap qDefaultRenderState;
+
+        qDefaultRenderState["distance"] = distance;
+        qDefaultRenderState["start"] = defaultRenderState->getStartID();
+        qDefaultRenderState["path"] = std::static_pointer_cast<RenderState>(defaultRenderState)->getPathID();
+        qDefaultRenderState["end"] = defaultRenderState->getEndID();
+        qDefaultRenderStates[iter->first.c_str()] = qDefaultRenderState;
+    }
+    qVariantMap["defaultRenderStates"] = qDefaultRenderStates;
+
+    return qVariantMap;
+}
+
 glm::vec3 LaserPointer::getPickOrigin(const PickResultPointer& pickResult) const {
     auto rayPickResult = std::static_pointer_cast<RayPickResult>(pickResult);
     return (rayPickResult ? vec3FromVariant(rayPickResult->pickVariant["origin"]) : glm::vec3(0.0f));
@@ -42,6 +73,9 @@ glm::vec3 LaserPointer::getPickOrigin(const PickResultPointer& pickResult) const
 
 glm::vec3 LaserPointer::getPickEnd(const PickResultPointer& pickResult, float distance) const {
     auto rayPickResult = std::static_pointer_cast<RayPickResult>(pickResult);
+    if (!rayPickResult) {
+        return glm::vec3(0.0f);
+    }
     if (distance > 0.0f) {
         PickRay pick = PickRay(rayPickResult->pickVariant);
         return pick.origin + distance * pick.direction;

interface/src/raypick/LaserPointer.h

@@ -42,6 +42,8 @@ public:
     LaserPointer(const QVariant& rayProps, const RenderStateMap& renderStates, const DefaultRenderStateMap& defaultRenderStates, bool hover, const PointerTriggers& triggers,
         bool faceAvatar, bool followNormal, float followNormalStrength, bool centerEndY, bool lockEnd, bool distanceScaleEnd, bool scaleWithAvatar, bool enabled);
 
+    QVariantMap toVariantMap() const override;
+
     static std::shared_ptr<StartEndRenderState> buildRenderState(const QVariantMap& propMap);
 
 protected:

interface/src/raypick/MouseTransformNode.cpp

@@ -0,0 +1,27 @@
+//
+//  Created by Sabrina Shanman 8/14/2018
+//  Copyright 2018 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "MouseTransformNode.h"
+
+#include "Application.h"
+#include "display-plugins/CompositorHelper.h"
+#include "RayPick.h"
+
+Transform MouseTransformNode::getTransform() {
+    QVariant position = qApp->getApplicationCompositor().getReticleInterface()->getPosition();
+    if (position.isValid()) {
+        Transform transform;
+        QVariantMap posMap = position.toMap();
+        PickRay pickRay = qApp->getCamera().computePickRay(posMap["x"].toFloat(), posMap["y"].toFloat());
+        transform.setTranslation(pickRay.origin);
+        transform.setRotation(rotationBetween(Vectors::UP, pickRay.direction));
+        return transform;
+    }
+
+    return Transform();
+}

interface/src/raypick/MouseTransformNode.h

@@ -0,0 +1,18 @@
+//
+//  Created by Sabrina Shanman 8/14/2018
+//  Copyright 2018 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_MouseTransformNode_h
+#define hifi_MouseTransformNode_h
+
+#include "TransformNode.h"
+
+class MouseTransformNode : public TransformNode {
+public:
+    Transform getTransform() override;
+};
+
+#endif // hifi_MouseTransformNode_h

interface/src/raypick/ParabolaPick.cpp

@@ -13,11 +13,13 @@
 #include "avatar/AvatarManager.h"
 #include "scripting/HMDScriptingInterface.h"
 #include "DependencyManager.h"
+#include "PickManager.h"
 
 PickResultPointer ParabolaPick::getEntityIntersection(const PickParabola& pick) {
     if (glm::length2(pick.acceleration) > EPSILON && glm::length2(pick.velocity) > EPSILON) {
+        bool precisionPicking = !(getFilter().doesPickCoarse() || DependencyManager::get<PickManager>()->getForceCoarsePicking());
         ParabolaToEntityIntersectionResult entityRes =
-            DependencyManager::get<EntityScriptingInterface>()->findParabolaIntersectionVector(pick, !getFilter().doesPickCoarse(),
+            DependencyManager::get<EntityScriptingInterface>()->findParabolaIntersectionVector(pick, precisionPicking,
                 getIncludeItemsAs<EntityItemID>(), getIgnoreItemsAs<EntityItemID>(), !getFilter().doesPickInvisible(), !getFilter().doesPickNonCollidable());
         if (entityRes.intersects) {
             return std::make_shared<ParabolaPickResult>(IntersectionType::ENTITY, entityRes.entityID, entityRes.distance, entityRes.parabolicDistance, entityRes.intersection, pick, entityRes.surfaceNormal, entityRes.extraInfo);
@@ -28,8 +30,9 @@ PickResultPointer ParabolaPick::getEntityIntersection(const PickParabola& pick)
 
 PickResultPointer ParabolaPick::getOverlayIntersection(const PickParabola& pick) {
     if (glm::length2(pick.acceleration) > EPSILON && glm::length2(pick.velocity) > EPSILON) {
+        bool precisionPicking = !(getFilter().doesPickCoarse() || DependencyManager::get<PickManager>()->getForceCoarsePicking());
         ParabolaToOverlayIntersectionResult overlayRes =
-            qApp->getOverlays().findParabolaIntersectionVector(pick, !getFilter().doesPickCoarse(),
+            qApp->getOverlays().findParabolaIntersectionVector(pick, precisionPicking,
                 getIncludeItemsAs<OverlayID>(), getIgnoreItemsAs<OverlayID>(), !getFilter().doesPickInvisible(), !getFilter().doesPickNonCollidable());
         if (overlayRes.intersects) {
             return std::make_shared<ParabolaPickResult>(IntersectionType::OVERLAY, overlayRes.overlayID, overlayRes.distance, overlayRes.parabolicDistance, overlayRes.intersection, pick, overlayRes.surfaceNormal, overlayRes.extraInfo);
@@ -67,4 +70,16 @@ glm::vec3 ParabolaPick::getAcceleration() const {
         return scale * (DependencyManager::get<AvatarManager>()->getMyAvatar()->getWorldOrientation() * _accelerationAxis);
     }
     return scale * _accelerationAxis;
+}
+
+Transform ParabolaPick::getResultTransform() const {
+    PickResultPointer result = getPrevPickResult();
+    if (!result) {
+        return Transform();
+    }
+
+    auto parabolaResult = std::static_pointer_cast<ParabolaPickResult>(result);
+    Transform transform;
+    transform.setTranslation(parabolaResult->intersection);
+    return transform;
 }

interface/src/raypick/ParabolaPick.h

@@ -83,6 +83,7 @@ public:
     PickResultPointer getOverlayIntersection(const PickParabola& pick) override;
     PickResultPointer getAvatarIntersection(const PickParabola& pick) override;
     PickResultPointer getHUDIntersection(const PickParabola& pick) override;
+    Transform getResultTransform() const override;
 
 protected:
     float _speed;

interface/src/raypick/ParabolaPointer.cpp

@@ -60,6 +60,35 @@ void ParabolaPointer::editRenderStatePath(const std::string& state, const QVaria
     }
 }
 
+QVariantMap ParabolaPointer::toVariantMap() const {
+    QVariantMap qVariantMap;
+
+    QVariantMap qRenderStates;
+    for (auto iter = _renderStates.cbegin(); iter != _renderStates.cend(); iter++) {
+        auto renderState = iter->second;
+        QVariantMap qRenderState;
+        qRenderState["start"] = renderState->getStartID();
+        qRenderState["end"] = renderState->getEndID();
+        qRenderStates[iter->first.c_str()] = qRenderState;
+    }
+    qVariantMap["renderStates"] = qRenderStates;
+
+    QVariantMap qDefaultRenderStates;
+    for (auto iter = _defaultRenderStates.cbegin(); iter != _defaultRenderStates.cend(); iter++) {
+        float distance = iter->second.first;
+        auto defaultRenderState = iter->second.second;
+        QVariantMap qDefaultRenderState;
+
+        qDefaultRenderState["distance"] = distance;
+        qDefaultRenderState["start"] = defaultRenderState->getStartID();
+        qDefaultRenderState["end"] = defaultRenderState->getEndID();
+        qDefaultRenderStates[iter->first.c_str()] = qDefaultRenderState;
+    }
+    qVariantMap["defaultRenderStates"] = qDefaultRenderStates;
+
+    return qVariantMap;
+}
+
 glm::vec3 ParabolaPointer::getPickOrigin(const PickResultPointer& pickResult) const {
     auto parabolaPickResult = std::static_pointer_cast<ParabolaPickResult>(pickResult);
     return (parabolaPickResult ? vec3FromVariant(parabolaPickResult->pickVariant["origin"]) : glm::vec3(0.0f));
@@ -67,6 +96,9 @@ glm::vec3 ParabolaPointer::getPickOrigin(const PickResultPointer& pickResult) co
 
 glm::vec3 ParabolaPointer::getPickEnd(const PickResultPointer& pickResult, float distance) const {
     auto parabolaPickResult = std::static_pointer_cast<ParabolaPickResult>(pickResult);
+    if (!parabolaPickResult) {
+        return glm::vec3(0.0f);
+    }
     if (distance > 0.0f) {
         PickParabola pick = PickParabola(parabolaPickResult->pickVariant);
         return pick.origin + pick.velocity * distance + 0.5f * pick.acceleration * distance * distance;

interface/src/raypick/ParabolaPointer.h

@@ -97,6 +97,8 @@ public:
     ParabolaPointer(const QVariant& rayProps, const RenderStateMap& renderStates, const DefaultRenderStateMap& defaultRenderStates, bool hover, const PointerTriggers& triggers,
         bool faceAvatar, bool followNormal, float followNormalStrength, bool centerEndY, bool lockEnd, bool distanceScaleEnd, bool scaleWithAvatar, bool enabled);
 
+    QVariantMap toVariantMap() const override;
+
     static std::shared_ptr<StartEndRenderState> buildRenderState(const QVariantMap& propMap);
 
 protected:

interface/src/raypick/PathPointer.cpp

@@ -54,11 +54,13 @@ PathPointer::~PathPointer() {
 void PathPointer::setRenderState(const std::string& state) {
     withWriteLock([&] {
         if (!_currentRenderState.empty() && state != _currentRenderState) {
-            if (_renderStates.find(_currentRenderState) != _renderStates.end()) {
-                _renderStates[_currentRenderState]->disable();
+            auto renderState = _renderStates.find(_currentRenderState);
+            if (renderState != _renderStates.end()) {
+                renderState->second->disable();
             }
-            if (_defaultRenderStates.find(_currentRenderState) != _defaultRenderStates.end()) {
-                _defaultRenderStates[_currentRenderState].second->disable();
+            auto defaultRenderState = _defaultRenderStates.find(_currentRenderState);
+            if (defaultRenderState != _defaultRenderStates.end()) {
+                defaultRenderState->second.second->disable();
             }
         }
         _currentRenderState = state;
@@ -105,7 +107,7 @@ PickResultPointer PathPointer::getVisualPickResult(const PickResultPointer& pick
                 glm::mat4 entityMat = createMatFromQuatAndPos(props.getRotation(), props.getPosition());
                 glm::mat4 finalPosAndRotMat = entityMat * _lockEndObject.offsetMat;
                 pos = extractTranslation(finalPosAndRotMat);
-                rot = glmExtractRotation(finalPosAndRotMat);
+                rot = props.getRotation();
                 dim = props.getDimensions();
                 registrationPoint = props.getRegistrationPoint();
             }
@@ -142,52 +144,57 @@ PickResultPointer PathPointer::getVisualPickResult(const PickResultPointer& pick
 
 void PathPointer::updateVisuals(const PickResultPointer& pickResult) {
     IntersectionType type = getPickedObjectType(pickResult);
-    if (_enabled && !_currentRenderState.empty() && _renderStates.find(_currentRenderState) != _renderStates.end() &&
+    auto renderState = _renderStates.find(_currentRenderState);
+    auto defaultRenderState = _defaultRenderStates.find(_currentRenderState);
+    if (_enabled && !_currentRenderState.empty() && renderState != _renderStates.end() &&
         (type != IntersectionType::NONE || _pathLength > 0.0f)) {
         glm::vec3 origin = getPickOrigin(pickResult);
         glm::vec3 end = getPickEnd(pickResult, _pathLength);
         glm::vec3 surfaceNormal = getPickedObjectNormal(pickResult);
-        _renderStates[_currentRenderState]->update(origin, end, surfaceNormal, _scaleWithAvatar, _distanceScaleEnd, _centerEndY, _faceAvatar,
-                                                   _followNormal, _followNormalStrength, _pathLength, pickResult);
-        if (_defaultRenderStates.find(_currentRenderState) != _defaultRenderStates.end()) {
-            _defaultRenderStates[_currentRenderState].second->disable();
+        renderState->second->update(origin, end, surfaceNormal, _scaleWithAvatar, _distanceScaleEnd, _centerEndY, _faceAvatar,
+                                    _followNormal, _followNormalStrength, _pathLength, pickResult);
+        if (defaultRenderState != _defaultRenderStates.end() && defaultRenderState->second.second->isEnabled()) {
+            defaultRenderState->second.second->disable();
         }
-    } else if (_enabled && !_currentRenderState.empty() && _defaultRenderStates.find(_currentRenderState) != _defaultRenderStates.end()) {
-        if (_renderStates.find(_currentRenderState) != _renderStates.end()) {
-            _renderStates[_currentRenderState]->disable();
+    } else if (_enabled && !_currentRenderState.empty() && defaultRenderState != _defaultRenderStates.end()) {
+        if (renderState != _renderStates.end() && renderState->second->isEnabled()) {
+            renderState->second->disable();
         }
         glm::vec3 origin = getPickOrigin(pickResult);
-        glm::vec3 end = getPickEnd(pickResult, _defaultRenderStates[_currentRenderState].first);
-        _defaultRenderStates[_currentRenderState].second->update(origin, end, Vectors::UP, _scaleWithAvatar, _distanceScaleEnd, _centerEndY,
-                                                                _faceAvatar, _followNormal, _followNormalStrength, _defaultRenderStates[_currentRenderState].first, pickResult);
+        glm::vec3 end = getPickEnd(pickResult, defaultRenderState->second.first);
+        defaultRenderState->second.second->update(origin, end, Vectors::UP, _scaleWithAvatar, _distanceScaleEnd, _centerEndY,
+                                                  _faceAvatar, _followNormal, _followNormalStrength, defaultRenderState->second.first, pickResult);
     } else if (!_currentRenderState.empty()) {
-        if (_renderStates.find(_currentRenderState) != _renderStates.end()) {
-            _renderStates[_currentRenderState]->disable();
+        if (renderState != _renderStates.end() && renderState->second->isEnabled()) {
+            renderState->second->disable();
         }
-        if (_defaultRenderStates.find(_currentRenderState) != _defaultRenderStates.end()) {
-            _defaultRenderStates[_currentRenderState].second->disable();
+        if (defaultRenderState != _defaultRenderStates.end() && defaultRenderState->second.second->isEnabled()) {
+            defaultRenderState->second.second->disable();
         }
     }
 }
 
 void PathPointer::editRenderState(const std::string& state, const QVariant& startProps, const QVariant& pathProps, const QVariant& endProps) {
     withWriteLock([&] {
-        updateRenderStateOverlay(_renderStates[state]->getStartID(), startProps);
-        updateRenderStateOverlay(_renderStates[state]->getEndID(), endProps);
-        QVariant startDim = startProps.toMap()["dimensions"];
-        if (startDim.isValid()) {
-            _renderStates[state]->setStartDim(vec3FromVariant(startDim));
-        }
-        QVariant endDim = endProps.toMap()["dimensions"];
-        if (endDim.isValid()) {
-            _renderStates[state]->setEndDim(vec3FromVariant(endDim));
-        }
-        QVariant rotation = endProps.toMap()["rotation"];
-        if (rotation.isValid()) {
-            _renderStates[state]->setEndRot(quatFromVariant(rotation));
-        }
+        auto renderState = _renderStates.find(state);
+        if (renderState != _renderStates.end()) {
+            updateRenderStateOverlay(renderState->second->getStartID(), startProps);
+            updateRenderStateOverlay(renderState->second->getEndID(), endProps);
+            QVariant startDim = startProps.toMap()["dimensions"];
+            if (startDim.isValid()) {
+                renderState->second->setStartDim(vec3FromVariant(startDim));
+            }
+            QVariant endDim = endProps.toMap()["dimensions"];
+            if (endDim.isValid()) {
+                renderState->second->setEndDim(vec3FromVariant(endDim));
+            }
+            QVariant rotation = endProps.toMap()["rotation"];
+            if (rotation.isValid()) {
+                renderState->second->setEndRot(quatFromVariant(rotation));
+            }
 
-        editRenderStatePath(state, pathProps);
+            editRenderStatePath(state, pathProps);
+        }
     });
 }
 
@@ -271,6 +278,7 @@ void StartEndRenderState::disable() {
         endProps.insert("ignoreRayIntersection", true);
         qApp->getOverlays().editOverlay(getEndID(), endProps);
     }
+    _enabled = false;
 }
 
 void StartEndRenderState::update(const glm::vec3& origin, const glm::vec3& end, const glm::vec3& surfaceNormal, bool scaleWithAvatar, bool distanceScaleEnd, bool centerEndY,
@@ -337,6 +345,7 @@ void StartEndRenderState::update(const glm::vec3& origin, const glm::vec3& end,
         endProps.insert("ignoreRayIntersection", doesEndIgnoreRays());
         qApp->getOverlays().editOverlay(getEndID(), endProps);
     }
+    _enabled = true;
 }
 
 glm::vec2 PathPointer::findPos2D(const PickedObject& pickedObject, const glm::vec3& origin) {
@@ -350,4 +359,4 @@ glm::vec2 PathPointer::findPos2D(const PickedObject& pickedObject, const glm::ve
     default:
         return glm::vec2(NAN);
     }
-}
+}

interface/src/raypick/PathPointer.h

@@ -47,6 +47,8 @@ public:
     virtual void update(const glm::vec3& origin, const glm::vec3& end, const glm::vec3& surfaceNormal, bool scaleWithAvatar, bool distanceScaleEnd, bool centerEndY,
                         bool faceAvatar, bool followNormal, float followNormalStrength, float distance, const PickResultPointer& pickResult);
 
+    bool isEnabled() const { return _enabled; }
+
 protected:
     OverlayID _startID;
     OverlayID _endID;
@@ -59,6 +61,8 @@ protected:
 
     glm::quat _avgEndRot;
     bool _avgEndRotInitialized { false };
+
+    bool _enabled { true };
 };
 
 typedef std::unordered_map<std::string, std::shared_ptr<StartEndRenderState>> RenderStateMap;

interface/src/raypick/PickScriptingInterface.cpp

@@ -23,6 +23,13 @@
 #include "MouseParabolaPick.h"
 #include "CollisionPick.h"
 
+#include "SpatialParentFinder.h"
+#include "NestableTransformNode.h"
+#include "PickTransformNode.h"
+#include "MouseTransformNode.h"
+#include "avatar/MyAvatarHeadTransformNode.h"
+#include "avatar/AvatarManager.h"
+
 #include <ScriptEngine.h>
 
 unsigned int PickScriptingInterface::createPick(const PickQuery::PickType type, const QVariant& properties) {
@@ -276,8 +283,10 @@ unsigned int PickScriptingInterface::createCollisionPick(const QVariant& propert
     }
 
     CollisionRegion collisionRegion(propMap);
+    auto collisionPick = std::make_shared<CollisionPick>(filter, maxDistance, enabled, collisionRegion, qApp->getPhysicsEngine());
+    collisionPick->parentTransform = createTransformNode(propMap);
 
-    return DependencyManager::get<PickManager>()->addPick(PickQuery::Collision, std::make_shared<CollisionPick>(filter, maxDistance, enabled, collisionRegion, qApp->getPhysicsEngine()));
+    return DependencyManager::get<PickManager>()->addPick(PickQuery::Collision, collisionPick);
 }
 
 void PickScriptingInterface::enablePick(unsigned int uid) {
@@ -351,3 +360,43 @@ unsigned int PickScriptingInterface::getPerFrameTimeBudget() const {
 void PickScriptingInterface::setPerFrameTimeBudget(unsigned int numUsecs) {
     DependencyManager::get<PickManager>()->setPerFrameTimeBudget(numUsecs);
 }
+
+std::shared_ptr<TransformNode> PickScriptingInterface::createTransformNode(const QVariantMap& propMap) {
+    if (propMap["parentID"].isValid()) {
+        QUuid parentUuid = propMap["parentID"].toUuid();
+        if (!parentUuid.isNull()) {
+            // Infer object type from parentID
+            // For now, assume a QUuuid is a SpatiallyNestable. This should change when picks are converted over to QUuids.
+            bool success;
+            std::weak_ptr<SpatiallyNestable> nestablePointer = DependencyManager::get<SpatialParentFinder>()->find(parentUuid, success, nullptr);
+            int parentJointIndex = 0;
+            if (propMap["parentJointIndex"].isValid()) {
+                parentJointIndex = propMap["parentJointIndex"].toInt();
+            }
+            auto sharedNestablePointer = nestablePointer.lock();
+            if (success && sharedNestablePointer) {
+                return std::make_shared<NestableTransformNode>(nestablePointer, parentJointIndex);
+            }
+        }
+
+        unsigned int pickID = propMap["parentID"].toUInt();
+        if (pickID != 0) {
+            return std::make_shared<PickTransformNode>(pickID);
+        }
+    }
+    
+    if (propMap["joint"].isValid()) {
+        QString joint = propMap["joint"].toString();
+        if (joint == "Mouse") {
+            return std::make_shared<MouseTransformNode>();
+        } else if (joint == "Avatar") {
+            return std::make_shared<MyAvatarHeadTransformNode>();
+        } else if (!joint.isNull()) {
+            auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
+            int jointIndex = myAvatar->getJointIndex(joint);
+            return std::make_shared<NestableTransformNode>(myAvatar, jointIndex);
+        }
+    }
+
+    return std::shared_ptr<TransformNode>();
+}

interface/src/raypick/PickScriptingInterface.h

@@ -152,9 +152,6 @@ public:
      * @property {CollisionRegion} collisionRegion The CollisionRegion that was used. Valid even if there was no intersection.
      */
 
-    // TODO: Add this to the CollisionPickResult jsdoc once model collision picks are working
-    //* @property {boolean} loaded If the CollisionRegion was successfully loaded (may be false if a model was used)
-
     /**jsdoc
     * Information about the Collision Pick's intersection with an object
     *
@@ -320,6 +317,9 @@ public slots:
      * @returns {number}
      */
     static constexpr unsigned int INTERSECTED_HUD() { return IntersectionType::HUD; }
+
+protected:
+    static std::shared_ptr<TransformNode> createTransformNode(const QVariantMap& propMap);
 };
 
 #endif // hifi_PickScriptingInterface_h

interface/src/raypick/PointerScriptingInterface.cpp

@@ -76,16 +76,19 @@ unsigned int PointerScriptingInterface::createStylus(const QVariant& properties)
  * @property {number} distance The distance at which to render the end of this Ray Pointer, if one is defined.
  */
 /**jsdoc
- * A set of properties used to define the visual aspect of a Ray Pointer in the case that the Pointer is intersecting something.
+ * A set of properties which define the visual aspect of a Ray Pointer in the case that the Pointer is intersecting something.
  *
  * @typedef {object} Pointers.RayPointerRenderState
- * @property {string} name The name of this render state, used by {@link Pointers.setRenderState} and {@link Pointers.editRenderState}
- * @property {Overlays.OverlayProperties} [start] All of the properties you would normally pass to {@link Overlays.addOverlay}, plus the type (as a <code>type</code> field).
- * An overlay to represent the beginning of the Ray Pointer, if desired.
- * @property {Overlays.OverlayProperties} [path] All of the properties you would normally pass to {@link Overlays.addOverlay}, plus the type (as a <code>type</code> field), which <b>must</b> be <code>"line3d"</code>.
- * An overlay to represent the path of the Ray Pointer, if desired.
- * @property {Overlays.OverlayProperties} [end] All of the properties you would normally pass to {@link Overlays.addOverlay}, plus the type (as a <code>type</code> field).
- * An overlay to represent the end of the Ray Pointer, if desired.
+ * @property {string} name When using {@link Pointers.createPointer}, the name of this render state, used by {@link Pointers.setRenderState} and {@link Pointers.editRenderState}
+ * @property {Overlays.OverlayProperties|QUuid} [start] When using {@link Pointers.createPointer}, an optionally defined overlay to represent the beginning of the Ray Pointer,
+ * using the properties you would normally pass to {@link Overlays.addOverlay}, plus the type (as a <code>type</code> field).
+ * When returned from {@link Pointers.getPointerProperties}, the ID of the created overlay if it exists, or a null ID otherwise.
+ * @property {Overlays.OverlayProperties|QUuid} [path] When using {@link Pointers.createPointer}, an optionally defined overlay to represent the path of the Ray Pointer,
+ * using the properties you would normally pass to {@link Overlays.addOverlay}, plus the type (as a <code>type</code> field), which <b>must</b> be <code>"line3d"</code>.
+ * When returned from {@link Pointers.getPointerProperties}, the ID of the created overlay if it exists, or a null ID otherwise.
+ * @property {Overlays.OverlayProperties|QUuid} [end] When using {@link Pointers.createPointer}, an optionally defined overlay to represent the end of the Ray Pointer,
+ * using the properties you would normally pass to {@link Overlays.addOverlay}, plus the type (as a <code>type</code> field).
+ * When returned from {@link Pointers.getPointerProperties}, the ID of the created overlay if it exists, or a null ID otherwise.
  */
 /**jsdoc
  * A set of properties that can be passed to {@link Pointers.createPointer} to create a new Pointer. Contains the relevant {@link Picks.PickProperties} to define the underlying Pick.
@@ -99,8 +102,12 @@ unsigned int PointerScriptingInterface::createStylus(const QVariant& properties)
  * @property {number} [followNormalStrength=0.0] The strength of the interpolation between the real normal and the visual normal if followNormal is true. <code>0-1</code>.  If 0 or 1,
  * the normal will follow exactly.
  * @property {boolean} [enabled=false]
- * @property {Pointers.RayPointerRenderState[]} [renderStates] A list of different visual states to switch between.
- * @property {Pointers.DefaultRayPointerRenderState[]} [defaultRenderStates] A list of different visual states to use if there is no intersection.
+ * @property {Pointers.RayPointerRenderState[]|Object.<string, Pointers.RayPointerRenderState>} [renderStates] A collection of different visual states to switch between.
+ * When using {@link Pointers.createPointer}, a list of RayPointerRenderStates.
+ * When returned from {@link Pointers.getPointerProperties}, a map between render state names and RayPointRenderStates.
+ * @property {Pointers.DefaultRayPointerRenderState[]|Object.<string, Pointers.DefaultRayPointerRenderState>} [defaultRenderStates] A collection of different visual states to use if there is no intersection.
+ * When using {@link Pointers.createPointer}, a list of DefaultRayPointerRenderStates.
+ * When returned from {@link Pointers.getPointerProperties}, a map between render state names and DefaultRayPointRenderStates.
  * @property {boolean} [hover=false] If this Pointer should generate hover events.
  * @property {Pointers.Trigger[]} [triggers] A list of different triggers mechanisms that control this Pointer's click event generation.
  */
@@ -224,12 +231,15 @@ unsigned int PointerScriptingInterface::createLaserPointer(const QVariant& prope
 * A set of properties used to define the visual aspect of a Parabola Pointer in the case that the Pointer is intersecting something.
 *
 * @typedef {object} Pointers.ParabolaPointerRenderState
-* @property {string} name The name of this render state, used by {@link Pointers.setRenderState} and {@link Pointers.editRenderState}
-* @property {Overlays.OverlayProperties} [start] All of the properties you would normally pass to {@link Overlays.addOverlay}, plus the type (as a <code>type</code> field).
-* An overlay to represent the beginning of the Parabola Pointer, if desired.
-* @property {Pointers.ParabolaProperties} [path] The rendering properties of the parabolic path defined by the Parabola Pointer.
-* @property {Overlays.OverlayProperties} [end] All of the properties you would normally pass to {@link Overlays.addOverlay}, plus the type (as a <code>type</code> field).
-* An overlay to represent the end of the Parabola Pointer, if desired.
+* @property {string} name When using {@link Pointers.createPointer}, the name of this render state, used by {@link Pointers.setRenderState} and {@link Pointers.editRenderState}
+* @property {Overlays.OverlayProperties|QUuid} [start] When using {@link Pointers.createPointer}, an optionally defined overlay to represent the beginning of the Parabola Pointer,
+* using the properties you would normally pass to {@link Overlays.addOverlay}, plus the type (as a <code>type</code> field).
+* When returned from {@link Pointers.getPointerProperties}, the ID of the created overlay if it exists, or a null ID otherwise.
+* @property {Pointers.ParabolaProperties} [path]  When using {@link Pointers.createPointer}, the optionally defined rendering properties of the parabolic path defined by the Parabola Pointer.
+* Not defined in {@link Pointers.getPointerProperties}.
+* @property {Overlays.OverlayProperties|QUuid} [end] When using {@link Pointers.createPointer}, an optionally defined overlay to represent the end of the Parabola Pointer,
+* using the properties you would normally pass to {@link Overlays.addOverlay}, plus the type (as a <code>type</code> field).
+* When returned from {@link Pointers.getPointerProperties}, the ID of the created overlay if it exists, or a null ID otherwise.
 */
 /**jsdoc
 * A set of properties that can be passed to {@link Pointers.createPointer} to create a new Pointer. Contains the relevant {@link Picks.PickProperties} to define the underlying Pick.
@@ -243,8 +253,12 @@ unsigned int PointerScriptingInterface::createLaserPointer(const QVariant& prope
 * @property {number} [followNormalStrength=0.0] The strength of the interpolation between the real normal and the visual normal if followNormal is true. <code>0-1</code>.  If 0 or 1,
 * the normal will follow exactly.
 * @property {boolean} [enabled=false]
-* @property {Pointers.ParabolaPointerRenderState[]} [renderStates] A list of different visual states to switch between.
-* @property {Pointers.DefaultParabolaPointerRenderState[]} [defaultRenderStates] A list of different visual states to use if there is no intersection.
+* @property {Pointers.ParabolaPointerRenderState[]|Object.<string, Pointers.ParabolaPointerRenderState>} [renderStates] A collection of different visual states to switch between.
+* When using {@link Pointers.createPointer}, a list of ParabolaPointerRenderStates.
+* When returned from {@link Pointers.getPointerProperties}, a map between render state names and ParabolaPointerRenderStates.
+* @property {Pointers.DefaultParabolaPointerRenderState[]|Object.<string, Pointers.DefaultParabolaPointerRenderState>} [defaultRenderStates] A collection of different visual states to use if there is no intersection.
+* When using {@link Pointers.createPointer}, a list of DefaultParabolaPointerRenderStates.
+* When returned from {@link Pointers.getPointerProperties}, a map between render state names and DefaultParabolaPointerRenderStates.
 * @property {boolean} [hover=false] If this Pointer should generate hover events.
 * @property {Pointers.Trigger[]} [triggers] A list of different triggers mechanisms that control this Pointer's click event generation.
 */
@@ -375,4 +389,8 @@ QVariantMap PointerScriptingInterface::getPrevPickResult(unsigned int uid) const
         result = pickResult->toVariantMap();
     }
     return result;
+}
+
+QVariantMap PointerScriptingInterface::getPointerProperties(unsigned int uid) const {
+    return DependencyManager::get<PointerManager>()->getPointerProperties(uid);
 }

interface/src/raypick/PointerScriptingInterface.h

@@ -203,6 +203,14 @@ public:
      */
     Q_INVOKABLE bool isMouse(unsigned int uid) { return DependencyManager::get<PointerManager>()->isMouse(uid); }
 
+    /**jsdoc
+     * Returns information about an existing Pointer
+     * @function Pointers.getPointerState
+     * @param {number} uid The ID of the Pointer, as returned by {@link Pointers.createPointer}.
+     * @returns {Pointers.LaserPointerProperties|Pointers.StylusPointerProperties|Pointers.ParabolaPointerProperties} The information about the Pointer.
+     * Currently only includes renderStates and defaultRenderStates with associated overlay IDs.
+     */
+    Q_INVOKABLE QVariantMap getPointerProperties(unsigned int uid) const;
 };
 
 #endif // hifi_PointerScriptingInterface_h

interface/src/raypick/RayPick.cpp

@@ -13,10 +13,12 @@
 #include "avatar/AvatarManager.h"
 #include "scripting/HMDScriptingInterface.h"
 #include "DependencyManager.h"
+#include "PickManager.h"
 
 PickResultPointer RayPick::getEntityIntersection(const PickRay& pick) {
+    bool precisionPicking = !(getFilter().doesPickCoarse() || DependencyManager::get<PickManager>()->getForceCoarsePicking());
     RayToEntityIntersectionResult entityRes =
-        DependencyManager::get<EntityScriptingInterface>()->findRayIntersectionVector(pick, !getFilter().doesPickCoarse(),
+        DependencyManager::get<EntityScriptingInterface>()->findRayIntersectionVector(pick, precisionPicking,
             getIncludeItemsAs<EntityItemID>(), getIgnoreItemsAs<EntityItemID>(), !getFilter().doesPickInvisible(), !getFilter().doesPickNonCollidable());
     if (entityRes.intersects) {
         return std::make_shared<RayPickResult>(IntersectionType::ENTITY, entityRes.entityID, entityRes.distance, entityRes.intersection, pick, entityRes.surfaceNormal, entityRes.extraInfo);
@@ -26,8 +28,9 @@ PickResultPointer RayPick::getEntityIntersection(const PickRay& pick) {
 }
 
 PickResultPointer RayPick::getOverlayIntersection(const PickRay& pick) {
+    bool precisionPicking = !(getFilter().doesPickCoarse() || DependencyManager::get<PickManager>()->getForceCoarsePicking());
     RayToOverlayIntersectionResult overlayRes =
-        qApp->getOverlays().findRayIntersectionVector(pick, !getFilter().doesPickCoarse(),
+        qApp->getOverlays().findRayIntersectionVector(pick, precisionPicking,
             getIncludeItemsAs<OverlayID>(), getIgnoreItemsAs<OverlayID>(), !getFilter().doesPickInvisible(), !getFilter().doesPickNonCollidable());
     if (overlayRes.intersects) {
         return std::make_shared<RayPickResult>(IntersectionType::OVERLAY, overlayRes.overlayID, overlayRes.distance, overlayRes.intersection, pick, overlayRes.surfaceNormal, overlayRes.extraInfo);
@@ -50,6 +53,18 @@ PickResultPointer RayPick::getHUDIntersection(const PickRay& pick) {
     return std::make_shared<RayPickResult>(IntersectionType::HUD, QUuid(), glm::distance(pick.origin, hudRes), hudRes, pick);
 }
 
+Transform RayPick::getResultTransform() const {
+    PickResultPointer result = getPrevPickResult();
+    if (!result) {
+        return Transform();
+    }
+
+    auto rayResult = std::static_pointer_cast<RayPickResult>(result);
+    Transform transform;
+    transform.setTranslation(rayResult->intersection);
+    return transform;
+}
+
 glm::vec3 RayPick::intersectRayWithXYPlane(const glm::vec3& origin, const glm::vec3& direction, const glm::vec3& point, const glm::quat& rotation, const glm::vec3& registration) {
     // TODO: take into account registration
     glm::vec3 n = rotation * Vectors::FRONT;

interface/src/raypick/RayPick.h

@@ -77,6 +77,7 @@ public:
     PickResultPointer getOverlayIntersection(const PickRay& pick) override;
     PickResultPointer getAvatarIntersection(const PickRay& pick) override;
     PickResultPointer getHUDIntersection(const PickRay& pick) override;
+    Transform getResultTransform() const override;
 
     // These are helper functions for projecting and intersecting rays
     static glm::vec3 intersectRayWithEntityXYPlane(const QUuid& entityID, const glm::vec3& origin, const glm::vec3& direction);

interface/src/raypick/StylusPick.cpp

@@ -225,4 +225,16 @@ PickResultPointer StylusPick::getAvatarIntersection(const StylusTip& pick) {
 
 PickResultPointer StylusPick::getHUDIntersection(const StylusTip& pick) {
     return std::make_shared<StylusPickResult>(pick.toVariantMap());
+}
+
+Transform StylusPick::getResultTransform() const {
+    PickResultPointer result = getPrevPickResult();
+    if (!result) {
+        return Transform();
+    }
+
+    auto stylusResult = std::static_pointer_cast<StylusPickResult>(result);
+    Transform transform;
+    transform.setTranslation(stylusResult->intersection);
+    return transform;
 }

interface/src/raypick/StylusPick.h

@@ -66,6 +66,7 @@ public:
     PickResultPointer getOverlayIntersection(const StylusTip& pick) override;
     PickResultPointer getAvatarIntersection(const StylusTip& pick) override;
     PickResultPointer getHUDIntersection(const StylusTip& pick) override;
+    Transform getResultTransform() const override;
 
     bool isLeftHand() const override { return _side == Side::Left; }
     bool isRightHand() const override { return _side == Side::Right; }

interface/src/raypick/StylusPointer.cpp

@@ -64,7 +64,9 @@ void StylusPointer::updateVisuals(const PickResultPointer& pickResult) {
             return;
         }
     }
-    hide();
+    if (_showing) {
+        hide();
+    }
 }
 
 void StylusPointer::show(const StylusTip& tip) {
@@ -80,6 +82,7 @@ void StylusPointer::show(const StylusTip& tip) {
         props["visible"] = true;
         qApp->getOverlays().editOverlay(_stylusOverlay, props);
     }
+    _showing = true;
 }
 
 void StylusPointer::hide() {
@@ -88,6 +91,7 @@ void StylusPointer::hide() {
         props.insert("visible", false);
         qApp->getOverlays().editOverlay(_stylusOverlay, props);
     }
+    _showing = false;
 }
 
 bool StylusPointer::shouldHover(const PickResultPointer& pickResult) {
@@ -203,6 +207,10 @@ void StylusPointer::setRenderState(const std::string& state) {
     }
 }
 
+QVariantMap StylusPointer::toVariantMap() const {
+    return QVariantMap();
+}
+
 glm::vec3 StylusPointer::findIntersection(const PickedObject& pickedObject, const glm::vec3& origin, const glm::vec3& direction) {
     switch (pickedObject.type) {
         case ENTITY:

interface/src/raypick/StylusPointer.h

@@ -33,6 +33,8 @@ public:
     void setRenderState(const std::string& state) override;
     void editRenderState(const std::string& state, const QVariant& startProps, const QVariant& pathProps, const QVariant& endProps) override {}
 
+    QVariantMap toVariantMap() const override;
+
     static OverlayID buildStylusOverlay(const QVariantMap& properties);
 
 protected:
@@ -76,6 +78,8 @@ private:
     static glm::vec3 findIntersection(const PickedObject& pickedObject, const glm::vec3& origin, const glm::vec3& direction);
     static glm::vec2 findPos2D(const PickedObject& pickedObject, const glm::vec3& origin);
 
+    bool _showing { true };
+
 };
 
 #endif // hifi_StylusPointer_h

interface/src/scripting/TestScriptingInterface.cpp

@@ -190,4 +190,12 @@ void TestScriptingInterface::saveObject(QVariant variant, const QString& filenam
 
 void TestScriptingInterface::showMaximized() {
     qApp->getWindow()->showMaximized();
+}
+
+void TestScriptingInterface::setOtherAvatarsReplicaCount(int count) {
+    qApp->setOtherAvatarsReplicaCount(count);
+}
+
+int TestScriptingInterface::getOtherAvatarsReplicaCount() {
+    return qApp->getOtherAvatarsReplicaCount();
 }

interface/src/scripting/TestScriptingInterface.h

@@ -149,6 +149,20 @@ public slots:
     */
     void showMaximized();
 
+    /**jsdoc
+    * Values higher than 0 will create replicas of other-avatars when entering a domain for testing purpouses
+    * @function Test.setOtherAvatarsReplicaCount
+    * @param {number} count - Number of replicas we want to create
+    */
+    Q_INVOKABLE void setOtherAvatarsReplicaCount(int count);
+
+    /**jsdoc
+    * Return the number of replicas that are being created of other-avatars when entering a domain
+    * @function Test.getOtherAvatarsReplicaCount
+    * @returns {number} Current number of replicas of other-avatars.
+    */
+    Q_INVOKABLE int getOtherAvatarsReplicaCount();
+
 private:
     bool waitForCondition(qint64 maxWaitMs, std::function<bool()> condition);
     QString _testResultsLocation;

interface/src/ui/Stats.cpp

@@ -26,6 +26,7 @@
 #include <OffscreenUi.h>
 #include <PerfStat.h>
 #include <plugins/DisplayPlugin.h>
+#include <PickManager.h>
 
 #include <gl/Context.h>
 
@@ -147,6 +148,20 @@ void Stats::updateStats(bool force) {
     }
     STAT_UPDATE(gameLoopRate, (int)qApp->getGameLoopRate());
 
+    auto pickManager = DependencyManager::get<PickManager>();
+    if (pickManager && (_expanded || force)) {
+        std::vector<int> totalPicks = pickManager->getTotalPickCounts();
+        STAT_UPDATE(stylusPicksCount, totalPicks[PickQuery::Stylus]);
+        STAT_UPDATE(rayPicksCount, totalPicks[PickQuery::Ray]);
+        STAT_UPDATE(parabolaPicksCount, totalPicks[PickQuery::Parabola]);
+        STAT_UPDATE(collisionPicksCount, totalPicks[PickQuery::Collision]);
+        std::vector<QVector4D> updatedPicks = pickManager->getUpdatedPickCounts();
+        STAT_UPDATE(stylusPicksUpdated, updatedPicks[PickQuery::Stylus]);
+        STAT_UPDATE(rayPicksUpdated, updatedPicks[PickQuery::Ray]);
+        STAT_UPDATE(parabolaPicksUpdated, updatedPicks[PickQuery::Parabola]);
+        STAT_UPDATE(collisionPicksUpdated, updatedPicks[PickQuery::Collision]);
+    }
+
     auto bandwidthRecorder = DependencyManager::get<BandwidthRecorder>();
     STAT_UPDATE(packetInCount, (int)bandwidthRecorder->getCachedTotalAverageInputPacketsPerSecond());
     STAT_UPDATE(packetOutCount, (int)bandwidthRecorder->getCachedTotalAverageOutputPacketsPerSecond());
@@ -286,7 +301,7 @@ void Stats::updateStats(bool force) {
         //    downloads << (int)(resource->getProgress() * 100.0f) << "% ";
         //}
         //downloads << "(" <<  << " pending)";
-    } // expanded avatar column
+    }
 
     // Fourth column, octree stats
     int serverCount = 0;

interface/src/ui/Stats.h

@@ -22,7 +22,6 @@ public: \
 private: \
     type _##name{ initialValue };
 
-
 /**jsdoc
  * @namespace Stats
  *
@@ -169,6 +168,15 @@ private: \
  * @property {number} implicitHeight
  *
  * @property {object} layer - <em>Read-only.</em>
+
+ * @property {number} stylusPicksCount - <em>Read-only.</em>
+ * @property {number} rayPicksCount - <em>Read-only.</em>
+ * @property {number} parabolaPicksCount - <em>Read-only.</em>
+ * @property {number} collisionPicksCount - <em>Read-only.</em>
+ * @property {Vec4} stylusPicksUpdated - <em>Read-only.</em>
+ * @property {Vec4} rayPicksUpdated - <em>Read-only.</em>
+ * @property {Vec4} parabolaPicksUpdated - <em>Read-only.</em>
+ * @property {Vec4} collisionPicksUpdated - <em>Read-only.</em>
  */
 // Properties from x onwards are QQuickItem properties.
 
@@ -287,6 +295,15 @@ class Stats : public QQuickItem {
     STATS_PROPERTY(float, avatarSimulationTime, 0)
     Q_PROPERTY(QStringList animStackNames READ animStackNames NOTIFY animStackNamesChanged)
 
+    STATS_PROPERTY(int, stylusPicksCount, 0)
+    STATS_PROPERTY(int, rayPicksCount, 0)
+    STATS_PROPERTY(int, parabolaPicksCount, 0)
+    STATS_PROPERTY(int, collisionPicksCount, 0)
+    STATS_PROPERTY(QVector4D, stylusPicksUpdated, QVector4D(0, 0, 0, 0))
+    STATS_PROPERTY(QVector4D, rayPicksUpdated, QVector4D(0, 0, 0, 0))
+    STATS_PROPERTY(QVector4D, parabolaPicksUpdated, QVector4D(0, 0, 0, 0))
+    STATS_PROPERTY(QVector4D, collisionPicksUpdated, QVector4D(0, 0, 0, 0))
+
 public:
     static Stats* getInstance();
 
@@ -1254,6 +1271,62 @@ signals:
      * @function Stats.update
      */
 
+    /**jsdoc
+     * Triggered when the value of the <code>stylusPicksCount</code> property changes.
+     * @function Stats.stylusPicksCountChanged
+     * @returns {Signal}
+     */
+    void stylusPicksCountChanged();
+
+    /**jsdoc
+     * Triggered when the value of the <code>rayPicksCount</code> property changes.
+     * @function Stats.rayPicksCountChanged
+     * @returns {Signal}
+     */
+    void rayPicksCountChanged();
+
+    /**jsdoc
+     * Triggered when the value of the <code>parabolaPicksCount</code> property changes.
+     * @function Stats.parabolaPicksCountChanged
+     * @returns {Signal}
+     */
+    void parabolaPicksCountChanged();
+
+    /**jsdoc
+     * Triggered when the value of the <code>collisionPicksCount</code> property changes.
+     * @function Stats.collisionPicksCountChanged
+     * @returns {Signal}
+     */
+    void collisionPicksCountChanged();
+
+    /**jsdoc
+     * Triggered when the value of the <code>stylusPicksUpdated</code> property changes.
+     * @function Stats.stylusPicksUpdatedChanged
+     * @returns {Signal}
+     */
+    void stylusPicksUpdatedChanged();
+
+    /**jsdoc
+     * Triggered when the value of the <code>rayPicksUpdated</code> property changes.
+     * @function Stats.rayPicksUpdatedChanged
+     * @returns {Signal}
+     */
+    void rayPicksUpdatedChanged();
+
+    /**jsdoc
+     * Triggered when the value of the <code>parabolaPicksUpdated</code> property changes.
+     * @function Stats.parabolaPicksUpdatedChanged
+     * @returns {Signal}
+     */
+    void parabolaPicksUpdatedChanged();
+
+    /**jsdoc
+     * Triggered when the value of the <code>collisionPicksUpdated</code> property changes.
+     * @function Stats.collisionPicksUpdatedChanged
+     * @returns {Signal}
+     */
+    void collisionPicksUpdatedChanged();
+
 private:
     int _recentMaxPackets{ 0 } ; // recent max incoming voxel packets to process
     bool _resetRecentMaxPacketsSoon{ true };

interface/src/ui/overlays/ContextOverlayInterface.cpp

@@ -181,7 +181,7 @@ bool ContextOverlayInterface::createOrDestroyContextOverlay(const EntityItemID&
                 float distance;
                 BoxFace face;
                 glm::vec3 normal;
-                boundingBox.findRayIntersection(cameraPosition, direction, distance, face, normal);
+                boundingBox.findRayIntersection(cameraPosition, direction, 1.0f / direction, distance, face, normal);
                 float offsetAngle = -CONTEXT_OVERLAY_OFFSET_ANGLE;
                 if (event.getID() == 1) { // "1" is left hand
                     offsetAngle *= -1.0f;

interface/src/ui/overlays/Volume3DOverlay.cpp

@@ -88,7 +88,7 @@ bool Volume3DOverlay::findRayIntersection(const glm::vec3& origin, const glm::ve
 
     // we can use the AABox's ray intersection by mapping our origin and direction into the overlays frame
     // and testing intersection there.
-    bool hit = _localBoundingBox.findRayIntersection(overlayFrameOrigin, overlayFrameDirection, distance, face, surfaceNormal);
+    bool hit = _localBoundingBox.findRayIntersection(overlayFrameOrigin, overlayFrameDirection, 1.0f / overlayFrameDirection, distance, face, surfaceNormal);
 
     if (hit) {
         surfaceNormal = transform.getRotation() * surfaceNormal;

libraries/animation/src/AnimBlendLinearMove.cpp

@@ -156,6 +156,10 @@ void AnimBlendLinearMove::setFrameAndPhase(float dt, float alpha, int prevPoseIn
     // integrate phase forward in time.
     _phase += omega * dt;
 
+    if (_phase < 0.0f) {
+        _phase = 0.0f;
+    }
+    
     // detect loop trigger events
     if (_phase >= 1.0f) {
         triggersOut.setTrigger(_id + "Loop");

libraries/audio/src/AudioHRTF.cpp

@@ -240,7 +240,7 @@ static void FIR_1x4_SSE(float* src, float* dst0, float* dst1, float* dst2, float
 
         float* ps = &src[i - HRTF_TAPS + 1];    // process forwards
 
-        assert(HRTF_TAPS % 4 == 0);
+        static_assert(HRTF_TAPS % 4 == 0, "HRTF_TAPS must be a multiple of 4");
 
         for (int k = 0; k < HRTF_TAPS; k += 4) {
 
@@ -276,23 +276,8 @@ static void FIR_1x4_SSE(float* src, float* dst0, float* dst1, float* dst2, float
     }
 }
 
-//
-// Runtime CPU dispatch
-//
-
-#include "CPUDetect.h"
-
-void FIR_1x4_AVX2(float* src, float* dst0, float* dst1, float* dst2, float* dst3, float coef[4][HRTF_TAPS], int numFrames);
-void FIR_1x4_AVX512(float* src, float* dst0, float* dst1, float* dst2, float* dst3, float coef[4][HRTF_TAPS], int numFrames);
-
-static void FIR_1x4(float* src, float* dst0, float* dst1, float* dst2, float* dst3, float coef[4][HRTF_TAPS], int numFrames) {
-
-    static auto f = cpuSupportsAVX512() ? FIR_1x4_AVX512 : (cpuSupportsAVX2() ? FIR_1x4_AVX2 : FIR_1x4_SSE);
-    (*f)(src, dst0, dst1, dst2, dst3, coef, numFrames); // dispatch
-}
-
 // 4 channel planar to interleaved
-static void interleave_4x4(float* src0, float* src1, float* src2, float* src3, float* dst, int numFrames) {
+static void interleave_4x4_SSE(float* src0, float* src1, float* src2, float* src3, float* dst, int numFrames) {
 
     assert(numFrames % 4 == 0);
 
@@ -323,7 +308,7 @@ static void interleave_4x4(float* src0, float* src1, float* src2, float* src3, f
 
 // process 2 cascaded biquads on 4 channels (interleaved)
 // biquads computed in parallel, by adding one sample of delay
-static void biquad2_4x4(float* src, float* dst, float coef[5][8], float state[3][8], int numFrames) {
+static void biquad2_4x4_SSE(float* src, float* dst, float coef[5][8], float state[3][8], int numFrames) {
 
     // enable flush-to-zero mode to prevent denormals
     unsigned int ftz = _MM_GET_FLUSH_ZERO_MODE();
@@ -388,7 +373,7 @@ static void biquad2_4x4(float* src, float* dst, float coef[5][8], float state[3]
 }
 
 // crossfade 4 inputs into 2 outputs with accumulation (interleaved)
-static void crossfade_4x2(float* src, float* dst, const float* win, int numFrames) {
+static void crossfade_4x2_SSE(float* src, float* dst, const float* win, int numFrames) {
 
     assert(numFrames % 4 == 0);
 
@@ -435,12 +420,12 @@ static void crossfade_4x2(float* src, float* dst, const float* win, int numFrame
 }
 
 // linear interpolation with gain
-static void interpolate(float* dst, const float* src0, const float* src1, float frac, float gain) {
+static void interpolate_SSE(const float* src0, const float* src1, float* dst, float frac, float gain) {
 
     __m128 f0 = _mm_set1_ps(gain * (1.0f - frac));
     __m128 f1 = _mm_set1_ps(gain * frac);
 
-    assert(HRTF_TAPS % 4 == 0);
+    static_assert(HRTF_TAPS % 4 == 0, "HRTF_TAPS must be a multiple of 4");
 
     for (int k = 0; k < HRTF_TAPS; k += 4) {
 
@@ -453,6 +438,44 @@ static void interpolate(float* dst, const float* src0, const float* src1, float
     }
 }
 
+//
+// Runtime CPU dispatch
+//
+
+#include "CPUDetect.h"
+
+void FIR_1x4_AVX2(float* src, float* dst0, float* dst1, float* dst2, float* dst3, float coef[4][HRTF_TAPS], int numFrames);
+void FIR_1x4_AVX512(float* src, float* dst0, float* dst1, float* dst2, float* dst3, float coef[4][HRTF_TAPS], int numFrames);
+void interleave_4x4_AVX2(float* src0, float* src1, float* src2, float* src3, float* dst, int numFrames);
+void biquad2_4x4_AVX2(float* src, float* dst, float coef[5][8], float state[3][8], int numFrames);
+void crossfade_4x2_AVX2(float* src, float* dst, const float* win, int numFrames);
+void interpolate_AVX2(const float* src0, const float* src1, float* dst, float frac, float gain);
+
+static void FIR_1x4(float* src, float* dst0, float* dst1, float* dst2, float* dst3, float coef[4][HRTF_TAPS], int numFrames) {
+    static auto f = cpuSupportsAVX512() ? FIR_1x4_AVX512 : (cpuSupportsAVX2() ? FIR_1x4_AVX2 : FIR_1x4_SSE);
+    (*f)(src, dst0, dst1, dst2, dst3, coef, numFrames); // dispatch
+}
+
+static void interleave_4x4(float* src0, float* src1, float* src2, float* src3, float* dst, int numFrames) {
+    static auto f = cpuSupportsAVX2() ? interleave_4x4_AVX2 : interleave_4x4_SSE;
+    (*f)(src0, src1, src2, src3, dst, numFrames); // dispatch
+}
+
+static void biquad2_4x4(float* src, float* dst, float coef[5][8], float state[3][8], int numFrames) {
+    static auto f = cpuSupportsAVX2() ? biquad2_4x4_AVX2 : biquad2_4x4_SSE;
+    (*f)(src, dst, coef, state, numFrames); // dispatch
+}
+
+static void crossfade_4x2(float* src, float* dst, const float* win, int numFrames) {
+    static auto f = cpuSupportsAVX2() ? crossfade_4x2_AVX2 : crossfade_4x2_SSE;
+    (*f)(src, dst, win, numFrames); // dispatch
+}
+
+static void interpolate(const float* src0, const float* src1, float* dst, float frac, float gain) {
+    static auto f = cpuSupportsAVX2() ? interpolate_AVX2 : interpolate_SSE;
+    (*f)(src0, src1, dst, frac, gain); // dispatch
+}
+
 #else   // portable reference code
 
 // 1 channel input, 4 channel output
@@ -489,7 +512,7 @@ static void FIR_1x4(float* src, float* dst0, float* dst1, float* dst2, float* ds
 
         float* ps = &src[i - HRTF_TAPS + 1];    // process forwards
 
-        assert(HRTF_TAPS % 4 == 0);
+        static_assert(HRTF_TAPS % 4 == 0, "HRTF_TAPS must be a multiple of 4");
 
         for (int k = 0; k < HRTF_TAPS; k += 4) {
 
@@ -715,7 +738,7 @@ static void crossfade_4x2(float* src, float* dst, const float* win, int numFrame
 }
 
 // linear interpolation with gain
-static void interpolate(float* dst, const float* src0, const float* src1, float frac, float gain) {
+static void interpolate(const float* src0, const float* src1, float* dst, float frac, float gain) {
 
     float f0 = gain * (1.0f - frac);
     float f1 = gain * frac;
@@ -967,8 +990,8 @@ static void setFilters(float firCoef[4][HRTF_TAPS], float bqCoef[5][8], int dela
     azimuthToIndex(azimuth, az0, az1, frac);
 
     // interpolate FIR
-    interpolate(firCoef[channel+0], ir_table_table[index][azL0][0], ir_table_table[index][azL1][0], fracL, gain * gainL);
-    interpolate(firCoef[channel+1], ir_table_table[index][azR0][1], ir_table_table[index][azR1][1], fracR, gain * gainR);
+    interpolate(ir_table_table[index][azL0][0], ir_table_table[index][azL1][0], firCoef[channel+0], fracL, gain * gainL);
+    interpolate(ir_table_table[index][azR0][1], ir_table_table[index][azR1][1], firCoef[channel+1], fracR, gain * gainR);
 
     // interpolate ITD
     float itd = (1.0f - frac) * itd_table_table[index][az0] + frac * itd_table_table[index][az1];

libraries/audio/src/avx2/AudioHRTF_avx2.cpp

@@ -44,7 +44,7 @@ void FIR_1x4_AVX2(float* src, float* dst0, float* dst1, float* dst2, float* dst3
 
         float* ps = &src[i - HRTF_TAPS + 1];    // process forwards
 
-        assert(HRTF_TAPS % 4 == 0);
+        static_assert(HRTF_TAPS % 4 == 0, "HRTF_TAPS must be a multiple of 4");
 
         for (int k = 0; k < HRTF_TAPS; k += 4) {
 
@@ -87,4 +87,165 @@ void FIR_1x4_AVX2(float* src, float* dst0, float* dst1, float* dst2, float* dst3
     _mm256_zeroupper();
 }
 
+// 4 channel planar to interleaved
+void interleave_4x4_AVX2(float* src0, float* src1, float* src2, float* src3, float* dst, int numFrames) {
+
+    assert(numFrames % 8 == 0);
+
+    for (int i = 0; i < numFrames; i += 8) {
+
+        __m256 x0 = _mm256_loadu_ps(&src0[i]);
+        __m256 x1 = _mm256_loadu_ps(&src1[i]);
+        __m256 x2 = _mm256_loadu_ps(&src2[i]);
+        __m256 x3 = _mm256_loadu_ps(&src3[i]);
+
+        // interleave (4x4 matrix transpose)
+        __m256 t0 = _mm256_unpacklo_ps(x0, x1);
+        __m256 t1 = _mm256_unpackhi_ps(x0, x1);
+        __m256 t2 = _mm256_unpacklo_ps(x2, x3);
+        __m256 t3 = _mm256_unpackhi_ps(x2, x3);
+
+        x0 = _mm256_shuffle_ps(t0, t2, _MM_SHUFFLE(1,0,1,0));
+        x1 = _mm256_shuffle_ps(t0, t2, _MM_SHUFFLE(3,2,3,2));
+        x2 = _mm256_shuffle_ps(t1, t3, _MM_SHUFFLE(1,0,1,0));
+        x3 = _mm256_shuffle_ps(t1, t3, _MM_SHUFFLE(3,2,3,2));
+
+        t0 = _mm256_permute2f128_ps(x0, x1, 0x20);
+        t1 = _mm256_permute2f128_ps(x2, x3, 0x20);
+        t2 = _mm256_permute2f128_ps(x0, x1, 0x31);
+        t3 = _mm256_permute2f128_ps(x2, x3, 0x31);
+
+        _mm256_storeu_ps(&dst[4*i+0], t0);
+        _mm256_storeu_ps(&dst[4*i+8], t1);
+        _mm256_storeu_ps(&dst[4*i+16], t2);
+        _mm256_storeu_ps(&dst[4*i+24], t3);
+    }
+
+    _mm256_zeroupper();
+}
+
+// process 2 cascaded biquads on 4 channels (interleaved)
+// biquads are computed in parallel, by adding one sample of delay
+void biquad2_4x4_AVX2(float* src, float* dst, float coef[5][8], float state[3][8], int numFrames) {
+
+    // enable flush-to-zero mode to prevent denormals
+    unsigned int ftz = _MM_GET_FLUSH_ZERO_MODE();
+    _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_ON);
+
+    // restore state
+    __m256 x0 = _mm256_setzero_ps();
+    __m256 y0 = _mm256_loadu_ps(state[0]);
+    __m256 w1 = _mm256_loadu_ps(state[1]);
+    __m256 w2 = _mm256_loadu_ps(state[2]);
+
+    //  biquad coefs
+    __m256 b0 = _mm256_loadu_ps(coef[0]);
+    __m256 b1 = _mm256_loadu_ps(coef[1]);
+    __m256 b2 = _mm256_loadu_ps(coef[2]);
+    __m256 a1 = _mm256_loadu_ps(coef[3]);
+    __m256 a2 = _mm256_loadu_ps(coef[4]);
+
+    for (int i = 0; i < numFrames; i++) {
+
+        // x0 = (first biquad output << 128) | input
+        x0 = _mm256_insertf128_ps(_mm256_permute2f128_ps(y0, y0, 0x01), _mm_loadu_ps(&src[4*i]), 0);
+
+        // transposed Direct Form II
+        y0 = _mm256_fmadd_ps(x0, b0, w1);
+        w1 = _mm256_fmadd_ps(x0, b1, w2);
+        w2 = _mm256_mul_ps(x0, b2);
+        w1 = _mm256_fnmadd_ps(y0, a1, w1);
+        w2 = _mm256_fnmadd_ps(y0, a2, w2);
+
+        _mm_storeu_ps(&dst[4*i], _mm256_extractf128_ps(y0, 1)); // second biquad output
+    }
+
+    // save state
+    _mm256_storeu_ps(state[0], y0);
+    _mm256_storeu_ps(state[1], w1);
+    _mm256_storeu_ps(state[2], w2);
+
+    _MM_SET_FLUSH_ZERO_MODE(ftz);
+    _mm256_zeroupper();
+}
+
+// crossfade 4 inputs into 2 outputs with accumulation (interleaved)
+void crossfade_4x2_AVX2(float* src, float* dst, const float* win, int numFrames) {
+
+    assert(numFrames % 8 == 0);
+
+    for (int i = 0; i < numFrames; i += 8) {
+
+        __m256 f0 = _mm256_loadu_ps(&win[i]);
+
+        __m256 x0 = _mm256_castps128_ps256(_mm_loadu_ps(&src[4*i+0]));
+        __m256 x1 = _mm256_castps128_ps256(_mm_loadu_ps(&src[4*i+4]));
+        __m256 x2 = _mm256_castps128_ps256(_mm_loadu_ps(&src[4*i+8]));
+        __m256 x3 = _mm256_castps128_ps256(_mm_loadu_ps(&src[4*i+12]));
+
+        x0 = _mm256_insertf128_ps(x0, _mm_loadu_ps(&src[4*i+16]), 1);
+        x1 = _mm256_insertf128_ps(x1, _mm_loadu_ps(&src[4*i+20]), 1);
+        x2 = _mm256_insertf128_ps(x2, _mm_loadu_ps(&src[4*i+24]), 1);
+        x3 = _mm256_insertf128_ps(x3, _mm_loadu_ps(&src[4*i+28]), 1);
+
+        __m256 y0 = _mm256_loadu_ps(&dst[2*i+0]);
+        __m256 y1 = _mm256_loadu_ps(&dst[2*i+8]);
+
+        // deinterleave (4x4 matrix transpose)
+        __m256 t0 = _mm256_unpacklo_ps(x0, x1);
+        __m256 t1 = _mm256_unpackhi_ps(x0, x1);
+        __m256 t2 = _mm256_unpacklo_ps(x2, x3);
+        __m256 t3 = _mm256_unpackhi_ps(x2, x3);
+
+        x0 = _mm256_shuffle_ps(t0, t2, _MM_SHUFFLE(1,0,1,0));
+        x1 = _mm256_shuffle_ps(t0, t2, _MM_SHUFFLE(3,2,3,2));
+        x2 = _mm256_shuffle_ps(t1, t3, _MM_SHUFFLE(1,0,1,0));
+        x3 = _mm256_shuffle_ps(t1, t3, _MM_SHUFFLE(3,2,3,2));
+
+        // crossfade
+        x0 = _mm256_sub_ps(x0, x2);
+        x1 = _mm256_sub_ps(x1, x3);
+        x2 = _mm256_fmadd_ps(f0, x0, x2);
+        x3 = _mm256_fmadd_ps(f0, x1, x3);
+
+        // interleave
+        t0 = _mm256_unpacklo_ps(x2, x3);
+        t1 = _mm256_unpackhi_ps(x2, x3);
+
+        x0 = _mm256_permute2f128_ps(t0, t1, 0x20);
+        x1 = _mm256_permute2f128_ps(t0, t1, 0x31);
+
+        // accumulate
+        y0 = _mm256_add_ps(y0, x0);
+        y1 = _mm256_add_ps(y1, x1);
+
+        _mm256_storeu_ps(&dst[2*i+0], y0);
+        _mm256_storeu_ps(&dst[2*i+8], y1);
+    }
+
+    _mm256_zeroupper();
+}
+
+// linear interpolation with gain
+void interpolate_AVX2(const float* src0, const float* src1, float* dst, float frac, float gain) {
+
+    __m256 f0 = _mm256_set1_ps(gain * (1.0f - frac));
+    __m256 f1 = _mm256_set1_ps(gain * frac);
+
+    static_assert(HRTF_TAPS % 8 == 0, "HRTF_TAPS must be a multiple of 8");
+
+    for (int k = 0; k < HRTF_TAPS; k += 8) {
+
+        __m256 x0 = _mm256_loadu_ps(&src0[k]);
+        __m256 x1 = _mm256_loadu_ps(&src1[k]);
+
+        x0 = _mm256_mul_ps(f0, x0);
+        x0 = _mm256_fmadd_ps(f1, x1, x0);
+
+        _mm256_storeu_ps(&dst[k], x0);
+    }
+
+    _mm256_zeroupper();
+}
+
 #endif

libraries/audio/src/avx512/AudioHRTF_avx512.cpp

@@ -44,7 +44,7 @@ void FIR_1x4_AVX512(float* src, float* dst0, float* dst1, float* dst2, float* ds
 
         float* ps = &src[i - HRTF_TAPS + 1];    // process forwards
 
-        assert(HRTF_TAPS % 4 == 0);
+        static_assert(HRTF_TAPS % 4 == 0, "HRTF_TAPS must be a multiple of 4");
 
         for (int k = 0; k < HRTF_TAPS; k += 4) {
 

libraries/avatars-renderer/src/avatars-renderer/Avatar.h

@@ -458,7 +458,6 @@ protected:
     glm::vec3 _lastAngularVelocity;
     glm::vec3 _angularAcceleration;
     glm::quat _lastOrientation;
-
     glm::vec3 _worldUpDirection { Vectors::UP };
     bool _moving { false }; ///< set when position is changing
 

libraries/avatars-renderer/src/avatars-renderer/SkeletonModel.cpp

@@ -294,6 +294,15 @@ bool SkeletonModel::getEyeModelPositions(glm::vec3& firstEyePosition, glm::vec3&
         firstEyePosition = baseEyePosition + headRotation * glm::vec3(EYE_SEPARATION, 0.0f, EYES_FORWARD) * headHeight;
         secondEyePosition = baseEyePosition + headRotation * glm::vec3(-EYE_SEPARATION, 0.0f, EYES_FORWARD) * headHeight;
         return true;
+    } else if (getJointPosition(geometry.headJointIndex, headPosition)) {
+        glm::vec3 baseEyePosition = headPosition;
+        glm::quat headRotation;
+        getJointRotation(geometry.headJointIndex, headRotation);
+        const float EYES_FORWARD_HEAD_ONLY = 0.30f;
+        const float EYE_SEPARATION = 0.1f;
+        firstEyePosition = baseEyePosition + headRotation * glm::vec3(EYE_SEPARATION, 0.0f, EYES_FORWARD_HEAD_ONLY);
+        secondEyePosition = baseEyePosition + headRotation * glm::vec3(-EYE_SEPARATION, 0.0f, EYES_FORWARD_HEAD_ONLY);
+        return true;
     }
     return false;
 }

libraries/avatars/src/AvatarData.cpp

@@ -918,7 +918,18 @@ int AvatarData::parseDataFromBuffer(const QByteArray& buffer) {
 
         PACKET_READ_CHECK(AvatarGlobalPosition, sizeof(AvatarDataPacket::AvatarGlobalPosition));
         auto data = reinterpret_cast<const AvatarDataPacket::AvatarGlobalPosition*>(sourceBuffer);
-        auto newValue = glm::vec3(data->globalPosition[0], data->globalPosition[1], data->globalPosition[2]);
+
+        glm::vec3 offset = glm::vec3(0.0f, 0.0f, 0.0f);
+
+        if (_replicaIndex > 0) {
+            const float SPACE_BETWEEN_AVATARS = 2.0f;
+            const int AVATARS_PER_ROW = 3;
+            int row = _replicaIndex % AVATARS_PER_ROW;
+            int col = floor(_replicaIndex / AVATARS_PER_ROW);
+            offset = glm::vec3(row * SPACE_BETWEEN_AVATARS, 0.0f, col * SPACE_BETWEEN_AVATARS);
+        }
+
+        auto newValue = glm::vec3(data->globalPosition[0], data->globalPosition[1], data->globalPosition[2]) + offset;
         if (_globalPosition != newValue) {
             _globalPosition = newValue;
             _globalPositionChanged = usecTimestampNow();

libraries/avatars/src/AvatarData.h

@@ -337,6 +337,7 @@ enum KillAvatarReason : uint8_t {
     TheirAvatarEnteredYourBubble,
     YourAvatarEnteredTheirBubble
 };
+
 Q_DECLARE_METATYPE(KillAvatarReason);
 
 class QDataStream;
@@ -1186,6 +1187,8 @@ public:
 
     virtual void addMaterial(graphics::MaterialLayer material, const std::string& parentMaterialName) {}
     virtual void removeMaterial(graphics::MaterialPointer material, const std::string& parentMaterialName) {}
+    void setReplicaIndex(int replicaIndex) { _replicaIndex = replicaIndex; }
+    int getReplicaIndex() { return _replicaIndex; }
 
 signals:
 
@@ -1445,6 +1448,7 @@ protected:
     udt::SequenceNumber _identitySequenceNumber { 0 };
     bool _hasProcessedFirstIdentity { false };
     float _density;
+    int _replicaIndex { 0 };
 
     // null unless MyAvatar or ScriptableAvatar sending traits data to mixer
     std::unique_ptr<ClientTraitsHandler> _clientTraitsHandler;
@@ -1561,7 +1565,7 @@ class RayToAvatarIntersectionResult {
 public:
     bool intersects { false };
     QUuid avatarID;
-    float distance { 0.0f };
+    float distance { FLT_MAX };
     BoxFace face;
     glm::vec3 intersection;
     glm::vec3 surfaceNormal;

libraries/avatars/src/AvatarHashMap.cpp

@@ -21,6 +21,84 @@
 #include "AvatarLogging.h"
 #include "AvatarTraits.h"
 
+
+void AvatarReplicas::addReplica(const QUuid& parentID, AvatarSharedPointer replica) {
+    if (parentID == QUuid()) {
+        return;
+    }
+    if (_replicasMap.find(parentID) == _replicasMap.end()) {
+        std::vector<AvatarSharedPointer> emptyReplicas = std::vector<AvatarSharedPointer>();
+        _replicasMap.insert(std::pair<QUuid, std::vector<AvatarSharedPointer>>(parentID, emptyReplicas));
+    }
+    auto &replicas = _replicasMap[parentID];
+    replica->setReplicaIndex((int)replicas.size() + 1);
+    replicas.push_back(replica);
+}
+
+std::vector<QUuid> AvatarReplicas::getReplicaIDs(const QUuid& parentID) {
+    std::vector<QUuid> ids;
+    if (_replicasMap.find(parentID) != _replicasMap.end()) {
+        auto &replicas = _replicasMap[parentID];
+        for (int i = 0; i < (int)replicas.size(); i++) {
+            ids.push_back(replicas[i]->getID());
+        }
+    } else if (_replicaCount > 0) {
+        for (int i = 0; i < _replicaCount; i++) {
+            ids.push_back(QUuid::createUuid());
+        }
+    }
+    return ids;
+}
+
+void AvatarReplicas::parseDataFromBuffer(const QUuid& parentID, const QByteArray& buffer) {
+    if (_replicasMap.find(parentID) != _replicasMap.end()) {
+        auto &replicas = _replicasMap[parentID];
+        for (auto avatar : replicas) {
+            avatar->parseDataFromBuffer(buffer);
+        }
+    }
+}
+
+void AvatarReplicas::removeReplicas(const QUuid& parentID) {
+    if (_replicasMap.find(parentID) != _replicasMap.end()) {
+        _replicasMap.erase(parentID);
+    }
+}
+
+void AvatarReplicas::processAvatarIdentity(const QUuid& parentID, const QByteArray& identityData, bool& identityChanged, bool& displayNameChanged) {
+    if (_replicasMap.find(parentID) != _replicasMap.end()) {
+        auto &replicas = _replicasMap[parentID];
+        for (auto avatar : replicas) {
+            avatar->processAvatarIdentity(identityData, identityChanged, displayNameChanged);
+        }
+    }
+}
+void AvatarReplicas::processTrait(const QUuid& parentID, AvatarTraits::TraitType traitType, QByteArray traitBinaryData) {
+    if (_replicasMap.find(parentID) != _replicasMap.end()) {
+        auto &replicas = _replicasMap[parentID];
+        for (auto avatar : replicas) {
+            avatar->processTrait(traitType, traitBinaryData);
+        }
+    }
+}
+void AvatarReplicas::processDeletedTraitInstance(const QUuid& parentID, AvatarTraits::TraitType traitType, AvatarTraits::TraitInstanceID instanceID) {
+    if (_replicasMap.find(parentID) != _replicasMap.end()) {
+        auto &replicas = _replicasMap[parentID];
+        for (auto avatar : replicas) {
+            avatar->processDeletedTraitInstance(traitType, instanceID);
+        }
+    }
+}
+void AvatarReplicas::processTraitInstance(const QUuid& parentID, AvatarTraits::TraitType traitType,
+    AvatarTraits::TraitInstanceID instanceID, QByteArray traitBinaryData) {
+    if (_replicasMap.find(parentID) != _replicasMap.end()) {
+        auto &replicas = _replicasMap[parentID];
+        for (auto avatar : replicas) {
+            avatar->processTraitInstance(traitType, instanceID, traitBinaryData);
+        }
+    }
+}
+
 AvatarHashMap::AvatarHashMap() {
     auto nodeList = DependencyManager::get<NodeList>();
 
@@ -64,6 +142,21 @@ bool AvatarHashMap::isAvatarInRange(const glm::vec3& position, const float range
     return false;
 }
 
+void AvatarHashMap::setReplicaCount(int count) {
+    _replicas.setReplicaCount(count);
+    auto avatars = getAvatarIdentifiers();
+    for (int i = 0; i < avatars.size(); i++) {
+        KillAvatarReason reason = KillAvatarReason::NoReason;
+        if (avatars[i] != QUuid()) {
+            removeAvatar(avatars[i], reason);
+            auto replicaIDs = _replicas.getReplicaIDs(avatars[i]);
+            for (auto id : replicaIDs) {
+                removeAvatar(id, reason);
+            }
+        }
+    }
+}
+
 int AvatarHashMap::numberOfAvatarsInRange(const glm::vec3& position, float rangeMeters) {
     auto hashCopy = getHashCopy();
     auto rangeMeters2 = rangeMeters * rangeMeters;
@@ -135,18 +228,25 @@ AvatarSharedPointer AvatarHashMap::parseAvatarData(QSharedPointer<ReceivedMessag
 
     // make sure this isn't our own avatar data or for a previously ignored node
     auto nodeList = DependencyManager::get<NodeList>();
-
     bool isNewAvatar;
     if (sessionUUID != _lastOwnerSessionUUID && (!nodeList->isIgnoringNode(sessionUUID) || nodeList->getRequestsDomainListData())) {
         auto avatar = newOrExistingAvatar(sessionUUID, sendingNode, isNewAvatar);
+
         if (isNewAvatar) {
             QWriteLocker locker(&_hashLock);
             _pendingAvatars.insert(sessionUUID, { std::chrono::steady_clock::now(), 0, avatar });
-        }
+            auto replicaIDs = _replicas.getReplicaIDs(sessionUUID);
+            for (auto replicaID : replicaIDs) {
+                auto replicaAvatar = addAvatar(replicaID, sendingNode);
+                _replicas.addReplica(sessionUUID, replicaAvatar);
+            }
+        } 
 
         // have the matching (or new) avatar parse the data from the packet
         int bytesRead = avatar->parseDataFromBuffer(byteArray);
         message->seek(positionBeforeRead + bytesRead);
+        _replicas.parseDataFromBuffer(sessionUUID, byteArray);
+
         return avatar;
     } else {
         // create a dummy AvatarData class to throw this data on the ground
@@ -191,10 +291,13 @@ void AvatarHashMap::processAvatarIdentityPacket(QSharedPointer<ReceivedMessage>
         bool displayNameChanged = false;
         // In this case, the "sendingNode" is the Avatar Mixer.
         avatar->processAvatarIdentity(message->getMessage(), identityChanged, displayNameChanged);
+        _replicas.processAvatarIdentity(identityUUID, message->getMessage(), identityChanged, displayNameChanged);
+
     }
 }
 
 void AvatarHashMap::processBulkAvatarTraits(QSharedPointer<ReceivedMessage> message, SharedNodePointer sendingNode) {
+
     while (message->getBytesLeftToRead()) {
         // read the avatar ID to figure out which avatar this is for
         auto avatarID = QUuid::fromRfc4122(message->readWithoutCopy(NUM_BYTES_RFC4122_UUID));
@@ -202,7 +305,6 @@ void AvatarHashMap::processBulkAvatarTraits(QSharedPointer<ReceivedMessage> mess
         // grab the avatar so we can ask it to process trait data
         bool isNewAvatar;
         auto avatar = newOrExistingAvatar(avatarID, sendingNode, isNewAvatar);
-
         // read the first trait type for this avatar
         AvatarTraits::TraitType traitType;
         message->readPrimitive(&traitType);
@@ -217,13 +319,14 @@ void AvatarHashMap::processBulkAvatarTraits(QSharedPointer<ReceivedMessage> mess
             AvatarTraits::TraitWireSize traitBinarySize;
             bool skipBinaryTrait = false;
 
-
             if (AvatarTraits::isSimpleTrait(traitType)) {
                 message->readPrimitive(&traitBinarySize);
 
                 // check if this trait version is newer than what we already have for this avatar
                 if (packetTraitVersion > lastProcessedVersions[traitType]) {
-                    avatar->processTrait(traitType, message->read(traitBinarySize));
+                    auto traitData = message->read(traitBinarySize);
+                    avatar->processTrait(traitType, traitData);
+                    _replicas.processTrait(avatarID, traitType, traitData);
                     lastProcessedVersions[traitType] = packetTraitVersion;
                 } else {
                     skipBinaryTrait = true;
@@ -238,8 +341,11 @@ void AvatarHashMap::processBulkAvatarTraits(QSharedPointer<ReceivedMessage> mess
                 if (packetTraitVersion > processedInstanceVersion) {
                     if (traitBinarySize == AvatarTraits::DELETED_TRAIT_SIZE) {
                         avatar->processDeletedTraitInstance(traitType, traitInstanceID);
+                        _replicas.processDeletedTraitInstance(avatarID, traitType, traitInstanceID);
                     } else {
-                        avatar->processTraitInstance(traitType, traitInstanceID, message->read(traitBinarySize));
+                        auto traitData = message->read(traitBinarySize);
+                        avatar->processTraitInstance(traitType, traitInstanceID, traitData);
+                        _replicas.processTraitInstance(avatarID, traitType, traitInstanceID, traitData);
                     }
                     processedInstanceVersion = packetTraitVersion;
                 } else {
@@ -265,17 +371,31 @@ void AvatarHashMap::processKillAvatar(QSharedPointer<ReceivedMessage> message, S
     KillAvatarReason reason;
     message->readPrimitive(&reason);
     removeAvatar(sessionUUID, reason);
+    auto replicaIDs = _replicas.getReplicaIDs(sessionUUID);
+    for (auto id : replicaIDs) {
+        removeAvatar(id, reason);
+    }
 }
 
 void AvatarHashMap::removeAvatar(const QUuid& sessionUUID, KillAvatarReason removalReason) {
     QWriteLocker locker(&_hashLock);
 
+    auto replicaIDs = _replicas.getReplicaIDs(sessionUUID);
+    _replicas.removeReplicas(sessionUUID);
+    for (auto id : replicaIDs) {
+        auto removedReplica = _avatarHash.take(id);
+        if (removedReplica) {
+            handleRemovedAvatar(removedReplica, removalReason);
+        }
+    }
+
     _pendingAvatars.remove(sessionUUID);
     auto removedAvatar = _avatarHash.take(sessionUUID);
 
     if (removedAvatar) {
         handleRemovedAvatar(removedAvatar, removalReason);
     }
+
 }
 
 void AvatarHashMap::handleRemovedAvatar(const AvatarSharedPointer& removedAvatar, KillAvatarReason removalReason) {

libraries/avatars/src/AvatarHashMap.h

@@ -41,6 +41,27 @@
  * @hifi-assignment-client
  */
 
+class AvatarReplicas {
+public:
+    AvatarReplicas() {}
+    void addReplica(const QUuid& parentID, AvatarSharedPointer replica);
+    std::vector<QUuid> getReplicaIDs(const QUuid& parentID);
+    void parseDataFromBuffer(const QUuid& parentID, const QByteArray& buffer);
+    void processAvatarIdentity(const QUuid& parentID, const QByteArray& identityData, bool& identityChanged, bool& displayNameChanged);
+    void removeReplicas(const QUuid& parentID);
+    void processTrait(const QUuid& parentID, AvatarTraits::TraitType traitType, QByteArray traitBinaryData);
+    void processDeletedTraitInstance(const QUuid& parentID, AvatarTraits::TraitType traitType, AvatarTraits::TraitInstanceID instanceID);
+    void processTraitInstance(const QUuid& parentID, AvatarTraits::TraitType traitType,
+                                AvatarTraits::TraitInstanceID instanceID, QByteArray traitBinaryData);
+    void setReplicaCount(int count) { _replicaCount = count; }
+    int getReplicaCount() { return _replicaCount; }
+
+private:
+    std::map<QUuid, std::vector<AvatarSharedPointer>> _replicasMap;
+    int _replicaCount { 0 };
+};
+
+
 class AvatarHashMap : public QObject, public Dependency {
     Q_OBJECT
     SINGLETON_DEPENDENCY
@@ -77,6 +98,9 @@ public:
     virtual AvatarSharedPointer getAvatarBySessionID(const QUuid& sessionID) const { return findAvatar(sessionID); }
     int numberOfAvatarsInRange(const glm::vec3& position, float rangeMeters);
 
+    void setReplicaCount(int count);
+    int getReplicaCount() { return _replicas.getReplicaCount(); };
+
 signals:
 
     /**jsdoc
@@ -167,6 +191,8 @@ protected:
     mutable QReadWriteLock _hashLock;
 
     std::unordered_map<QUuid, AvatarTraits::TraitVersions> _processedTraitVersions;
+    AvatarReplicas _replicas;
+
 private:
     QUuid _lastOwnerSessionUUID;
 };

libraries/entities-renderer/src/RenderablePolyVoxEntityItem.cpp

@@ -571,7 +571,6 @@ bool RenderablePolyVoxEntityItem::findDetailedRayIntersection(const glm::vec3& o
     }
 
     glm::mat4 wtvMatrix = worldToVoxelMatrix();
-    glm::mat4 vtwMatrix = voxelToWorldMatrix();
     glm::vec3 normDirection = glm::normalize(direction);
 
     // the PolyVox ray intersection code requires a near and far point.
@@ -584,8 +583,6 @@ bool RenderablePolyVoxEntityItem::findDetailedRayIntersection(const glm::vec3& o
     glm::vec4 originInVoxel = wtvMatrix * glm::vec4(origin, 1.0f);
     glm::vec4 farInVoxel = wtvMatrix * glm::vec4(farPoint, 1.0f);
 
-    glm::vec4 directionInVoxel = glm::normalize(farInVoxel - originInVoxel);
-
     glm::vec4 result = glm::vec4(0.0f, 0.0f, 0.0f, 0.0f);
     PolyVox::RaycastResult raycastResult = doRayCast(originInVoxel, farInVoxel, result);
     if (raycastResult == PolyVox::RaycastResults::Completed) {
@@ -599,14 +596,9 @@ bool RenderablePolyVoxEntityItem::findDetailedRayIntersection(const glm::vec3& o
     voxelBox += result3 - Vectors::HALF;
     voxelBox += result3 + Vectors::HALF;
 
-    float voxelDistance;
-    bool hit = voxelBox.findRayIntersection(glm::vec3(originInVoxel), glm::vec3(directionInVoxel),
-                                            voxelDistance, face, surfaceNormal);
-
-    glm::vec4 voxelIntersectionPoint = glm::vec4(glm::vec3(originInVoxel) + glm::vec3(directionInVoxel) * voxelDistance, 1.0);
-    glm::vec4 intersectionPoint = vtwMatrix * voxelIntersectionPoint;
-    distance = glm::distance(origin, glm::vec3(intersectionPoint));
-    return hit;
+    glm::vec3 directionInVoxel = vec3(wtvMatrix * glm::vec4(direction, 0.0f));
+    return voxelBox.findRayIntersection(glm::vec3(originInVoxel), directionInVoxel, 1.0f / directionInVoxel,
+                                        distance, face, surfaceNormal);
 }
 
 bool RenderablePolyVoxEntityItem::findDetailedParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity,

libraries/entities/src/EntityTree.cpp

@@ -48,6 +48,7 @@ public:
     // Inputs
     glm::vec3 origin;
     glm::vec3 direction;
+    glm::vec3 invDirection;
     const QVector<EntityItemID>& entityIdsToInclude;
     const QVector<EntityItemID>& entityIdsToDiscard;
     bool visibleOnly;
@@ -825,28 +826,51 @@ bool findRayIntersectionOp(const OctreeElementPointer& element, void* extraData)
     RayArgs* args = static_cast<RayArgs*>(extraData);
     bool keepSearching = true;
     EntityTreeElementPointer entityTreeElementPointer = std::static_pointer_cast<EntityTreeElement>(element);
-    EntityItemID entityID = entityTreeElementPointer->findRayIntersection(args->origin, args->direction, keepSearching,
+    EntityItemID entityID = entityTreeElementPointer->findRayIntersection(args->origin, args->direction,
         args->element, args->distance, args->face, args->surfaceNormal, args->entityIdsToInclude,
         args->entityIdsToDiscard, args->visibleOnly, args->collidableOnly, args->extraInfo, args->precisionPicking);
     if (!entityID.isNull()) {
         args->entityID = entityID;
+        // We recurse OctreeElements in order, so if we hit something, we can stop immediately
+        keepSearching = false;
     }
     return keepSearching;
 }
 
+float findRayIntersectionSortingOp(const OctreeElementPointer& element, void* extraData) {
+    RayArgs* args = static_cast<RayArgs*>(extraData);
+    EntityTreeElementPointer entityTreeElementPointer = std::static_pointer_cast<EntityTreeElement>(element);
+    float distance = FLT_MAX;
+    // If origin is inside the cube, always check this element first
+    if (entityTreeElementPointer->getAACube().contains(args->origin)) {
+        distance = 0.0f;
+    } else {
+        float boundDistance = FLT_MAX;
+        BoxFace face;
+        glm::vec3 surfaceNormal;
+        if (entityTreeElementPointer->getAACube().findRayIntersection(args->origin, args->direction, args->invDirection, boundDistance, face, surfaceNormal)) {
+            // Don't add this cell if it's already farther than our best distance so far
+            if (boundDistance < args->distance) {
+                distance = boundDistance;
+            }
+        }
+    }
+    return distance;
+}
+
 EntityItemID EntityTree::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
                                     QVector<EntityItemID> entityIdsToInclude, QVector<EntityItemID> entityIdsToDiscard,
                                     bool visibleOnly, bool collidableOnly, bool precisionPicking, 
                                     OctreeElementPointer& element, float& distance,
                                     BoxFace& face, glm::vec3& surfaceNormal, QVariantMap& extraInfo,
                                     Octree::lockType lockType, bool* accurateResult) {
-    RayArgs args = { origin, direction, entityIdsToInclude, entityIdsToDiscard,
+    RayArgs args = { origin, direction, 1.0f / direction, entityIdsToInclude, entityIdsToDiscard,
             visibleOnly, collidableOnly, precisionPicking, element, distance, face, surfaceNormal, extraInfo, EntityItemID() };
     distance = FLT_MAX;
 
     bool requireLock = lockType == Octree::Lock;
     bool lockResult = withReadLock([&]{
-        recurseTreeWithOperation(findRayIntersectionOp, &args);
+        recurseTreeWithOperationSorted(findRayIntersectionOp, findRayIntersectionSortingOp, &args);
     }, requireLock);
 
     if (accurateResult) {
@@ -860,15 +884,38 @@ bool findParabolaIntersectionOp(const OctreeElementPointer& element, void* extra
     ParabolaArgs* args = static_cast<ParabolaArgs*>(extraData);
     bool keepSearching = true;
     EntityTreeElementPointer entityTreeElementPointer = std::static_pointer_cast<EntityTreeElement>(element);
-    EntityItemID entityID = entityTreeElementPointer->findParabolaIntersection(args->origin, args->velocity, args->acceleration, keepSearching,
+    EntityItemID entityID = entityTreeElementPointer->findParabolaIntersection(args->origin, args->velocity, args->acceleration,
         args->element, args->parabolicDistance, args->face, args->surfaceNormal, args->entityIdsToInclude,
         args->entityIdsToDiscard, args->visibleOnly, args->collidableOnly, args->extraInfo, args->precisionPicking);
     if (!entityID.isNull()) {
         args->entityID = entityID;
+        // We recurse OctreeElements in order, so if we hit something, we can stop immediately
+        keepSearching = false;
     }
     return keepSearching;
 }
 
+float findParabolaIntersectionSortingOp(const OctreeElementPointer& element, void* extraData) {
+    ParabolaArgs* args = static_cast<ParabolaArgs*>(extraData);
+    EntityTreeElementPointer entityTreeElementPointer = std::static_pointer_cast<EntityTreeElement>(element);
+    float distance = FLT_MAX;
+    // If origin is inside the cube, always check this element first
+    if (entityTreeElementPointer->getAACube().contains(args->origin)) {
+        distance = 0.0f;
+    } else {
+        float boundDistance = FLT_MAX;
+        BoxFace face;
+        glm::vec3 surfaceNormal;
+        if (entityTreeElementPointer->getAACube().findParabolaIntersection(args->origin, args->velocity, args->acceleration, boundDistance, face, surfaceNormal)) {
+            // Don't add this cell if it's already farther than our best distance so far
+            if (boundDistance < args->parabolicDistance) {
+                distance = boundDistance;
+            }
+        }
+    }
+    return distance;
+}
+
 EntityItemID EntityTree::findParabolaIntersection(const PickParabola& parabola,
                                     QVector<EntityItemID> entityIdsToInclude, QVector<EntityItemID> entityIdsToDiscard,
                                     bool visibleOnly, bool collidableOnly, bool precisionPicking,
@@ -882,7 +929,7 @@ EntityItemID EntityTree::findParabolaIntersection(const PickParabola& parabola,
 
     bool requireLock = lockType == Octree::Lock;
     bool lockResult = withReadLock([&] {
-        recurseTreeWithOperation(findParabolaIntersectionOp, &args);
+        recurseTreeWithOperationSorted(findParabolaIntersectionOp, findParabolaIntersectionSortingOp, &args);
     }, requireLock);
 
     if (accurateResult) {

libraries/entities/src/EntityTreeElement.cpp

@@ -140,31 +140,18 @@ bool EntityTreeElement::bestFitBounds(const glm::vec3& minPoint, const glm::vec3
 }
 
 EntityItemID EntityTreeElement::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
-    bool& keepSearching, OctreeElementPointer& element, float& distance,
-    BoxFace& face, glm::vec3& surfaceNormal, const QVector<EntityItemID>& entityIdsToInclude,
-    const QVector<EntityItemID>& entityIdsToDiscard, bool visibleOnly, bool collidableOnly,
-    QVariantMap& extraInfo, bool precisionPicking) {
+    OctreeElementPointer& element, float& distance, BoxFace& face, glm::vec3& surfaceNormal,
+    const QVector<EntityItemID>& entityIdsToInclude, const QVector<EntityItemID>& entityIdsToDiscard,
+    bool visibleOnly, bool collidableOnly, QVariantMap& extraInfo, bool precisionPicking) {
 
     EntityItemID result;
-    float distanceToElementCube = std::numeric_limits<float>::max();
     BoxFace localFace;
     glm::vec3 localSurfaceNormal;
 
-    // if the ray doesn't intersect with our cube OR the distance to element is less than current best distance
-    // we can stop searching!
-    bool hit = _cube.findRayIntersection(origin, direction, distanceToElementCube, localFace, localSurfaceNormal);
-    if (!hit || (!_cube.contains(origin) && distanceToElementCube > distance)) {
-        keepSearching = false; // no point in continuing to search
-        return result; // we did not intersect
-    }
-
-    // by default, we only allow intersections with leaves with content
     if (!canPickIntersect()) {
-        return result; // we don't intersect with non-leaves, and we keep searching
+        return result;
     }
 
-    // if the distance to the element cube is not less than the current best distance, then it's not possible
-    // for any details inside the cube to be closer so we don't need to consider them.
     QVariantMap localExtraInfo;
     float distanceToElementDetails = distance;
     EntityItemID entityID = findDetailedRayIntersection(origin, direction, element, distanceToElementDetails,
@@ -228,7 +215,7 @@ EntityItemID EntityTreeElement::findDetailedRayIntersection(const glm::vec3& ori
         float localDistance;
         BoxFace localFace;
         glm::vec3 localSurfaceNormal;
-        if (entityFrameBox.findRayIntersection(entityFrameOrigin, entityFrameDirection, localDistance,
+        if (entityFrameBox.findRayIntersection(entityFrameOrigin, entityFrameDirection, 1.0f / entityFrameDirection, localDistance,
                                                 localFace, localSurfaceNormal)) {
             if (entityFrameBox.contains(entityFrameOrigin) || localDistance < distance) {
                 // now ask the entity if we actually intersect
@@ -289,31 +276,19 @@ bool EntityTreeElement::findSpherePenetration(const glm::vec3& center, float rad
 }
 
 EntityItemID EntityTreeElement::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity,
-    const glm::vec3& acceleration, bool& keepSearching, OctreeElementPointer& element, float& parabolicDistance,
+    const glm::vec3& acceleration, OctreeElementPointer& element, float& parabolicDistance,
     BoxFace& face, glm::vec3& surfaceNormal, const QVector<EntityItemID>& entityIdsToInclude,
     const QVector<EntityItemID>& entityIdsToDiscard, bool visibleOnly, bool collidableOnly,
     QVariantMap& extraInfo, bool precisionPicking) {
 
     EntityItemID result;
-    float distanceToElementCube = std::numeric_limits<float>::max();
     BoxFace localFace;
     glm::vec3 localSurfaceNormal;
 
-    // if the parabola doesn't intersect with our cube OR the distance to element is less than current best distance
-    // we can stop searching!
-    bool hit = _cube.findParabolaIntersection(origin, velocity, acceleration, distanceToElementCube, localFace, localSurfaceNormal);
-    if (!hit || (!_cube.contains(origin) && distanceToElementCube > parabolicDistance)) {
-        keepSearching = false; // no point in continuing to search
-        return result; // we did not intersect
-    }
-
-    // by default, we only allow intersections with leaves with content
     if (!canPickIntersect()) {
-        return result; // we don't intersect with non-leaves, and we keep searching
+        return result;
     }
 
-    // if the distance to the element cube is not less than the current best distance, then it's not possible
-    // for any details inside the cube to be closer so we don't need to consider them.
     QVariantMap localExtraInfo;
     float distanceToElementDetails = parabolicDistance;
     // We can precompute the world-space parabola normal and reuse it for the parabola plane intersects AABox sphere check

libraries/entities/src/EntityTreeElement.h

@@ -136,10 +136,9 @@ public:
 
     virtual bool canPickIntersect() const override { return hasEntities(); }
     virtual EntityItemID findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
-        bool& keepSearching, OctreeElementPointer& element, float& distance,
-        BoxFace& face, glm::vec3& surfaceNormal, const QVector<EntityItemID>& entityIdsToInclude,
-        const QVector<EntityItemID>& entityIdsToDiscard, bool visibleOnly, bool collidableOnly,
-        QVariantMap& extraInfo, bool precisionPicking = false);
+        OctreeElementPointer& element, float& distance, BoxFace& face, glm::vec3& surfaceNormal,
+        const QVector<EntityItemID>& entityIdsToInclude, const QVector<EntityItemID>& entityIdsToDiscard,
+        bool visibleOnly, bool collidableOnly, QVariantMap& extraInfo, bool precisionPicking = false);
     virtual EntityItemID findDetailedRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
                          OctreeElementPointer& element, float& distance,
                          BoxFace& face, glm::vec3& surfaceNormal, const QVector<EntityItemID>& entityIdsToInclude,
@@ -149,7 +148,7 @@ public:
                         glm::vec3& penetration, void** penetratedObject) const override;
 
     virtual EntityItemID findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity,
-        const glm::vec3& acceleration, bool& keepSearching, OctreeElementPointer& element, float& parabolicDistance,
+        const glm::vec3& acceleration, OctreeElementPointer& element, float& parabolicDistance,
         BoxFace& face, glm::vec3& surfaceNormal, const QVector<EntityItemID>& entityIdsToInclude,
         const QVector<EntityItemID>& entityIdsToDiscard, bool visibleOnly, bool collidableOnly,
         QVariantMap& extraInfo, bool precisionPicking = false);

libraries/entities/src/ShapeEntityItem.cpp

@@ -256,20 +256,18 @@ bool ShapeEntityItem::findDetailedRayIntersection(const glm::vec3& origin, const
     glm::mat4 entityToWorldMatrix = getEntityToWorldMatrix();
     glm::mat4 worldToEntityMatrix = glm::inverse(entityToWorldMatrix);
     glm::vec3 entityFrameOrigin = glm::vec3(worldToEntityMatrix * glm::vec4(origin, 1.0f));
-    glm::vec3 entityFrameDirection = glm::normalize(glm::vec3(worldToEntityMatrix * glm::vec4(direction, 0.0f)));
+    glm::vec3 entityFrameDirection = glm::vec3(worldToEntityMatrix * glm::vec4(direction, 0.0f));
 
-    float localDistance;
     // NOTE: unit sphere has center of 0,0,0 and radius of 0.5
-    if (findRaySphereIntersection(entityFrameOrigin, entityFrameDirection, glm::vec3(0.0f), 0.5f, localDistance)) {
-        // determine where on the unit sphere the hit point occured
-        glm::vec3 entityFrameHitAt = entityFrameOrigin + (entityFrameDirection * localDistance);
-        // then translate back to work coordinates
-        glm::vec3 hitAt = glm::vec3(entityToWorldMatrix * glm::vec4(entityFrameHitAt, 1.0f));
-        distance = glm::distance(origin, hitAt);
+    if (findRaySphereIntersection(entityFrameOrigin, entityFrameDirection, glm::vec3(0.0f), 0.5f, distance)) {
         bool success;
-        // FIXME: this is only correct for uniformly scaled spheres
-        surfaceNormal = glm::normalize(hitAt - getCenterPosition(success));
-        if (!success) {
+        glm::vec3 center = getCenterPosition(success);
+        if (success) {
+            // FIXME: this is only correct for uniformly scaled spheres
+            // determine where on the unit sphere the hit point occured
+            glm::vec3 hitAt = origin + (direction * distance);
+            surfaceNormal = glm::normalize(hitAt - center);
+        } else {
             return false;
         }
         return true;
@@ -291,9 +289,11 @@ bool ShapeEntityItem::findDetailedParabolaIntersection(const glm::vec3& origin,
     // NOTE: unit sphere has center of 0,0,0 and radius of 0.5
     if (findParabolaSphereIntersection(entityFrameOrigin, entityFrameVelocity, entityFrameAcceleration, glm::vec3(0.0f), 0.5f, parabolicDistance)) {
         bool success;
-        // FIXME: this is only correct for uniformly scaled spheres
-        surfaceNormal = glm::normalize((origin + velocity * parabolicDistance + 0.5f * acceleration * parabolicDistance * parabolicDistance) - getCenterPosition(success));
-        if (!success) {
+        glm::vec3 center = getCenterPosition(success);
+        if (success) {
+            // FIXME: this is only correct for uniformly scaled spheres
+            surfaceNormal = glm::normalize((origin + velocity * parabolicDistance + 0.5f * acceleration * parabolicDistance * parabolicDistance) - center);
+        } else {
             return false;
         }
         return true;

libraries/fbx/src/GLTFReader.cpp

@@ -935,7 +935,7 @@ FBXGeometry* GLTFReader::readGLTF(QByteArray& model, const QVariantHash& mapping
 }
 
 bool GLTFReader::readBinary(const QString& url, QByteArray& outdata) {
-    QUrl binaryUrl = _url.resolved(QUrl(url).fileName());
+    QUrl binaryUrl = _url.resolved(url);
 
     qCDebug(modelformat) << "binaryUrl: " << binaryUrl << "  OriginalUrl: " << _url;
     bool success;
@@ -948,7 +948,7 @@ bool GLTFReader::doesResourceExist(const QString& url) {
     if (_url.isEmpty()) {
         return false;
     }
-    QUrl candidateUrl = _url.resolved(QUrl(url).fileName());
+    QUrl candidateUrl = _url.resolved(url);
     return DependencyManager::get<ResourceManager>()->resourceExists(candidateUrl);
 }
 
@@ -1001,8 +1001,9 @@ FBXTexture GLTFReader::getFBXTexture(const GLTFTexture& texture) {
     fbxtex.texcoordSet = 0;
     
     if (texture.defined["source"]) {
-        QString fname = QUrl(_file.images[texture.source].uri).fileName();
-        QUrl textureUrl = _url.resolved(fname);
+        QString url = _file.images[texture.source].uri;
+        QString fname = QUrl(url).fileName();
+        QUrl textureUrl = _url.resolved(url);
         qCDebug(modelformat) << "fname: " << fname;
         qCDebug(modelformat) << "textureUrl: " << textureUrl;
         qCDebug(modelformat) << "Url: " << _url;

libraries/fbx/src/OBJReader.cpp

@@ -263,15 +263,19 @@ void OBJReader::parseMaterialLibrary(QIODevice* device) {
             default:
                 materials[matName] = currentMaterial;
                 #ifdef WANT_DEBUG
-                qCDebug(modelformat) << "OBJ Reader Last material illumination model:" << currentMaterial.illuminationModel <<
-                                     " shininess:" << currentMaterial.shininess << " opacity:" << currentMaterial.opacity <<
-                                     " diffuse color:" << currentMaterial.diffuseColor << " specular color:" << 
-                                     currentMaterial.specularColor << " emissive color:" << currentMaterial.emissiveColor << 
-                                     " diffuse texture:" << currentMaterial.diffuseTextureFilename << " specular texture:" <<
-                                     currentMaterial.specularTextureFilename << " emissive texture:" <<
-                                     currentMaterial.emissiveTextureFilename << " bump texture:" <<
-                                     currentMaterial.bumpTextureFilename;
-                #endif
+                qCDebug(modelformat) << 
+                                     "OBJ Reader Last material illumination model:" << currentMaterial.illuminationModel <<
+                                     " shininess:" << currentMaterial.shininess << 
+                                     " opacity:" << currentMaterial.opacity <<
+                                     " diffuse color:" << currentMaterial.diffuseColor << 
+                                     " specular color:" << currentMaterial.specularColor << 
+                                     " emissive color:" << currentMaterial.emissiveColor << 
+                                     " diffuse texture:" << currentMaterial.diffuseTextureFilename << 
+                                     " specular texture:" << currentMaterial.specularTextureFilename << 
+                                     " emissive texture:" << currentMaterial.emissiveTextureFilename << 
+                                     " bump texture:" << currentMaterial.bumpTextureFilename <<
+                                     " opacity texture:" << currentMaterial.opacityTextureFilename;
+#endif
                 return;
         }
         QByteArray token = tokenizer.getDatum();
@@ -289,6 +293,8 @@ void OBJReader::parseMaterialLibrary(QIODevice* device) {
             currentMaterial.emissiveTextureFilename = "";
             currentMaterial.specularTextureFilename = "";
             currentMaterial.bumpTextureFilename = "";
+            currentMaterial.opacityTextureFilename = "";
+
         } else if (token == "Ns") {
             currentMaterial.shininess = tokenizer.getFloat();
         } else if (token == "Ni") {
@@ -321,7 +327,7 @@ void OBJReader::parseMaterialLibrary(QIODevice* device) {
             currentMaterial.emissiveColor = tokenizer.getVec3();
         } else if (token == "Ks") {
             currentMaterial.specularColor = tokenizer.getVec3();
-        } else if ((token == "map_Kd") || (token == "map_Ke") || (token == "map_Ks") || (token == "map_bump") || (token == "bump")) {
+        } else if ((token == "map_Kd") || (token == "map_Ke") || (token == "map_Ks") || (token == "map_bump") || (token == "bump") || (token == "map_d")) {
             const QByteArray textureLine = tokenizer.getLineAsDatum();
             QByteArray filename;
             OBJMaterialTextureOptions textureOptions;
@@ -341,6 +347,8 @@ void OBJReader::parseMaterialLibrary(QIODevice* device) {
             } else if ((token == "map_bump") || (token == "bump")) {
                 currentMaterial.bumpTextureFilename = filename;
                 currentMaterial.bumpTextureOptions = textureOptions;
+            } else if (token == "map_d") {
+                currentMaterial.opacityTextureFilename = filename;
             }
         }
     }
@@ -900,6 +908,9 @@ FBXGeometry::Pointer OBJReader::readOBJ(QByteArray& model, const QVariantHash& m
             fbxMaterial.normalTexture.isBumpmap = true;
             fbxMaterial.bumpMultiplier = objMaterial.bumpTextureOptions.bumpMultiplier;
         }
+        if (!objMaterial.opacityTextureFilename.isEmpty()) {
+            fbxMaterial.opacityTexture.filename = objMaterial.opacityTextureFilename;
+        }
 
         modelMaterial->setEmissive(fbxMaterial.emissiveColor);
         modelMaterial->setAlbedo(fbxMaterial.diffuseColor);

libraries/fbx/src/OBJReader.h

@@ -66,6 +66,8 @@ public:
     QByteArray specularTextureFilename;
     QByteArray emissiveTextureFilename;
     QByteArray bumpTextureFilename;
+    QByteArray opacityTextureFilename;
+
     OBJMaterialTextureOptions bumpTextureOptions;
     int illuminationModel;
     bool used { false };

libraries/gpu-gl-common/src/gpu/gl/GLBackend.cpp

@@ -765,7 +765,6 @@ void GLBackend::recycle() const {
     }
     
     _textureManagement._transferEngine->manageMemory();
-    Texture::KtxStorage::releaseOpenKtxFiles();
 }
 
 void GLBackend::setCameraCorrection(const Mat4& correction, const Mat4& prevRenderView, bool reset) {

libraries/gpu-gl-common/src/gpu/gl/GLTextureTransfer.cpp

@@ -405,6 +405,7 @@ bool GLTextureTransferEngineDefault::processActiveBufferQueue() {
         _activeTransferQueue.splice(_activeTransferQueue.end(), activeBufferQueue);
     }
 
+    Texture::KtxStorage::releaseOpenKtxFiles();
     return true;
 }
 

libraries/input-plugins/src/input-plugins/KeyboardMouseDevice.cpp

@@ -302,6 +302,8 @@ controller::Input::NamedVector KeyboardMouseDevice::InputDevice::getAvailableInp
         availableInputs.append(Input::NamedPair(makeInput(Qt::Key_PageDown), QKeySequence(Qt::Key_PageDown).toString()));
         availableInputs.append(Input::NamedPair(makeInput(Qt::Key_Tab), QKeySequence(Qt::Key_Tab).toString()));
         availableInputs.append(Input::NamedPair(makeInput(Qt::Key_Control), "Control"));
+        availableInputs.append(Input::NamedPair(makeInput(Qt::Key_Delete), "Delete"));
+        availableInputs.append(Input::NamedPair(makeInput(Qt::Key_Backspace), QKeySequence(Qt::Key_Backspace).toString()));
 
         availableInputs.append(Input::NamedPair(makeInput(Qt::LeftButton), "LeftMouseButton"));
         availableInputs.append(Input::NamedPair(makeInput(Qt::MiddleButton), "MiddleMouseButton"));

libraries/networking/src/AccountManager.h

@@ -96,6 +96,8 @@ public:
 
     QUrl getMetaverseServerURL() { return NetworkingConstants::METAVERSE_SERVER_URL(); }
 
+    void removeAccountFromFile();
+
 public slots:
     void requestAccessToken(const QString& login, const QString& password);
     void requestAccessTokenWithSteam(QByteArray authSessionTicket);
@@ -133,7 +135,6 @@ private:
     void operator=(AccountManager const& other) = delete;
 
     void persistAccountToFile();
-    void removeAccountFromFile();
 
     void passSuccessToCallback(QNetworkReply* reply);
     void passErrorToCallback(QNetworkReply* reply);

libraries/networking/src/NodeList.cpp

@@ -273,6 +273,7 @@ void NodeList::reset(bool skipDomainHandlerReset) {
 
     // refresh the owner UUID to the NULL UUID
     setSessionUUID(QUuid());
+    setSessionLocalID(Node::NULL_LOCAL_ID);
 
     // if we setup the DTLS socket, also disconnect from the DTLS socket readyRead() so it can handle handshaking
     if (_dtlsSocket) {
@@ -647,6 +648,23 @@ void NodeList::processDomainServerList(QSharedPointer<ReceivedMessage> message)
     Node::LocalID newLocalID;
     packetStream >> newUUID;
     packetStream >> newLocalID;
+
+    // when connected, if the session ID or local ID were not null and changed, we should reset
+    auto currentLocalID = getSessionLocalID();
+    auto currentSessionID = getSessionUUID();
+    if (_domainHandler.isConnected() &&
+        ((currentLocalID != Node::NULL_LOCAL_ID && newLocalID != currentLocalID) ||
+        (!currentSessionID.isNull() && newUUID != currentSessionID))) {
+            qCDebug(networking) << "Local ID or Session ID changed while connected to domain - forcing NodeList reset";
+
+            // reset the nodelist, but don't do a domain handler reset since we're about to process a good domain list
+            reset(true);
+
+            // tell the domain handler that we're no longer connected so that below
+            // it can re-perform actions as if we just connected
+            _domainHandler.setIsConnected(false);
+    }
+
     setSessionLocalID(newLocalID);
     setSessionUUID(newUUID);
 

libraries/octree/src/Octree.cpp

@@ -68,55 +68,12 @@ Octree::~Octree() {
     eraseAllOctreeElements(false);
 }
 
-
-// Inserts the value and key into three arrays sorted by the key array, the first array is the value,
-// the second array is a sorted key for the value, the third array is the index for the value in it original
-// non-sorted array
-// returns -1 if size exceeded
-// originalIndexArray is optional
-int insertOctreeElementIntoSortedArrays(const OctreeElementPointer& value, float key, int originalIndex,
-                                        OctreeElementPointer* valueArray, float* keyArray, int* originalIndexArray,
-                                        int currentCount, int maxCount) {
-
-    if (currentCount < maxCount) {
-        int i = 0;
-        if (currentCount > 0) {
-            while (i < currentCount && key > keyArray[i]) {
-                i++;
-            }
-            // i is our desired location
-            // shift array elements to the right
-            if (i < currentCount && i+1 < maxCount) {
-                for (int j = currentCount - 1; j > i; j--) {
-                    valueArray[j] = valueArray[j - 1];
-                    keyArray[j] = keyArray[j - 1];
-                }
-            }
-        }
-        // place new element at i
-        valueArray[i] = value;
-        keyArray[i] = key;
-        if (originalIndexArray) {
-            originalIndexArray[i] = originalIndex;
-        }
-        return currentCount + 1;
-    }
-    return -1; // error case
-}
-
-
-
 // Recurses voxel tree calling the RecurseOctreeOperation function for each element.
 // stops recursion if operation function returns false.
 void Octree::recurseTreeWithOperation(const RecurseOctreeOperation& operation, void* extraData) {
     recurseElementWithOperation(_rootElement, operation, extraData);
 }
 
-// Recurses voxel tree calling the RecurseOctreePostFixOperation function for each element in post-fix order.
-void Octree::recurseTreeWithPostOperation(const RecurseOctreeOperation& operation, void* extraData) {
-    recurseElementWithPostOperation(_rootElement, operation, extraData);
-}
-
 // Recurses voxel element with an operation function
 void Octree::recurseElementWithOperation(const OctreeElementPointer& element, const RecurseOctreeOperation& operation, void* extraData,
                         int recursionCount) {
@@ -129,71 +86,53 @@ void Octree::recurseElementWithOperation(const OctreeElementPointer& element, co
         for (int i = 0; i < NUMBER_OF_CHILDREN; i++) {
             OctreeElementPointer child = element->getChildAtIndex(i);
             if (child) {
-                recurseElementWithOperation(child, operation, extraData, recursionCount+1);
+                recurseElementWithOperation(child, operation, extraData, recursionCount + 1);
             }
         }
     }
 }
 
-// Recurses voxel element with an operation function
-void Octree::recurseElementWithPostOperation(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
-                                             void* extraData, int recursionCount) {
+void Octree::recurseTreeWithOperationSorted(const RecurseOctreeOperation& operation, const RecurseOctreeSortingOperation& sortingOperation, void* extraData) {
+    recurseElementWithOperationSorted(_rootElement, operation, sortingOperation, extraData);
+}
+
+// Recurses voxel element with an operation function, calling operation on its children in a specific order
+bool Octree::recurseElementWithOperationSorted(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
+                                               const RecurseOctreeSortingOperation& sortingOperation, void* extraData, int recursionCount) {
     if (recursionCount > DANGEROUSLY_DEEP_RECURSION) {
-        HIFI_FCDEBUG(octree(), "Octree::recurseElementWithPostOperation() reached DANGEROUSLY_DEEP_RECURSION, bailing!");
-        return;
+        HIFI_FCDEBUG(octree(), "Octree::recurseElementWithOperationSorted() reached DANGEROUSLY_DEEP_RECURSION, bailing!");
+        // If we go too deep, we want to keep searching other paths
+        return true;
     }
 
+    bool keepSearching = operation(element, extraData);
+
+    std::vector<SortedChild> sortedChildren;
     for (int i = 0; i < NUMBER_OF_CHILDREN; i++) {
         OctreeElementPointer child = element->getChildAtIndex(i);
         if (child) {
-            recurseElementWithPostOperation(child, operation, extraData, recursionCount+1);
-        }
-    }
-    operation(element, extraData);
-}
-
-// Recurses voxel tree calling the RecurseOctreeOperation function for each element.
-// stops recursion if operation function returns false.
-void Octree::recurseTreeWithOperationDistanceSorted(const RecurseOctreeOperation& operation,
-                                                       const glm::vec3& point, void* extraData) {
-
-    recurseElementWithOperationDistanceSorted(_rootElement, operation, point, extraData);
-}
-
-// Recurses voxel element with an operation function
-void Octree::recurseElementWithOperationDistanceSorted(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
-                                                       const glm::vec3& point, void* extraData, int recursionCount) {
-
-    if (recursionCount > DANGEROUSLY_DEEP_RECURSION) {
-        HIFI_FCDEBUG(octree(), "Octree::recurseElementWithOperationDistanceSorted() reached DANGEROUSLY_DEEP_RECURSION, bailing!");
-        return;
-    }
-
-    if (operation(element, extraData)) {
-        // determine the distance sorted order of our children
-        OctreeElementPointer sortedChildren[NUMBER_OF_CHILDREN] = { NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL };
-        float distancesToChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-        int indexOfChildren[NUMBER_OF_CHILDREN] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-        int currentCount = 0;
-
-        for (int i = 0; i < NUMBER_OF_CHILDREN; i++) {
-            OctreeElementPointer childElement = element->getChildAtIndex(i);
-            if (childElement) {
-                // chance to optimize, doesn't need to be actual distance!! Could be distance squared
-                float distanceSquared = childElement->distanceSquareToPoint(point);
-                currentCount = insertOctreeElementIntoSortedArrays(childElement, distanceSquared, i,
-                                                                   sortedChildren, (float*)&distancesToChildren,
-                                                                   (int*)&indexOfChildren, currentCount, NUMBER_OF_CHILDREN);
-            }
-        }
-
-        for (int i = 0; i < currentCount; i++) {
-            OctreeElementPointer childElement = sortedChildren[i];
-            if (childElement) {
-                recurseElementWithOperationDistanceSorted(childElement, operation, point, extraData);
+            float priority = sortingOperation(child, extraData);
+            if (priority < FLT_MAX) {
+                sortedChildren.emplace_back(priority, child);
             }
         }
     }
+
+    if (sortedChildren.size() > 1) {
+        static auto comparator = [](const SortedChild& left, const SortedChild& right) { return left.first < right.first; };
+        std::sort(sortedChildren.begin(), sortedChildren.end(), comparator);
+    }
+
+    for (auto it = sortedChildren.begin(); it != sortedChildren.end(); ++it) {
+        const SortedChild& sortedChild = *it;
+        // Our children were sorted, so if one hits something, we don't need to check the others
+        if (!recurseElementWithOperationSorted(sortedChild.second, operation, sortingOperation, extraData, recursionCount + 1)) {
+            return false;
+        }
+    }
+    // We checked all our children and didn't find anything.
+    // Stop if we hit something in this element.  Continue if we didn't.
+    return keepSearching;
 }
 
 void Octree::recurseTreeWithOperator(RecurseOctreeOperator* operatorObject) {

libraries/octree/src/Octree.h

@@ -49,6 +49,9 @@ public:
 
 // Callback function, for recuseTreeWithOperation
 using RecurseOctreeOperation = std::function<bool(const OctreeElementPointer&, void*)>;
+// Function for sorting octree children during recursion.  If return value == FLT_MAX, child is discarded
+using RecurseOctreeSortingOperation = std::function<float(const OctreeElementPointer&, void*)>;
+using SortedChild = std::pair<float, OctreeElementPointer>;
 typedef QHash<uint, AACube> CubeList;
 
 const bool NO_EXISTS_BITS         = false;
@@ -163,17 +166,10 @@ public:
     OctreeElementPointer getOrCreateChildElementContaining(const AACube& box);
 
     void recurseTreeWithOperation(const RecurseOctreeOperation& operation, void* extraData = NULL);
-    void recurseTreeWithPostOperation(const RecurseOctreeOperation& operation, void* extraData = NULL);
-
-    /// \param operation type of operation
-    /// \param point point in world-frame (meters)
-    /// \param extraData hook for user data to be interpreted by special context
-    void recurseTreeWithOperationDistanceSorted(const RecurseOctreeOperation& operation,
-                                                const glm::vec3& point, void* extraData = NULL);
+    void recurseTreeWithOperationSorted(const RecurseOctreeOperation& operation, const RecurseOctreeSortingOperation& sortingOperation, void* extraData = NULL);
 
     void recurseTreeWithOperator(RecurseOctreeOperator* operatorObject);
 
-
     bool isDirty() const { return _isDirty; }
     void clearDirtyBit() { _isDirty = false; }
     void setDirtyBit() { _isDirty = true; }
@@ -227,14 +223,8 @@ public:
 
     void recurseElementWithOperation(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
                 void* extraData, int recursionCount = 0);
-
-    /// Traverse child nodes of node applying operation in post-fix order
-    ///
-    void recurseElementWithPostOperation(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
-                void* extraData, int recursionCount = 0);
-
-    void recurseElementWithOperationDistanceSorted(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
-                const glm::vec3& point, void* extraData, int recursionCount = 0);
+    bool recurseElementWithOperationSorted(const OctreeElementPointer& element, const RecurseOctreeOperation& operation,
+        const RecurseOctreeSortingOperation& sortingOperation, void* extraData, int recursionCount = 0);
 
     bool recurseElementWithOperator(const OctreeElementPointer& element, RecurseOctreeOperator* operatorObject, int recursionCount = 0);
 

libraries/physics/src/EntityMotionState.cpp

@@ -778,8 +778,10 @@ void EntityMotionState::computeCollisionGroupAndMask(int32_t& group, int32_t& ma
 
 bool EntityMotionState::shouldSendBid() const {
     // NOTE: this method is only ever called when the entity's simulation is NOT locally owned
-    return _body->isActive() && (_region == workload::Region::R1) &&
-        glm::max(glm::max(VOLUNTEER_SIMULATION_PRIORITY, _bumpedPriority), _entity->getScriptSimulationPriority()) >= _entity->getSimulationPriority();
+    return _body->isActive()
+        && (_region == workload::Region::R1)
+        && glm::max(glm::max(VOLUNTEER_SIMULATION_PRIORITY, _bumpedPriority), _entity->getScriptSimulationPriority()) >= _entity->getSimulationPriority()
+        && !_entity->getLocked();
 }
 
 uint8_t EntityMotionState::computeFinalBidPriority() const {

libraries/physics/src/PhysicsEngine.cpp

@@ -898,11 +898,12 @@ void PhysicsEngine::setShowBulletConstraintLimits(bool value) {
 }
 
 struct AllContactsCallback : public btCollisionWorld::ContactResultCallback {
-    AllContactsCallback(int32_t mask, int32_t group, const ShapeInfo& shapeInfo, const Transform& transform, btCollisionObject* myAvatarCollisionObject) :
+    AllContactsCallback(int32_t mask, int32_t group, const ShapeInfo& shapeInfo, const Transform& transform, btCollisionObject* myAvatarCollisionObject, float threshold) :
         btCollisionWorld::ContactResultCallback(),
         collisionObject(),
         contacts(),
-        myAvatarCollisionObject(myAvatarCollisionObject) {
+        myAvatarCollisionObject(myAvatarCollisionObject),
+        threshold(threshold) {
         const btCollisionShape* collisionShape = ObjectMotionState::getShapeManager()->getShape(shapeInfo);
 
         collisionObject.setCollisionShape(const_cast<btCollisionShape*>(collisionShape));
@@ -924,8 +925,13 @@ struct AllContactsCallback : public btCollisionWorld::ContactResultCallback {
     btCollisionObject collisionObject;
     std::vector<ContactTestResult> contacts;
     btCollisionObject* myAvatarCollisionObject;
+    btScalar threshold;
 
     btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0, int partId0, int index0, const btCollisionObjectWrapper* colObj1, int partId1, int index1) override {
+        if (cp.m_distance1 > -threshold) {
+            return 0;
+        }
+
         const btCollisionObject* otherBody;
         btVector3 penetrationPoint;
         btVector3 otherPenetrationPoint;
@@ -968,14 +974,14 @@ protected:
     }
 };
 
-std::vector<ContactTestResult> PhysicsEngine::contactTest(uint16_t mask, const ShapeInfo& regionShapeInfo, const Transform& regionTransform, uint16_t group) const {
+std::vector<ContactTestResult> PhysicsEngine::contactTest(uint16_t mask, const ShapeInfo& regionShapeInfo, const Transform& regionTransform, uint16_t group, float threshold) const {
     // TODO: Give MyAvatar a motion state so we don't have to do this
     btCollisionObject* myAvatarCollisionObject = nullptr;
     if ((mask & USER_COLLISION_GROUP_MY_AVATAR) && _myAvatarController) {
         myAvatarCollisionObject = _myAvatarController->getCollisionObject();
     }
 
-    auto contactCallback = AllContactsCallback((int32_t)mask, (int32_t)group, regionShapeInfo, regionTransform, myAvatarCollisionObject);
+    auto contactCallback = AllContactsCallback((int32_t)mask, (int32_t)group, regionShapeInfo, regionTransform, myAvatarCollisionObject, threshold);
     _dynamicsWorld->contactTest(&contactCallback.collisionObject, contactCallback);
 
     return contactCallback.contacts;

libraries/physics/src/PhysicsEngine.h

@@ -142,7 +142,7 @@ public:
 
     // Function for getting colliding objects in the world of specified type
     // See PhysicsCollisionGroups.h for mask flags.
-    std::vector<ContactTestResult> contactTest(uint16_t mask, const ShapeInfo& regionShapeInfo, const Transform& regionTransform, uint16_t group = USER_COLLISION_GROUP_DYNAMIC) const;
+    std::vector<ContactTestResult> contactTest(uint16_t mask, const ShapeInfo& regionShapeInfo, const Transform& regionTransform, uint16_t group = USER_COLLISION_GROUP_DYNAMIC, float threshold = 0.0f) const;
 
 private:
     QList<EntityDynamicPointer> removeDynamicsForBody(btRigidBody* body);

libraries/pointers/src/Pick.cpp

@@ -72,11 +72,15 @@ PickResultPointer PickQuery::getPrevPickResult() const {
 void PickQuery::setIgnoreItems(const QVector<QUuid>& ignoreItems) {
     withWriteLock([&] {
         _ignoreItems = ignoreItems;
+        // We sort these items here so the PickCacheOptimizer can catch cases where two picks have the same ignoreItems in a different order
+        std::sort(_ignoreItems.begin(), _ignoreItems.end(), std::less<QUuid>());
     });
 }
 
 void PickQuery::setIncludeItems(const QVector<QUuid>& includeItems) {
     withWriteLock([&] {
         _includeItems = includeItems;
+        // We sort these items here so the PickCacheOptimizer can catch cases where two picks have the same includeItems in a different order
+        std::sort(_includeItems.begin(), _includeItems.end(), std::less<QUuid>());
     });
 }

libraries/pointers/src/Pick.h

@@ -17,6 +17,7 @@
 #include <QVariant>
 
 #include <shared/ReadWriteLockable.h>
+#include <TransformNode.h>
 
 enum IntersectionType {
     NONE = 0,
@@ -213,6 +214,10 @@ public:
     virtual bool isRightHand() const { return false; }
     virtual bool isMouse() const { return false; }
 
+    virtual Transform getResultTransform() const = 0;
+
+    std::shared_ptr<TransformNode> parentTransform;
+
 private:
     PickFilter _filter;
     const float _maxDistance;

libraries/pointers/src/PickCacheOptimizer.h

@@ -37,7 +37,7 @@ template<typename T>
 class PickCacheOptimizer {
 
 public:
-    void update(std::unordered_map<uint32_t, std::shared_ptr<PickQuery>>& picks, uint32_t& nextToUpdate, uint64_t expiry, bool shouldPickHUD);
+    QVector4D update(std::unordered_map<uint32_t, std::shared_ptr<PickQuery>>& picks, uint32_t& nextToUpdate, uint64_t expiry, bool shouldPickHUD);
 
 protected:
     typedef std::unordered_map<T, std::unordered_map<PickCacheKey, PickResultPointer>> PickCache;
@@ -57,8 +57,8 @@ bool PickCacheOptimizer<T>::checkAndCompareCachedResults(T& pick, PickCache& cac
 }
 
 template<typename T>
-void PickCacheOptimizer<T>::cacheResult(const bool intersects, const PickResultPointer& resTemp, const PickCacheKey& key, PickResultPointer& res, T& mathPick, PickCache& cache, const std::shared_ptr<Pick<T>> pick) {
-    if (intersects) {
+void PickCacheOptimizer<T>::cacheResult(const bool needToCompareResults, const PickResultPointer& resTemp, const PickCacheKey& key, PickResultPointer& res, T& mathPick, PickCache& cache, const std::shared_ptr<Pick<T>> pick) {
+    if (needToCompareResults) {
         cache[mathPick][key] = resTemp;
         res = res->compareAndProcessNewResult(resTemp);
     } else {
@@ -67,8 +67,9 @@ void PickCacheOptimizer<T>::cacheResult(const bool intersects, const PickResultP
 }
 
 template<typename T>
-void PickCacheOptimizer<T>::update(std::unordered_map<uint32_t, std::shared_ptr<PickQuery>>& picks,
+QVector4D PickCacheOptimizer<T>::update(std::unordered_map<uint32_t, std::shared_ptr<PickQuery>>& picks,
         uint32_t& nextToUpdate, uint64_t expiry, bool shouldPickHUD) {
+    QVector4D numIntersectionsComputed;
     PickCache results;
     const uint32_t INVALID_PICK_ID = 0;
     auto itr = picks.begin();
@@ -91,6 +92,7 @@ void PickCacheOptimizer<T>::update(std::unordered_map<uint32_t, std::shared_ptr<
                 PickCacheKey entityKey = { pick->getFilter().getEntityFlags(), pick->getIncludeItems(), pick->getIgnoreItems() };
                 if (!checkAndCompareCachedResults(mathematicalPick, results, res, entityKey)) {
                     PickResultPointer entityRes = pick->getEntityIntersection(mathematicalPick);
+                    numIntersectionsComputed[0]++;
                     if (entityRes) {
                         cacheResult(entityRes->doesIntersect(), entityRes, entityKey, res, mathematicalPick, results, pick);
                     }
@@ -101,6 +103,7 @@ void PickCacheOptimizer<T>::update(std::unordered_map<uint32_t, std::shared_ptr<
                 PickCacheKey overlayKey = { pick->getFilter().getOverlayFlags(), pick->getIncludeItems(), pick->getIgnoreItems() };
                 if (!checkAndCompareCachedResults(mathematicalPick, results, res, overlayKey)) {
                     PickResultPointer overlayRes = pick->getOverlayIntersection(mathematicalPick);
+                    numIntersectionsComputed[1]++;
                     if (overlayRes) {
                         cacheResult(overlayRes->doesIntersect(), overlayRes, overlayKey, res, mathematicalPick, results, pick);
                     }
@@ -111,6 +114,7 @@ void PickCacheOptimizer<T>::update(std::unordered_map<uint32_t, std::shared_ptr<
                 PickCacheKey avatarKey = { pick->getFilter().getAvatarFlags(), pick->getIncludeItems(), pick->getIgnoreItems() };
                 if (!checkAndCompareCachedResults(mathematicalPick, results, res, avatarKey)) {
                     PickResultPointer avatarRes = pick->getAvatarIntersection(mathematicalPick);
+                    numIntersectionsComputed[2]++;
                     if (avatarRes) {
                         cacheResult(avatarRes->doesIntersect(), avatarRes, avatarKey, res, mathematicalPick, results, pick);
                     }
@@ -122,6 +126,7 @@ void PickCacheOptimizer<T>::update(std::unordered_map<uint32_t, std::shared_ptr<
                 PickCacheKey hudKey = { pick->getFilter().getHUDFlags(), QVector<QUuid>(), QVector<QUuid>() };
                 if (!checkAndCompareCachedResults(mathematicalPick, results, res, hudKey)) {
                     PickResultPointer hudRes = pick->getHUDIntersection(mathematicalPick);
+                    numIntersectionsComputed[3]++;
                     if (hudRes) {
                         cacheResult(true, hudRes, hudKey, res, mathematicalPick, results, pick);
                     }
@@ -145,6 +150,7 @@ void PickCacheOptimizer<T>::update(std::unordered_map<uint32_t, std::shared_ptr<
             break;
         }
     }
+    return numIntersectionsComputed;
 }
 
 #endif // hifi_PickCacheOptimizer_h

libraries/pointers/src/PickManager.cpp

@@ -20,6 +20,7 @@ unsigned int PickManager::addPick(PickQuery::PickType type, const std::shared_pt
             id = _nextPickID++;
             _picks[type][id] = pick;
             _typeMap[id] = type;
+            _totalPickCounts[type]++;
         }
     });
     return id;
@@ -41,6 +42,7 @@ void PickManager::removePick(unsigned int uid) {
         if (type != _typeMap.end()) {
             _picks[type->second].erase(uid);
             _typeMap.erase(uid);
+            _totalPickCounts[type->second]--;
         }
     });
 }
@@ -88,6 +90,14 @@ void PickManager::setIncludeItems(unsigned int uid, const QVector<QUuid>& includ
     }
 }
 
+Transform PickManager::getResultTransform(unsigned int uid) const {
+    auto pick = findPick(uid);
+    if (pick) {
+        return pick->getResultTransform();
+    }
+    return Transform();
+}
+
 void PickManager::update() {
     uint64_t expiry = usecTimestampNow() + _perFrameTimeBudget;
     std::unordered_map<PickQuery::PickType, std::unordered_map<unsigned int, std::shared_ptr<PickQuery>>> cachedPicks;
@@ -96,12 +106,12 @@ void PickManager::update() {
     });
 
     bool shouldPickHUD = _shouldPickHUDOperator();
-    // we pass the same expiry to both updates, but the stylus updates are relatively cheap
-    // and the rayPicks updae will ALWAYS update at least one ray even when there is no budget
-    _stylusPickCacheOptimizer.update(cachedPicks[PickQuery::Stylus], _nextPickToUpdate[PickQuery::Stylus], expiry, false);
-    _rayPickCacheOptimizer.update(cachedPicks[PickQuery::Ray], _nextPickToUpdate[PickQuery::Ray], expiry, shouldPickHUD);
-    _parabolaPickCacheOptimizer.update(cachedPicks[PickQuery::Parabola], _nextPickToUpdate[PickQuery::Parabola], expiry, shouldPickHUD);
-    _collisionPickCacheOptimizer.update(cachedPicks[PickQuery::Collision], _nextPickToUpdate[PickQuery::Collision], expiry, false);
+    // FIXME: give each type its own expiry
+    // Each type will update at least one pick, regardless of the expiry
+    _updatedPickCounts[PickQuery::Stylus] = _stylusPickCacheOptimizer.update(cachedPicks[PickQuery::Stylus], _nextPickToUpdate[PickQuery::Stylus], expiry, false);
+    _updatedPickCounts[PickQuery::Ray] = _rayPickCacheOptimizer.update(cachedPicks[PickQuery::Ray], _nextPickToUpdate[PickQuery::Ray], expiry, shouldPickHUD);
+    _updatedPickCounts[PickQuery::Parabola] = _parabolaPickCacheOptimizer.update(cachedPicks[PickQuery::Parabola], _nextPickToUpdate[PickQuery::Parabola], expiry, shouldPickHUD);
+    _updatedPickCounts[PickQuery::Collision] = _collisionPickCacheOptimizer.update(cachedPicks[PickQuery::Collision], _nextPickToUpdate[PickQuery::Collision], expiry, false);
 }
 
 bool PickManager::isLeftHand(unsigned int uid) {

libraries/pointers/src/PickManager.h

@@ -16,7 +16,10 @@
 
 #include <NumericalConstants.h>
 
-class PickManager : public Dependency, protected ReadWriteLockable {
+#include <QObject>
+
+class PickManager : public QObject, public Dependency, protected ReadWriteLockable {
+    Q_OBJECT
     SINGLETON_DEPENDENCY
 
 public:
@@ -40,6 +43,8 @@ public:
     void setIgnoreItems(unsigned int uid, const QVector<QUuid>& ignore) const;
     void setIncludeItems(unsigned int uid, const QVector<QUuid>& include) const;
 
+    Transform getResultTransform(unsigned int uid) const;
+
     bool isLeftHand(unsigned int uid);
     bool isRightHand(unsigned int uid);
     bool isMouse(unsigned int uid);
@@ -53,7 +58,19 @@ public:
     unsigned int getPerFrameTimeBudget() const { return _perFrameTimeBudget; }
     void setPerFrameTimeBudget(unsigned int numUsecs) { _perFrameTimeBudget = numUsecs; }
 
+    bool getForceCoarsePicking() { return _forceCoarsePicking; }
+
+    const std::vector<QVector4D>& getUpdatedPickCounts() { return _updatedPickCounts; }
+    const std::vector<int>& getTotalPickCounts() { return _totalPickCounts; }
+
+public slots:
+    void setForceCoarsePicking(bool forceCoarsePicking) { _forceCoarsePicking = forceCoarsePicking; }
+
 protected:
+    std::vector<QVector4D> _updatedPickCounts { PickQuery::NUM_PICK_TYPES };
+    std::vector<int> _totalPickCounts { 0, 0, 0, 0 };
+
+    bool _forceCoarsePicking { false };
     std::function<bool()> _shouldPickHUDOperator;
     std::function<glm::vec2(const glm::vec3&)> _calculatePos2DFromHUDOperator;
 

libraries/pointers/src/PickTransformNode.cpp

@@ -0,0 +1,26 @@
+//
+//  Created by Sabrina Shanman 8/22/2018
+//  Copyright 2018 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "PickTransformNode.h"
+
+#include "DependencyManager.h"
+#include "PickManager.h"
+
+PickTransformNode::PickTransformNode(unsigned int uid) :
+    _uid(uid)
+{
+}
+
+Transform PickTransformNode::getTransform() {
+    auto pickManager = DependencyManager::get<PickManager>();
+    if (!pickManager) {
+        return Transform();
+    }
+
+    return pickManager->getResultTransform(_uid);
+}

libraries/pointers/src/PickTransformNode.h

@@ -0,0 +1,23 @@
+//
+//  Created by Sabrina Shanman 8/22/2018
+//  Copyright 2018 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_PickTransformNode_h
+#define hifi_PickTransformNode_h
+
+#include "TransformNode.h"
+
+// TODO: Remove this class when Picks are converted to SpatiallyNestables
+class PickTransformNode : public TransformNode {
+public:
+    PickTransformNode(unsigned int uid);
+    Transform getTransform() override;
+
+protected:
+    unsigned int _uid;
+};
+
+#endif // hifi_PickTransformNode_h

libraries/pointers/src/Pointer.h

@@ -50,6 +50,8 @@ public:
     virtual void setRenderState(const std::string& state) = 0;
     virtual void editRenderState(const std::string& state, const QVariant& startProps, const QVariant& pathProps, const QVariant& endProps) = 0;
     
+    virtual QVariantMap toVariantMap() const = 0;
+
     virtual void setPrecisionPicking(bool precisionPicking);
     virtual void setIgnoreItems(const QVector<QUuid>& ignoreItems) const;
     virtual void setIncludeItems(const QVector<QUuid>& includeItems) const;

libraries/pointers/src/PointerManager.cpp

@@ -77,6 +77,15 @@ PickResultPointer PointerManager::getPrevPickResult(unsigned int uid) const {
     return result;
 }
 
+QVariantMap PointerManager::getPointerProperties(unsigned int uid) const {
+    auto pointer = find(uid);
+    if (pointer) {
+        return pointer->toVariantMap();
+    } else {
+        return QVariantMap();
+    }
+}
+
 void PointerManager::update() {
     auto cachedPointers = resultWithReadLock<std::unordered_map<unsigned int, std::shared_ptr<Pointer>>>([&] {
         return _pointers;

libraries/pointers/src/PointerManager.h

@@ -30,6 +30,7 @@ public:
     void setRenderState(unsigned int uid, const std::string& renderState) const;
     void editRenderState(unsigned int uid, const std::string& state, const QVariant& startProps, const QVariant& pathProps, const QVariant& endProps) const;
     PickResultPointer getPrevPickResult(unsigned int uid) const;
+    QVariantMap getPointerProperties(unsigned int uid) const;
 
     void setPrecisionPicking(unsigned int uid, bool precisionPicking) const;
     void setIgnoreItems(unsigned int uid, const QVector<QUuid>& ignoreEntities) const;

libraries/render-utils/src/CauterizedModel.cpp

@@ -86,9 +86,8 @@ void CauterizedModel::createRenderItemSet() {
             // Create the render payloads
             int numParts = (int)mesh->getNumParts();
             for (int partIndex = 0; partIndex < numParts; partIndex++) {
-                if (!fbxGeometry.meshes[i].blendshapes.empty() && !_blendedVertexBuffers[i]) {
-                    _blendedVertexBuffers[i] = std::make_shared<gpu::Buffer>();
-                    _blendedVertexBuffers[i]->resize(fbxGeometry.meshes[i].vertices.size() * (sizeof(glm::vec3) + 2 * sizeof(NormalType)));
+                if (!fbxGeometry.meshes[i].blendshapes.empty()) {
+                    initializeBlendshapes(fbxGeometry.meshes[i], i);
                 }
                 auto ptr = std::make_shared<CauterizedMeshPartPayload>(shared_from_this(), i, partIndex, shapeID, transform, offset);
                 _modelMeshRenderItems << std::static_pointer_cast<ModelMeshPartPayload>(ptr);
@@ -103,7 +102,7 @@ void CauterizedModel::createRenderItemSet() {
     }
 }
 
-void CauterizedModel::updateClusterMatrices() {
+void CauterizedModel::updateClusterMatrices(bool triggerBlendshapes) {
     PerformanceTimer perfTimer("CauterizedModel::updateClusterMatrices");
 
     if (!_needsUpdateClusterMatrices || !isLoaded()) {
@@ -176,7 +175,7 @@ void CauterizedModel::updateClusterMatrices() {
 
     // post the blender if we're not currently waiting for one to finish
     auto modelBlender = DependencyManager::get<ModelBlender>();
-    if (modelBlender->shouldComputeBlendshapes() && geometry.hasBlendedMeshes() && _blendshapeCoefficients != _blendedBlendshapeCoefficients) {
+    if (triggerBlendshapes && modelBlender->shouldComputeBlendshapes() && geometry.hasBlendedMeshes() && _blendshapeCoefficients != _blendedBlendshapeCoefficients) {
         _blendedBlendshapeCoefficients = _blendshapeCoefficients;
         modelBlender->noteRequiresBlend(getThisPointer());
     }

libraries/render-utils/src/CauterizedModel.h

@@ -33,7 +33,7 @@ public:
 
     void createRenderItemSet() override;
     
-    virtual void updateClusterMatrices() override;
+    virtual void updateClusterMatrices(bool triggerBlendshapes = true) override;
     void updateRenderItems() override;
 
     const Model::MeshState& getCauterizeMeshState(int index) const;

libraries/render-utils/src/Model.cpp

@@ -302,52 +302,11 @@ bool Model::updateGeometry() {
         assert(_meshStates.empty());
 
         const FBXGeometry& fbxGeometry = getFBXGeometry();
-        int i = 0;
         foreach (const FBXMesh& mesh, fbxGeometry.meshes) {
             MeshState state;
             state.clusterDualQuaternions.resize(mesh.clusters.size());
             state.clusterMatrices.resize(mesh.clusters.size());
             _meshStates.push_back(state);
-
-            if (!mesh.blendshapes.isEmpty()) {
-                if (!_blendedVertexBuffers[i]) {
-                    _blendedVertexBuffers[i] = std::make_shared<gpu::Buffer>();
-                }
-                const auto& buffer = _blendedVertexBuffers[i];
-                QVector<NormalType> normalsAndTangents;
-                normalsAndTangents.resize(2 * mesh.normals.size());
-
-                // Interleave normals and tangents
-                // Parallel version for performance
-                tbb::parallel_for(tbb::blocked_range<int>(0, mesh.normals.size()), [&](const tbb::blocked_range<int>& range) {
-                    auto normalsRange = std::make_pair(mesh.normals.begin() + range.begin(), mesh.normals.begin() + range.end());
-                    auto tangentsRange = std::make_pair(mesh.tangents.begin() + range.begin(), mesh.tangents.begin() + range.end());
-                    auto normalsAndTangentsIt = normalsAndTangents.begin() + 2 * range.begin();
-
-                    for (auto normalIt = normalsRange.first, tangentIt = tangentsRange.first;
-                         normalIt != normalsRange.second;
-                         ++normalIt, ++tangentIt) {
-#if FBX_PACK_NORMALS
-                        glm::uint32 finalNormal;
-                        glm::uint32 finalTangent;
-                        buffer_helpers::packNormalAndTangent(*normalIt, *tangentIt, finalNormal, finalTangent);
-#else
-                        const auto& finalNormal = *normalIt;
-                        const auto& finalTangent = *tangentIt;
-#endif
-                        *normalsAndTangentsIt = finalNormal;
-                        ++normalsAndTangentsIt;
-                        *normalsAndTangentsIt = finalTangent;
-                        ++normalsAndTangentsIt;
-                    }
-                });
-                const auto verticesSize = mesh.vertices.size() * sizeof(glm::vec3);
-                buffer->resize(mesh.vertices.size() * sizeof(glm::vec3) + normalsAndTangents.size() * sizeof(NormalType));
-                buffer->setSubData(0, verticesSize, (const gpu::Byte*) mesh.vertices.constData());
-                buffer->setSubData(verticesSize, 2 * mesh.normals.size() * sizeof(NormalType), (const gpu::Byte*) normalsAndTangents.data());
-                mesh.normalsAndTangents = normalsAndTangents;
-            }
-            i++;
         }
         needFullUpdate = true;
         emit rigReady();
@@ -388,17 +347,20 @@ bool Model::findRayIntersectionAgainstSubMeshes(const glm::vec3& origin, const g
 
     // we can use the AABox's intersection by mapping our origin and direction into the model frame
     // and testing intersection there.
-    if (modelFrameBox.findRayIntersection(modelFrameOrigin, modelFrameDirection, distance, face, surfaceNormal)) {
+    if (modelFrameBox.findRayIntersection(modelFrameOrigin, modelFrameDirection, 1.0f / modelFrameDirection, distance, face, surfaceNormal)) {
         QMutexLocker locker(&_mutex);
 
-        float bestDistance = std::numeric_limits<float>::max();
+        float bestDistance = FLT_MAX;
+        BoxFace bestFace;
         Triangle bestModelTriangle;
         Triangle bestWorldTriangle;
+        glm::vec3 bestWorldIntersectionPoint;
+        glm::vec3 bestMeshIntersectionPoint;
+        int bestPartIndex = 0;
+        int bestShapeID = 0;
         int bestSubMeshIndex = 0;
 
-        int subMeshIndex = 0;
         const FBXGeometry& geometry = getFBXGeometry();
-
         if (!_triangleSetsValid) {
             calculateTriangleSets(geometry);
         }
@@ -409,41 +371,78 @@ bool Model::findRayIntersectionAgainstSubMeshes(const glm::vec3& origin, const g
 
         glm::vec3 meshFrameOrigin = glm::vec3(worldToMeshMatrix * glm::vec4(origin, 1.0f));
         glm::vec3 meshFrameDirection = glm::vec3(worldToMeshMatrix * glm::vec4(direction, 0.0f));
+        glm::vec3 meshFrameInvDirection = 1.0f / meshFrameDirection;
 
         int shapeID = 0;
+        int subMeshIndex = 0;
+
+        std::vector<SortedTriangleSet> sortedTriangleSets;
         for (auto& meshTriangleSets : _modelSpaceMeshTriangleSets) {
             int partIndex = 0;
-            for (auto &partTriangleSet : meshTriangleSets) {
-                float triangleSetDistance;
-                BoxFace triangleSetFace;
-                Triangle triangleSetTriangle;
-                if (partTriangleSet.findRayIntersection(meshFrameOrigin, meshFrameDirection, triangleSetDistance, triangleSetFace, triangleSetTriangle, pickAgainstTriangles, allowBackface)) {
-                    glm::vec3 meshIntersectionPoint = meshFrameOrigin + (meshFrameDirection * triangleSetDistance);
-                    glm::vec3 worldIntersectionPoint = glm::vec3(meshToWorldMatrix * glm::vec4(meshIntersectionPoint, 1.0f));
-                    float worldDistance = glm::distance(origin, worldIntersectionPoint);
-
-                    if (worldDistance < bestDistance) {
-                        bestDistance = worldDistance;
-                        intersectedSomething = true;
-                        face = triangleSetFace;
-                        bestModelTriangle = triangleSetTriangle;
-                        bestWorldTriangle = triangleSetTriangle * meshToWorldMatrix;
-                        extraInfo["worldIntersectionPoint"] = vec3toVariant(worldIntersectionPoint);
-                        extraInfo["meshIntersectionPoint"] = vec3toVariant(meshIntersectionPoint);
-                        extraInfo["partIndex"] = partIndex;
-                        extraInfo["shapeID"] = shapeID;
-                        bestSubMeshIndex = subMeshIndex;
+            for (auto& partTriangleSet : meshTriangleSets) {
+                float priority = FLT_MAX;
+                if (partTriangleSet.getBounds().contains(meshFrameOrigin)) {
+                    priority = 0.0f;
+                } else {
+                    float partBoundDistance = FLT_MAX;
+                    BoxFace partBoundFace;
+                    glm::vec3 partBoundNormal;
+                    if (partTriangleSet.getBounds().findRayIntersection(meshFrameOrigin, meshFrameDirection, meshFrameInvDirection,
+                                                                        partBoundDistance, partBoundFace, partBoundNormal)) {
+                        priority = partBoundDistance;
                     }
                 }
+
+                if (priority < FLT_MAX) {
+                    sortedTriangleSets.emplace_back(priority, &partTriangleSet, partIndex, shapeID, subMeshIndex);
+                }
                 partIndex++;
                 shapeID++;
             }
             subMeshIndex++;
         }
 
+        if (sortedTriangleSets.size() > 1) {
+            static auto comparator = [](const SortedTriangleSet& left, const SortedTriangleSet& right) { return left.distance < right.distance; };
+            std::sort(sortedTriangleSets.begin(), sortedTriangleSets.end(), comparator);
+        }
+
+        for (auto it = sortedTriangleSets.begin(); it != sortedTriangleSets.end(); ++it) {
+            const SortedTriangleSet& sortedTriangleSet = *it;
+            // We can exit once triangleSetDistance > bestDistance
+            if (sortedTriangleSet.distance > bestDistance) {
+                break;
+            }
+            float triangleSetDistance = FLT_MAX;
+            BoxFace triangleSetFace;
+            Triangle triangleSetTriangle;
+            if (sortedTriangleSet.triangleSet->findRayIntersection(meshFrameOrigin, meshFrameDirection, meshFrameInvDirection, triangleSetDistance, triangleSetFace,
+                                                                   triangleSetTriangle, pickAgainstTriangles, allowBackface)) {
+                if (triangleSetDistance < bestDistance) {
+                    bestDistance = triangleSetDistance;
+                    intersectedSomething = true;
+                    bestFace = triangleSetFace;
+                    bestModelTriangle = triangleSetTriangle;
+                    bestWorldTriangle = triangleSetTriangle * meshToWorldMatrix;
+                    glm::vec3 meshIntersectionPoint = meshFrameOrigin + (meshFrameDirection * triangleSetDistance);
+                    glm::vec3 worldIntersectionPoint = glm::vec3(meshToWorldMatrix * glm::vec4(meshIntersectionPoint, 1.0f));
+                    bestWorldIntersectionPoint = worldIntersectionPoint;
+                    bestMeshIntersectionPoint = meshIntersectionPoint;
+                    bestPartIndex = sortedTriangleSet.partIndex;
+                    bestShapeID = sortedTriangleSet.shapeID;
+                    bestSubMeshIndex = sortedTriangleSet.subMeshIndex;
+                }
+            }
+        }
+
         if (intersectedSomething) {
             distance = bestDistance;
+            face = bestFace;
             surfaceNormal = bestWorldTriangle.getNormal();
+            extraInfo["worldIntersectionPoint"] = vec3toVariant(bestWorldIntersectionPoint);
+            extraInfo["meshIntersectionPoint"] = vec3toVariant(bestMeshIntersectionPoint);
+            extraInfo["partIndex"] = bestPartIndex;
+            extraInfo["shapeID"] = bestShapeID;
             if (pickAgainstTriangles) {
                 extraInfo["subMeshIndex"] = bestSubMeshIndex;
                 extraInfo["subMeshName"] = geometry.getModelNameOfMesh(bestSubMeshIndex);
@@ -495,13 +494,16 @@ bool Model::findParabolaIntersectionAgainstSubMeshes(const glm::vec3& origin, co
         QMutexLocker locker(&_mutex);
 
         float bestDistance = FLT_MAX;
+        BoxFace bestFace;
         Triangle bestModelTriangle;
         Triangle bestWorldTriangle;
+        glm::vec3 bestWorldIntersectionPoint;
+        glm::vec3 bestMeshIntersectionPoint;
+        int bestPartIndex = 0;
+        int bestShapeID = 0;
         int bestSubMeshIndex = 0;
 
-        int subMeshIndex = 0;
         const FBXGeometry& geometry = getFBXGeometry();
-
         if (!_triangleSetsValid) {
             calculateTriangleSets(geometry);
         }
@@ -515,40 +517,79 @@ bool Model::findParabolaIntersectionAgainstSubMeshes(const glm::vec3& origin, co
         glm::vec3 meshFrameAcceleration = glm::vec3(worldToMeshMatrix * glm::vec4(acceleration, 0.0f));
 
         int shapeID = 0;
+        int subMeshIndex = 0;
+
+        std::vector<SortedTriangleSet> sortedTriangleSets;
         for (auto& meshTriangleSets : _modelSpaceMeshTriangleSets) {
             int partIndex = 0;
-            for (auto &partTriangleSet : meshTriangleSets) {
-                float triangleSetDistance;
-                BoxFace triangleSetFace;
-                Triangle triangleSetTriangle;
-                if (partTriangleSet.findParabolaIntersection(meshFrameOrigin, meshFrameVelocity, meshFrameAcceleration,
-                    triangleSetDistance, triangleSetFace, triangleSetTriangle, pickAgainstTriangles, allowBackface)) {
-                    if (triangleSetDistance < bestDistance) {
-                        bestDistance = triangleSetDistance;
-                        intersectedSomething = true;
-                        face = triangleSetFace;
-                        bestModelTriangle = triangleSetTriangle;
-                        bestWorldTriangle = triangleSetTriangle * meshToWorldMatrix;
-                        glm::vec3 meshIntersectionPoint = meshFrameOrigin + meshFrameVelocity * triangleSetDistance +
-                            0.5f * meshFrameAcceleration * triangleSetDistance * triangleSetDistance;
-                        glm::vec3 worldIntersectionPoint = origin + velocity * triangleSetDistance +
-                            0.5f * acceleration * triangleSetDistance * triangleSetDistance;
-                        extraInfo["worldIntersectionPoint"] = vec3toVariant(worldIntersectionPoint);
-                        extraInfo["meshIntersectionPoint"] = vec3toVariant(meshIntersectionPoint);
-                        extraInfo["partIndex"] = partIndex;
-                        extraInfo["shapeID"] = shapeID;
-                        bestSubMeshIndex = subMeshIndex;
+            for (auto& partTriangleSet : meshTriangleSets) {
+                float priority = FLT_MAX;
+                if (partTriangleSet.getBounds().contains(meshFrameOrigin)) {
+                    priority = 0.0f;
+                } else {
+                    float partBoundDistance = FLT_MAX;
+                    BoxFace partBoundFace;
+                    glm::vec3 partBoundNormal;
+                    if (partTriangleSet.getBounds().findParabolaIntersection(meshFrameOrigin, meshFrameVelocity, meshFrameAcceleration,
+                                                                             partBoundDistance, partBoundFace, partBoundNormal)) {
+                        priority = partBoundDistance;
                     }
                 }
+
+                if (priority < FLT_MAX) {
+                    sortedTriangleSets.emplace_back(priority, &partTriangleSet, partIndex, shapeID, subMeshIndex);
+                }
                 partIndex++;
                 shapeID++;
             }
             subMeshIndex++;
         }
 
+        if (sortedTriangleSets.size() > 1) {
+            static auto comparator = [](const SortedTriangleSet& left, const SortedTriangleSet& right) { return left.distance < right.distance; };
+            std::sort(sortedTriangleSets.begin(), sortedTriangleSets.end(), comparator);
+        }
+
+        for (auto it = sortedTriangleSets.begin(); it != sortedTriangleSets.end(); ++it) {
+            const SortedTriangleSet& sortedTriangleSet = *it;
+            // We can exit once triangleSetDistance > bestDistance
+            if (sortedTriangleSet.distance > bestDistance) {
+                break;
+            }
+            float triangleSetDistance = FLT_MAX;
+            BoxFace triangleSetFace;
+            Triangle triangleSetTriangle;
+            if (sortedTriangleSet.triangleSet->findParabolaIntersection(meshFrameOrigin, meshFrameVelocity, meshFrameAcceleration,
+                                                                        triangleSetDistance, triangleSetFace, triangleSetTriangle,
+                                                                        pickAgainstTriangles, allowBackface)) {
+                if (triangleSetDistance < bestDistance) {
+                    bestDistance = triangleSetDistance;
+                    intersectedSomething = true;
+                    bestFace = triangleSetFace;
+                    bestModelTriangle = triangleSetTriangle;
+                    bestWorldTriangle = triangleSetTriangle * meshToWorldMatrix;
+                    glm::vec3 meshIntersectionPoint = meshFrameOrigin + meshFrameVelocity * triangleSetDistance +
+                        0.5f * meshFrameAcceleration * triangleSetDistance * triangleSetDistance;
+                    glm::vec3 worldIntersectionPoint = origin + velocity * triangleSetDistance +
+                        0.5f * acceleration * triangleSetDistance * triangleSetDistance;
+                    bestWorldIntersectionPoint = worldIntersectionPoint;
+                    bestMeshIntersectionPoint = meshIntersectionPoint;
+                    bestPartIndex = sortedTriangleSet.partIndex;
+                    bestShapeID = sortedTriangleSet.shapeID;
+                    bestSubMeshIndex = sortedTriangleSet.subMeshIndex;
+                    // These sets can overlap, so we can't exit early if we find something
+                }
+            }
+        }
+
         if (intersectedSomething) {
             parabolicDistance = bestDistance;
+            face = bestFace;
             surfaceNormal = bestWorldTriangle.getNormal();
+            extraInfo["worldIntersectionPoint"] = vec3toVariant(bestWorldIntersectionPoint);
+            extraInfo["meshIntersectionPoint"] = vec3toVariant(bestMeshIntersectionPoint);
+            extraInfo["partIndex"] = bestPartIndex;
+            extraInfo["shapeID"] = bestShapeID;
             if (pickAgainstTriangles) {
                 extraInfo["subMeshIndex"] = bestSubMeshIndex;
                 extraInfo["subMeshName"] = geometry.getModelNameOfMesh(bestSubMeshIndex);
@@ -935,7 +976,7 @@ bool Model::addToScene(const render::ScenePointer& scene,
                        render::Transaction& transaction,
                        render::Item::Status::Getters& statusGetters) {
     if (!_addedToScene && isLoaded()) {
-        updateClusterMatrices();
+        updateClusterMatrices(false);
         if (_modelMeshRenderItems.empty()) {
             createRenderItemSet();
         }
@@ -1445,7 +1486,7 @@ void Model::computeMeshPartLocalBounds() {
 }
 
 // virtual
-void Model::updateClusterMatrices() {
+void Model::updateClusterMatrices(bool triggerBlendshapes) {
     DETAILED_PERFORMANCE_TIMER("Model::updateClusterMatrices");
 
     if (!_needsUpdateClusterMatrices || !isLoaded()) {
@@ -1474,7 +1515,7 @@ void Model::updateClusterMatrices() {
 
     // post the blender if we're not currently waiting for one to finish
     auto modelBlender = DependencyManager::get<ModelBlender>();
-    if (modelBlender->shouldComputeBlendshapes() && geometry.hasBlendedMeshes() && _blendshapeCoefficients != _blendedBlendshapeCoefficients) {
+    if (triggerBlendshapes && modelBlender->shouldComputeBlendshapes() && geometry.hasBlendedMeshes() && _blendshapeCoefficients != _blendedBlendshapeCoefficients) {
         _blendedBlendshapeCoefficients = _blendshapeCoefficients;
         modelBlender->noteRequiresBlend(getThisPointer());
     }
@@ -1514,10 +1555,10 @@ void Model::setBlendedVertices(int blendNumber, const Geometry::WeakPointer& geo
         assert(buffer);
         buffer->resize(mesh.vertices.size() * sizeof(glm::vec3) + mesh.normalsAndTangents.size() * sizeof(NormalType));
         buffer->setSubData(0, verticesSize, (gpu::Byte*) vertices.constData() + index * sizeof(glm::vec3));
-        buffer->setSubData(verticesSize, 2 * mesh.normals.size() * sizeof(NormalType), (const gpu::Byte*) normalsAndTangents.data() + normalAndTangentIndex * sizeof(NormalType));
+        buffer->setSubData(verticesSize, mesh.normalsAndTangents.size() * sizeof(NormalType), (const gpu::Byte*) normalsAndTangents.data() + normalAndTangentIndex * sizeof(NormalType));
 
         index += vertexCount;
-        normalAndTangentIndex += 2 * mesh.normals.size();
+        normalAndTangentIndex += mesh.normalsAndTangents.size();
     }
 }
 
@@ -1555,6 +1596,42 @@ const render::ItemIDs& Model::fetchRenderItemIDs() const {
     return _modelMeshRenderItemIDs;
 }
 
+void Model::initializeBlendshapes(const FBXMesh& mesh, int index) {
+    QVector<NormalType> normalsAndTangents;
+    normalsAndTangents.resize(2 * mesh.normals.size());
+
+    // Interleave normals and tangents
+    // Parallel version for performance
+    tbb::parallel_for(tbb::blocked_range<int>(0, mesh.normals.size()), [&](const tbb::blocked_range<int>& range) {
+        auto normalsRange = std::make_pair(mesh.normals.begin() + range.begin(), mesh.normals.begin() + range.end());
+        auto tangentsRange = std::make_pair(mesh.tangents.begin() + range.begin(), mesh.tangents.begin() + range.end());
+        auto normalsAndTangentsIt = normalsAndTangents.begin() + 2 * range.begin();
+
+        for (auto normalIt = normalsRange.first, tangentIt = tangentsRange.first;
+            normalIt != normalsRange.second;
+            ++normalIt, ++tangentIt) {
+#if FBX_PACK_NORMALS
+            glm::uint32 finalNormal;
+            glm::uint32 finalTangent;
+            buffer_helpers::packNormalAndTangent(*normalIt, *tangentIt, finalNormal, finalTangent);
+#else
+            const auto& finalNormal = *normalIt;
+            const auto& finalTangent = *tangentIt;
+#endif
+            *normalsAndTangentsIt = finalNormal;
+            ++normalsAndTangentsIt;
+            *normalsAndTangentsIt = finalTangent;
+            ++normalsAndTangentsIt;
+        }
+    });
+    const auto verticesSize = mesh.vertices.size() * sizeof(glm::vec3);
+    _blendedVertexBuffers[index] = std::make_shared<gpu::Buffer>();
+    _blendedVertexBuffers[index]->resize(mesh.vertices.size() * sizeof(glm::vec3) + normalsAndTangents.size() * sizeof(NormalType));
+    _blendedVertexBuffers[index]->setSubData(0, verticesSize, (const gpu::Byte*) mesh.vertices.constData());
+    _blendedVertexBuffers[index]->setSubData(verticesSize, normalsAndTangents.size() * sizeof(NormalType), (const gpu::Byte*) normalsAndTangents.data());
+    mesh.normalsAndTangents = normalsAndTangents;
+}
+
 void Model::createRenderItemSet() {
     assert(isLoaded());
     const auto& meshes = _renderGeometry->getMeshes();
@@ -1583,7 +1660,7 @@ void Model::createRenderItemSet() {
     // Run through all of the meshes, and place them into their segregated, but unsorted buckets
     int shapeID = 0;
     uint32_t numMeshes = (uint32_t)meshes.size();
-    auto fbxGeometry = getFBXGeometry();
+    auto& fbxGeometry = getFBXGeometry();
     for (uint32_t i = 0; i < numMeshes; i++) {
         const auto& mesh = meshes.at(i);
         if (!mesh) {
@@ -1593,8 +1670,8 @@ void Model::createRenderItemSet() {
         // Create the render payloads
         int numParts = (int)mesh->getNumParts();
         for (int partIndex = 0; partIndex < numParts; partIndex++) {
-            if (fbxGeometry.meshes[i].blendshapes.empty() && !_blendedVertexBuffers[i]) {
-                _blendedVertexBuffers[i] = std::make_shared<gpu::Buffer>();
+            if (!fbxGeometry.meshes[i].blendshapes.empty()) {
+                initializeBlendshapes(fbxGeometry.meshes[i], i);
             }
             _modelMeshRenderItems << std::make_shared<ModelMeshPartPayload>(shared_from_this(), i, partIndex, shapeID, transform, offset);
             auto material = getGeometry()->getShapeMaterial(shapeID);

libraries/render-utils/src/Model.h

@@ -64,6 +64,16 @@ class Model;
 using ModelPointer = std::shared_ptr<Model>;
 using ModelWeakPointer = std::weak_ptr<Model>;
 
+struct SortedTriangleSet {
+    SortedTriangleSet(float distance, TriangleSet* triangleSet, int partIndex, int shapeID, int subMeshIndex) :
+        distance(distance), triangleSet(triangleSet), partIndex(partIndex), shapeID(shapeID), subMeshIndex(subMeshIndex) {}
+
+    float distance;
+    TriangleSet* triangleSet;
+    int partIndex;
+    int shapeID;
+    int subMeshIndex;
+};
 
 /// A generic 3D model displaying geometry loaded from a URL.
 class Model : public QObject, public std::enable_shared_from_this<Model>, public scriptable::ModelProvider {
@@ -149,7 +159,7 @@ public:
     bool getSnapModelToRegistrationPoint() { return _snapModelToRegistrationPoint; }
 
     virtual void simulate(float deltaTime, bool fullUpdate = true);
-    virtual void updateClusterMatrices();
+    virtual void updateClusterMatrices(bool triggerBlendshapes = true);
 
     /// Returns a reference to the shared geometry.
     const Geometry::Pointer& getGeometry() const { return _renderGeometry; }
@@ -482,6 +492,8 @@ protected:
 
     bool shouldInvalidatePayloadShapeKey(int meshIndex);
 
+    void initializeBlendshapes(const FBXMesh& mesh, int index);
+
 private:
     float _loadingPriority { 0.0f };
 

libraries/render-utils/src/PickItemsJob.cpp

@@ -33,6 +33,7 @@ void PickItemsJob::run(const render::RenderContextPointer& renderContext, const
 render::ItemBound PickItemsJob::findNearestItem(const render::RenderContextPointer& renderContext, const render::ItemBounds& inputs, float& minIsectDistance) const {
     const glm::vec3 rayOrigin = renderContext->args->getViewFrustum().getPosition();
     const glm::vec3 rayDirection = renderContext->args->getViewFrustum().getDirection();
+    const glm::vec3 rayInvDirection = 1.0f / rayDirection;
     BoxFace face;
     glm::vec3 normal;
     float isectDistance;
@@ -42,7 +43,7 @@ render::ItemBound PickItemsJob::findNearestItem(const render::RenderContextPoint
     render::ItemKey itemKey;
 
     for (const auto& itemBound : inputs) {
-        if (!itemBound.bound.contains(rayOrigin) && itemBound.bound.findRayIntersection(rayOrigin, rayDirection, isectDistance, face, normal)) {
+        if (!itemBound.bound.contains(rayOrigin) && itemBound.bound.findRayIntersection(rayOrigin, rayDirection, rayInvDirection, isectDistance, face, normal)) {
             auto& item = renderContext->_scene->getItem(itemBound.id);
             itemKey = item.getKey();
             if (itemKey.isWorldSpace() && isectDistance>minDistance && isectDistance < minIsectDistance && isectDistance<maxDistance

libraries/render-utils/src/SoftAttachmentModel.cpp

@@ -31,7 +31,7 @@ int SoftAttachmentModel::getJointIndexOverride(int i) const {
 
 // virtual
 // use the _rigOverride matrices instead of the Model::_rig
-void SoftAttachmentModel::updateClusterMatrices() {
+void SoftAttachmentModel::updateClusterMatrices(bool triggerBlendshapes) {
     if (!_needsUpdateClusterMatrices) {
         return;
     }
@@ -78,7 +78,7 @@ void SoftAttachmentModel::updateClusterMatrices() {
 
     // post the blender if we're not currently waiting for one to finish
     auto modelBlender = DependencyManager::get<ModelBlender>();
-    if (modelBlender->shouldComputeBlendshapes() && geometry.hasBlendedMeshes() && _blendshapeCoefficients != _blendedBlendshapeCoefficients) {
+    if (triggerBlendshapes && modelBlender->shouldComputeBlendshapes() && geometry.hasBlendedMeshes() && _blendshapeCoefficients != _blendedBlendshapeCoefficients) {
         _blendedBlendshapeCoefficients = _blendshapeCoefficients;
         modelBlender->noteRequiresBlend(getThisPointer());
     }

libraries/render-utils/src/SoftAttachmentModel.h

@@ -27,7 +27,7 @@ public:
     ~SoftAttachmentModel();
 
     void updateRig(float deltaTime, glm::mat4 parentTransform) override;
-    void updateClusterMatrices() override;
+    void updateClusterMatrices(bool triggerBlendshapes = true) override;
 
 protected:
     int getJointIndexOverride(int i) const;

libraries/shared/src/AABox.cpp

@@ -192,9 +192,9 @@ bool AABox::expandedIntersectsSegment(const glm::vec3& start, const glm::vec3& e
                 isWithin(start.x + axisDistance*direction.x, expandedCorner.x, expandedSize.x));
 }
 
-bool AABox::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance,
-                                BoxFace& face, glm::vec3& surfaceNormal) const {
-    return findRayAABoxIntersection(origin, direction, _corner, _scale, distance, face, surfaceNormal);
+bool AABox::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::vec3& invDirection,
+                                float& distance, BoxFace& face, glm::vec3& surfaceNormal) const {
+    return findRayAABoxIntersection(origin, direction, invDirection, _corner, _scale, distance, face, surfaceNormal);
 }
 
 bool AABox::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration,

libraries/shared/src/AABox.h

@@ -69,7 +69,7 @@ public:
 
     bool expandedContains(const glm::vec3& point, float expansion) const;
     bool expandedIntersectsSegment(const glm::vec3& start, const glm::vec3& end, float expansion) const;
-    bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance,
+    bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::vec3& invDirection, float& distance,
                              BoxFace& face, glm::vec3& surfaceNormal) const;
     bool findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration,
                                   float& parabolicDistance, BoxFace& face, glm::vec3& surfaceNormal) const;

libraries/shared/src/AACube.cpp

@@ -187,9 +187,9 @@ bool AACube::expandedIntersectsSegment(const glm::vec3& start, const glm::vec3&
                 isWithin(start.x + axisDistance*direction.x, expandedCorner.x, expandedSize.x));
 }
 
-bool AACube::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance, 
-                                    BoxFace& face, glm::vec3& surfaceNormal) const {
-    return findRayAABoxIntersection(origin, direction, _corner, glm::vec3(_scale), distance, face, surfaceNormal);
+bool AACube::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::vec3& invDirection,
+                                 float& distance, BoxFace& face, glm::vec3& surfaceNormal) const {
+    return findRayAABoxIntersection(origin, direction, invDirection, _corner, glm::vec3(_scale), distance, face, surfaceNormal);
 }
 
 bool AACube::findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration,

libraries/shared/src/AACube.h

@@ -56,10 +56,10 @@ public:
     bool touches(const AABox& otherBox) const;
     bool expandedContains(const glm::vec3& point, float expansion) const;
     bool expandedIntersectsSegment(const glm::vec3& start, const glm::vec3& end, float expansion) const;
-    bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance,
-                                BoxFace& face, glm::vec3& surfaceNormal) const;
+    bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, const glm::vec3& invDirection,
+                             float& distance, BoxFace& face, glm::vec3& surfaceNormal) const;
     bool findParabolaIntersection(const glm::vec3& origin, const glm::vec3& velocity, const glm::vec3& acceleration,
-                                float& parabolicDistance, BoxFace& face, glm::vec3& surfaceNormal) const;
+                                  float& parabolicDistance, BoxFace& face, glm::vec3& surfaceNormal) const;
     bool touchesSphere(const glm::vec3& center, float radius) const;
     bool findSpherePenetration(const glm::vec3& center, float radius, glm::vec3& penetration) const;
     bool findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration) const;

libraries/shared/src/AvatarConstants.h

@@ -34,7 +34,7 @@ const float DEFAULT_HANDS_ANGULAR_VELOCITY_STEPPING_THRESHOLD = 3.3f;
 const float DEFAULT_HEAD_VELOCITY_STEPPING_THRESHOLD = 0.18f;
 const float DEFAULT_HEAD_PITCH_STEPPING_TOLERANCE = 7.0f;
 const float DEFAULT_HEAD_ROLL_STEPPING_TOLERANCE = 7.0f;
-const float DEFAULT_AVATAR_SPINE_STRETCH_LIMIT = 0.07f;
+const float DEFAULT_AVATAR_SPINE_STRETCH_LIMIT = 0.04f;
 const float DEFAULT_AVATAR_FORWARD_DAMPENING_FACTOR = 0.5f;
 const float DEFAULT_AVATAR_LATERAL_DAMPENING_FACTOR = 2.0f;
 const float DEFAULT_AVATAR_HIPS_MASS = 40.0f;

libraries/shared/src/GLMHelpers.cpp

@@ -458,11 +458,6 @@ vec4 toGlm(const glm::u8vec3& color, float alpha) {
     return vec4(glm::vec3(color) / MAX_COLOR, alpha);
 }
 
-glm::u8vec3 vec3UCharFromGlm(const glm::vec3 & color) {
-    static const float MAX_COLOR = 255.0f;
-    return { (uint8_t)(color.x * MAX_COLOR), (uint8_t)(color.y * MAX_COLOR), (uint8_t)(color.z * MAX_COLOR) };
-}
-
 glm::vec4 toGlm(const QColor& color) {
     return glm::vec4(color.redF(), color.greenF(), color.blueF(), color.alphaF());
 }

libraries/shared/src/GLMHelpers.h

@@ -180,8 +180,6 @@ vec4 toGlm(const QColor& color);
 ivec4 toGlm(const QRect& rect);
 vec4 toGlm(const glm::u8vec3& color, float alpha);
 
-glm::u8vec3 vec3UCharFromGlm(const glm::vec3 & c);
-
 QSize fromGlm(const glm::ivec2 & v);
 QMatrix4x4 fromGlm(const glm::mat4 & m);