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

interface/resources/qml/hifi/tablet/tabletWindows/TabletPreferencesDialog.qml

@@ -42,7 +42,11 @@ Item {
         }
 
         if (HMD.active) {
-            tablet.popFromStack();
+            if (gotoPreviousApp) {
+                tablet.returnToPreviousApp();
+            } else {
+                tablet.popFromStack();
+            }
         } else {
             closeDialog();
         }
@@ -55,7 +59,11 @@ Item {
         }
 
         if (HMD.active) {
-            tablet.popFromStack();
+            if (gotoPreviousApp) {
+                tablet.returnToPreviousApp();
+            } else {
+                tablet.popFromStack();
+            }
         } else {
             closeDialog();
         }

interface/src/Application.cpp

@@ -2302,8 +2302,7 @@ Application::Application(int& argc, char** argv, QElapsedTimer& startupTimer, bo
     connect(&AndroidHelper::instance(), &AndroidHelper::enterBackground, this, &Application::enterBackground);
     connect(&AndroidHelper::instance(), &AndroidHelper::enterForeground, this, &Application::enterForeground);
     AndroidHelper::instance().notifyLoadComplete();
-#endif
+#else
-
     static int CHECK_LOGIN_TIMER = 3000;
     QTimer* checkLoginTimer = new QTimer(this);
     checkLoginTimer->setInterval(CHECK_LOGIN_TIMER);
@@ -2321,6 +2320,7 @@ Application::Application(int& argc, char** argv, QElapsedTimer& startupTimer, bo
     });
     Setting::Handle<bool>{"loginDialogPoppedUp", false}.set(false);
     checkLoginTimer->start();
+#endif
 }
 
 void Application::updateVerboseLogging() {

interface/src/avatar/MyAvatar.cpp

@@ -113,6 +113,7 @@ MyAvatar::MyAvatar(QThread* thread) :
     _recentModeReadings(MODE_READINGS_RING_BUFFER_SIZE),
     _bodySensorMatrix(),
     _goToPending(false),
+    _goToSafe(true),
     _goToPosition(),
     _goToOrientation(),
     _prevShouldDrawHead(true),
@@ -148,7 +149,8 @@ MyAvatar::MyAvatar(QThread* thread) :
     });
     connect(_skeletonModel.get(), &Model::rigReady, this, &Avatar::rigReady);
     connect(_skeletonModel.get(), &Model::rigReset, this, &Avatar::rigReset);
-
+    connect(&_skeletonModel->getRig(), &Rig::onLoadComplete, this, &MyAvatar::updateCollisionCapsuleCache);
+    connect(this, &MyAvatar::sensorToWorldScaleChanged, this, &MyAvatar::updateCollisionCapsuleCache);
     using namespace recording;
     _skeletonModel->flagAsCauterized();
 
@@ -254,6 +256,7 @@ MyAvatar::MyAvatar(QThread* thread) :
     });
 
     connect(&(_skeletonModel->getRig()), SIGNAL(onLoadComplete()), this, SIGNAL(onLoadComplete()));
+
     _characterController.setDensity(_density);
 }
 
@@ -509,7 +512,9 @@ void MyAvatar::update(float deltaTime) {
     if (_physicsSafetyPending && qApp->isPhysicsEnabled() && _characterController.isEnabledAndReady()) {
         // When needed and ready, arrange to check and fix.
         _physicsSafetyPending = false;
-        safeLanding(_goToPosition); // no-op if already safe
+        if (_goToSafe) {
+            safeLanding(_goToPosition); // no-op if already safe
+        }
     }
 
     Head* head = getHead();
@@ -3011,7 +3016,7 @@ void MyAvatar::goToFeetLocation(const glm::vec3& newPosition,
 
 void MyAvatar::goToLocation(const glm::vec3& newPosition,
                             bool hasOrientation, const glm::quat& newOrientation,
-                            bool shouldFaceLocation) {
+                            bool shouldFaceLocation, bool withSafeLanding) {
 
     // Most cases of going to a place or user go through this now. Some possible improvements to think about in the future:
     // - It would be nice if this used the same teleport steps and smoothing as in the teleport.js script, as long as it
@@ -3031,6 +3036,7 @@ void MyAvatar::goToLocation(const glm::vec3& newPosition,
 
     _goToPending = true;
     _goToPosition = newPosition;
+    _goToSafe = withSafeLanding;
     _goToOrientation = getWorldOrientation();
     if (hasOrientation) {
         qCDebug(interfaceapp).nospace() << "MyAvatar goToLocation - new orientation is "
@@ -3303,6 +3309,22 @@ bool MyAvatar::getCollisionsEnabled() {
     return _characterController.computeCollisionGroup() != BULLET_COLLISION_GROUP_COLLISIONLESS;
 }
 
+void MyAvatar::updateCollisionCapsuleCache() {
+    glm::vec3 start, end;
+    float radius;
+    getCapsule(start, end, radius);
+    QVariantMap capsule;
+    capsule["start"] = vec3toVariant(start);
+    capsule["end"] = vec3toVariant(end);
+    capsule["radius"] = QVariant(radius);
+    _collisionCapsuleCache.set(capsule);
+}
+
+// thread safe
+QVariantMap MyAvatar::getCollisionCapsule() const {
+    return _collisionCapsuleCache.get();
+}
+
 void MyAvatar::setCharacterControllerEnabled(bool enabled) {
     qCDebug(interfaceapp) << "MyAvatar.characterControllerEnabled is deprecated. Use MyAvatar.collisionsEnabled instead.";
     setCollisionsEnabled(enabled);

interface/src/avatar/MyAvatar.h

@@ -1017,6 +1017,12 @@ public:
      */
     Q_INVOKABLE bool getCollisionsEnabled();
 
+    /**jsdoc
+    * @function MyAvatar.getCollisionCapsule
+    * @returns {object}
+    */
+    Q_INVOKABLE QVariantMap getCollisionCapsule() const;
+
     /**jsdoc
      * @function MyAvatar.setCharacterControllerEnabled
      * @param {boolean} enabled
@@ -1180,11 +1186,12 @@ public slots:
      * @param {boolean} [hasOrientation=false] - Set to <code>true</code> to set the orientation of the avatar.
      * @param {Quat} [orientation=Quat.IDENTITY] - The new orientation for the avatar.
      * @param {boolean} [shouldFaceLocation=false] - Set to <code>true</code> to position the avatar a short distance away from 
+     * @param {boolean} [withSafeLanding=true] - Set to <code>false</code> MyAvatar::safeLanding will not be called (used when teleporting).
      *     the new position and orientate the avatar to face the position.
      */
     void goToLocation(const glm::vec3& newPosition,
                       bool hasOrientation = false, const glm::quat& newOrientation = glm::quat(),
-                      bool shouldFaceLocation = false);
+                      bool shouldFaceLocation = false, bool withSafeLanding = true);
     /**jsdoc
      * @function MyAvatar.goToLocation
      * @param {object} properties
@@ -1498,6 +1505,7 @@ signals:
 
 private slots:
     void leaveDomain();
+    void updateCollisionCapsuleCache();
 
 protected:
     virtual void beParentOfChild(SpatiallyNestablePointer newChild) const override;
@@ -1722,6 +1730,7 @@ private:
 
     bool _goToPending { false };
     bool _physicsSafetyPending { false };
+    bool _goToSafe { true };
     glm::vec3 _goToPosition;
     glm::quat _goToOrientation;
 

interface/src/raypick/CollisionPick.cpp

@@ -51,6 +51,7 @@ void buildObjectIntersectionsMap(IntersectionType intersectionType, const std::v
         QVariantMap collisionPointPair;
         collisionPointPair["pointOnPick"] = vec3toVariant(objectIntersection.testCollisionPoint);
         collisionPointPair["pointOnObject"] = vec3toVariant(objectIntersection.foundCollisionPoint);
+        collisionPointPair["normalOnPick"] = vec3toVariant(objectIntersection.collisionNormal);
 
         collisionPointPairs[objectIntersection.foundID].append(collisionPointPair);
     }
@@ -397,7 +398,7 @@ PickResultPointer CollisionPick::getEntityIntersection(const CollisionRegion& pi
     }
     getShapeInfoReady(pick);
     
-    auto entityIntersections = _physicsEngine->contactTest(USER_COLLISION_MASK_ENTITIES, *_mathPick.shapeInfo, pick.transform, USER_COLLISION_GROUP_DYNAMIC, pick.threshold);
+    auto entityIntersections = _physicsEngine->contactTest(USER_COLLISION_MASK_ENTITIES, *_mathPick.shapeInfo, pick.transform, pick.collisionGroup, pick.threshold);
     filterIntersections(entityIntersections);
     return std::make_shared<CollisionPickResult>(pick, entityIntersections, std::vector<ContactTestResult>());
 }
@@ -413,13 +414,13 @@ PickResultPointer CollisionPick::getAvatarIntersection(const CollisionRegion& pi
     }
     getShapeInfoReady(pick);
     
-    auto avatarIntersections = _physicsEngine->contactTest(USER_COLLISION_MASK_AVATARS, *_mathPick.shapeInfo, pick.transform, USER_COLLISION_GROUP_DYNAMIC, pick.threshold);
+    auto avatarIntersections = _physicsEngine->contactTest(USER_COLLISION_MASK_AVATARS, *_mathPick.shapeInfo, pick.transform, pick.collisionGroup, pick.threshold);
     filterIntersections(avatarIntersections);
     return std::make_shared<CollisionPickResult>(pick, std::vector<ContactTestResult>(), avatarIntersections);
 }
 
 PickResultPointer CollisionPick::getHUDIntersection(const CollisionRegion& pick) {
-    return std::make_shared<CollisionPickResult>(pick.toVariantMap(), std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
+    return std::make_shared<CollisionPickResult>(pick, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
 }
 
 Transform CollisionPick::getResultTransform() const {

interface/src/raypick/CollisionPick.h

@@ -70,6 +70,9 @@ protected:
     CollisionRegion _mathPick;
     PhysicsEnginePointer _physicsEngine;
     QSharedPointer<GeometryResource> _cachedResource;
+
+    // Options for what information to get from collision results
+    bool _includeNormals;
 };
 
 #endif // hifi_CollisionPick_h

interface/src/raypick/PickScriptingInterface.cpp

@@ -270,6 +270,8 @@ unsigned int PickScriptingInterface::createParabolaPick(const QVariant& properti
 * @property {Quat} orientation - The orientation of the collision region, relative to a parent if defined.
 * @property {float} threshold - The approximate minimum penetration depth for a test object to be considered in contact with the collision region.
 * The depth is measured in world space, but will scale with the parent if defined.
+* @property {CollisionMask} [collisionGroup=8] - The type of object this collision pick collides as. Objects whose collision masks overlap with the pick's collision group
+* will be considered colliding with the pick.
 * @property {Uuid} parentID - The ID of the parent, either an avatar, an entity, or an overlay.
 * @property {number} parentJointIndex - The joint of the parent to parent to, for example, the joints on the model of an avatar. (default = 0, no joint)
 * @property {string} joint - If "Mouse," parents the pick to the mouse. If "Avatar," parents the pick to MyAvatar's head. Otherwise, parents to the joint of the given name on MyAvatar.

interface/src/raypick/PickScriptingInterface.h

@@ -167,6 +167,7 @@ public:
      * @typedef {object} CollisionContact
      * @property {Vec3} pointOnPick A point representing a penetration of the object's surface into the volume of the pick, in world space.
      * @property {Vec3} pointOnObject A point representing a penetration of the pick's surface into the volume of the found object, in world space.
+     * @property {Vec3} normalOnPick The normalized vector pointing away from the pick, representing the direction of collision.
      */
 
     /**jsdoc

libraries/avatars/src/AvatarData.h

@@ -1438,6 +1438,8 @@ protected:
     ThreadSafeValueCache<glm::mat4> _farGrabLeftMatrixCache { glm::mat4() };
     ThreadSafeValueCache<glm::mat4> _farGrabMouseMatrixCache { glm::mat4() };
 
+    ThreadSafeValueCache<QVariantMap> _collisionCapsuleCache{ QVariantMap() };
+
     int getFauxJointIndex(const QString& name) const;
 
     float _audioLoudness { 0.0f };

libraries/physics/src/EntityMotionState.cpp

@@ -446,8 +446,9 @@ bool EntityMotionState::shouldSendUpdate(uint32_t simulationStep) {
 
 void EntityMotionState::updateSendVelocities() {
     if (!_body->isActive()) {
-        // make sure all derivatives are zero
+        if (!_body->isKinematicObject()) {
-        clearObjectVelocities();
+            clearObjectVelocities();
+        }
         // we pretend we sent the inactive update for this object
         _numInactiveUpdates = 1;
     } else {

libraries/physics/src/PhysicalEntitySimulation.cpp

@@ -138,7 +138,6 @@ void PhysicalEntitySimulation::changeEntityInternal(EntityItemPointer entity) {
                 btRigidBody* body = motionState->getRigidBody();
                 if (body) {
                     body->forceActivationState(ISLAND_SLEEPING);
-                    motionState->updateSendVelocities(); // has side-effect of zeroing entity velocities for inactive body
                 }
 
                 // send packet to remove ownership

libraries/physics/src/PhysicsEngine.cpp

@@ -936,19 +936,22 @@ struct AllContactsCallback : public btCollisionWorld::ContactResultCallback {
         const btCollisionObject* otherBody;
         btVector3 penetrationPoint;
         btVector3 otherPenetrationPoint;
+        btVector3 normal;
         if (colObj0->m_collisionObject == &collisionObject) {
             otherBody = colObj1->m_collisionObject;
             penetrationPoint = getWorldPoint(cp.m_localPointB, colObj1->getWorldTransform());
             otherPenetrationPoint = getWorldPoint(cp.m_localPointA, colObj0->getWorldTransform());
+            normal = -cp.m_normalWorldOnB;
         } else {
             otherBody = colObj0->m_collisionObject;
             penetrationPoint = getWorldPoint(cp.m_localPointA, colObj0->getWorldTransform());
             otherPenetrationPoint = getWorldPoint(cp.m_localPointB, colObj1->getWorldTransform());
+            normal = cp.m_normalWorldOnB;
         }
 
         // TODO: Give MyAvatar a motion state so we don't have to do this
         if ((m_collisionFilterMask & BULLET_COLLISION_GROUP_MY_AVATAR) && myAvatarCollisionObject && myAvatarCollisionObject == otherBody) {
-            contacts.emplace_back(Physics::getSessionUUID(), bulletToGLM(penetrationPoint), bulletToGLM(otherPenetrationPoint));
+            contacts.emplace_back(Physics::getSessionUUID(), bulletToGLM(penetrationPoint), bulletToGLM(otherPenetrationPoint), bulletToGLM(normal));
             return 0;
         }
 
@@ -964,7 +967,7 @@ struct AllContactsCallback : public btCollisionWorld::ContactResultCallback {
         }
 
         // This is the correct object type. Add it to the list.
-        contacts.emplace_back(candidate->getObjectID(), bulletToGLM(penetrationPoint), bulletToGLM(otherPenetrationPoint));
+        contacts.emplace_back(candidate->getObjectID(), bulletToGLM(penetrationPoint), bulletToGLM(otherPenetrationPoint), bulletToGLM(normal));
 
         return 0;
     }

libraries/physics/src/PhysicsEngine.h

@@ -49,13 +49,15 @@ struct ContactTestResult {
     ContactTestResult(const ContactTestResult& contactTestResult) :
         foundID(contactTestResult.foundID),
         testCollisionPoint(contactTestResult.testCollisionPoint),
-        foundCollisionPoint(contactTestResult.foundCollisionPoint) {
+        foundCollisionPoint(contactTestResult.foundCollisionPoint),
+        collisionNormal(contactTestResult.collisionNormal) {
     }
 
-    ContactTestResult(QUuid foundID, glm::vec3 testCollisionPoint, glm::vec3 otherCollisionPoint) :
+    ContactTestResult(const QUuid& foundID, const glm::vec3& testCollisionPoint, const glm::vec3& otherCollisionPoint, const glm::vec3& collisionNormal) :
         foundID(foundID),
         testCollisionPoint(testCollisionPoint),
-        foundCollisionPoint(otherCollisionPoint) {
+        foundCollisionPoint(otherCollisionPoint),
+        collisionNormal(collisionNormal) {
     }
 
     QUuid foundID;
@@ -63,6 +65,8 @@ struct ContactTestResult {
     glm::vec3 testCollisionPoint;
     // The deepest point of an intersection within the volume of the found object, in world space.
     glm::vec3 foundCollisionPoint;
+    // The normal vector of this intersection
+    glm::vec3 collisionNormal;
 };
 
 using ContactMap = std::map<ContactKey, ContactInfo>;

libraries/shared/src/RegisteredMetaTypes.h

@@ -24,6 +24,7 @@
 #include "SharedUtil.h"
 #include "shared/Bilateral.h"
 #include "Transform.h"
+#include "PhysicsCollisionGroups.h"
 
 class QColor;
 class QUrl;
@@ -264,6 +265,8 @@ public:
 * @property {Quat} orientation - The orientation of the collision region, relative to a parent if defined.
 * @property {float} threshold - The approximate minimum penetration depth for a test object to be considered in contact with the collision region.
 * The depth is measured in world space, but will scale with the parent if defined.
+* @property {CollisionMask} [collisionGroup=8] - The type of object this collision pick collides as. Objects whose collision masks overlap with the pick's collision group
+* will be considered colliding with the pick.
 * @property {Uuid} parentID - The ID of the parent, either an avatar, an entity, or an overlay.
 * @property {number} parentJointIndex - The joint of the parent to parent to, for example, the joints on the model of an avatar. (default = 0, no joint)
 * @property {string} joint - If "Mouse," parents the pick to the mouse. If "Avatar," parents the pick to MyAvatar's head. Otherwise, parents to the joint of the given name on MyAvatar.
@@ -277,7 +280,8 @@ public:
         modelURL(collisionRegion.modelURL),
         shapeInfo(std::make_shared<ShapeInfo>()),
         transform(collisionRegion.transform),
-        threshold(collisionRegion.threshold)
+        threshold(collisionRegion.threshold),
+        collisionGroup(collisionRegion.collisionGroup)
     {
         shapeInfo->setParams(collisionRegion.shapeInfo->getType(), collisionRegion.shapeInfo->getHalfExtents(), collisionRegion.modelURL.toString());
     }
@@ -316,6 +320,9 @@ public:
         if (pickVariant["orientation"].isValid()) {
             transform.setRotation(quatFromVariant(pickVariant["orientation"]));
         }
+        if (pickVariant["collisionGroup"].isValid()) {
+            collisionGroup = pickVariant["collisionGroup"].toUInt();
+        }
     }
 
     QVariantMap toVariantMap() const override {
@@ -330,6 +337,7 @@ public:
         collisionRegion["loaded"] = loaded;
 
         collisionRegion["threshold"] = threshold;
+        collisionRegion["collisionGroup"] = collisionGroup;
 
         collisionRegion["position"] = vec3toVariant(transform.getTranslation());
         collisionRegion["orientation"] = quatToVariant(transform.getRotation());
@@ -341,12 +349,14 @@ public:
         return !std::isnan(threshold) &&
             !(glm::any(glm::isnan(transform.getTranslation())) ||
             glm::any(glm::isnan(transform.getRotation())) ||
-            shapeInfo->getType() == SHAPE_TYPE_NONE);
+            shapeInfo->getType() == SHAPE_TYPE_NONE ||
+            collisionGroup == 0);
     }
 
     bool operator==(const CollisionRegion& other) const {
         return loaded == other.loaded &&
             threshold == other.threshold &&
+            collisionGroup == other.collisionGroup &&
             glm::all(glm::equal(transform.getTranslation(), other.transform.getTranslation())) &&
             glm::all(glm::equal(transform.getRotation(), other.transform.getRotation())) &&
             glm::all(glm::equal(transform.getScale(), other.transform.getScale())) &&
@@ -362,6 +372,10 @@ public:
             return false;
         }
 
+        if (collisionGroup == 0) {
+            return false;
+        }
+
         return !shapeInfo->getPointCollection().size();
     }
 
@@ -372,7 +386,8 @@ public:
     // We can't compute the shapeInfo here without loading the model first, so we delegate that responsibility to the owning CollisionPick
     std::shared_ptr<ShapeInfo> shapeInfo = std::make_shared<ShapeInfo>();
     Transform transform;
-    float threshold;
+    float threshold { 0.0f };
+    uint16_t collisionGroup { USER_COLLISION_GROUP_MY_AVATAR };
 };
 
 namespace std {

scripts/system/controllers/controllerModules/teleport.js

@@ -62,40 +62,51 @@ Script.include("/~/system/libraries/controllers.js");
         alpha: 1,
         width: 0.025
     };
+    
     var teleportPath = {
         color: COLORS_TELEPORT_CAN_TELEPORT,
         alpha: 1,
         width: 0.025
     };
+    
     var seatPath = {
         color: COLORS_TELEPORT_SEAT,
         alpha: 1,
         width: 0.025
     };
+    
     var teleportEnd = {
         type: "model",
         url: TARGET_MODEL_URL,
         dimensions: TARGET_MODEL_DIMENSIONS,
         ignorePickIntersection: true
     };
+    
     var seatEnd = {
         type: "model",
         url: SEAT_MODEL_URL,
         dimensions: TARGET_MODEL_DIMENSIONS,
         ignorePickIntersection: true
     };
-
+    
-
+    var collisionEnd = {
+        type: "shape",
+        shape: "box",
+        dimensions: { x: 1.0, y: 0.001, z: 1.0 },
+        alpha: 0.0,
+        ignorePickIntersection: true
+    };
+    
     var teleportRenderStates = [{name: "cancel", path: cancelPath},
         {name: "teleport", path: teleportPath, end: teleportEnd},
-        {name: "seat", path: seatPath, end: seatEnd}];
+        {name: "seat", path: seatPath, end: seatEnd},
+        {name: "collision", end: collisionEnd}];
 
     var DEFAULT_DISTANCE = 8.0;
     var teleportDefaultRenderStates = [{name: "cancel", distance: DEFAULT_DISTANCE, path: cancelPath}];
 
     var ignoredEntities = [];
 
-
     var TELEPORTER_STATES = {
         IDLE: 'idle',
         TARGETTING: 'targetting',
@@ -104,8 +115,9 @@ Script.include("/~/system/libraries/controllers.js");
 
     var TARGET = {
         NONE: 'none', // Not currently targetting anything
-        INVISIBLE: 'invisible', // The current target is an invvsible surface
         INVALID: 'invalid', // The current target is invalid (wall, ceiling, etc.)
+        COLLIDES: 'collides', // Insufficient space to accommodate the avatar capsule
+        DISCREPANCY: 'discrepancy', // We are not 100% sure the avatar will fit so we trigger safe landing
         SURFACE: 'surface', // The current target is a valid surface
         SEAT: 'seat' // The current target is a seat
     };
@@ -115,6 +127,7 @@ Script.include("/~/system/libraries/controllers.js");
 
     function Teleporter(hand) {
         var _this = this;
+        this.init = false;
         this.hand = hand;
         this.buttonValue = 0;
         this.disabled = false; // used by the 'Hifi-Teleport-Disabler' message handler
@@ -122,74 +135,138 @@ Script.include("/~/system/libraries/controllers.js");
         this.state = TELEPORTER_STATES.IDLE;
         this.currentTarget = TARGET.INVALID;
         this.currentResult = null;
+        this.capsuleThreshold = 0.05;
+        this.pickHeightOffset = 0.05;
 
         this.getOtherModule = function() {
             var otherModule = this.hand === RIGHT_HAND ? leftTeleporter : rightTeleporter;
             return otherModule;
         };
 
-        this.teleportParabolaHandVisible = Pointers.createPointer(PickType.Parabola, {
+        this.teleportHeadCollisionPick;
-            joint: (_this.hand === RIGHT_HAND) ? "_CAMERA_RELATIVE_CONTROLLER_RIGHTHAND" : "_CAMERA_RELATIVE_CONTROLLER_LEFTHAND",
+        this.teleportHandCollisionPick;
-            dirOffset: { x: 0, y: 1, z: 0.1 },
+        this.teleportParabolaHandVisuals;
-            posOffset: { x: (_this.hand === RIGHT_HAND) ? 0.03 : -0.03, y: 0.2, z: 0.02 },
+        this.teleportParabolaHandCollisions;
-            filter: Picks.PICK_ENTITIES,
+        this.teleportParabolaHeadVisuals;
-            faceAvatar: true,
+        this.teleportParabolaHeadCollisions;
-            scaleWithAvatar: true,
-            centerEndY: false,
-            speed: speed,
-            accelerationAxis: accelerationAxis,
-            rotateAccelerationWithAvatar: true,
-            renderStates: teleportRenderStates,
-            defaultRenderStates: teleportDefaultRenderStates,
-            maxDistance: 8.0
-        });
-        this.teleportParabolaHandInvisible = Pointers.createPointer(PickType.Parabola, {
-            joint: (_this.hand === RIGHT_HAND) ? "_CAMERA_RELATIVE_CONTROLLER_RIGHTHAND" : "_CAMERA_RELATIVE_CONTROLLER_LEFTHAND",
-            dirOffset: { x: 0, y: 1, z: 0.1 },
-            posOffset: { x: (_this.hand === RIGHT_HAND) ? 0.03 : -0.03, y: 0.2, z: 0.02 },
-            filter: Picks.PICK_ENTITIES | Picks.PICK_INCLUDE_INVISIBLE,
-            faceAvatar: true,
-            scaleWithAvatar: true,
-            centerEndY: false,
-            speed: speed,
-            accelerationAxis: accelerationAxis,
-            rotateAccelerationWithAvatar: true,
-            renderStates: teleportRenderStates,
-            maxDistance: 8.0
-        });
-        this.teleportParabolaHeadVisible = Pointers.createPointer(PickType.Parabola, {
-            joint: "Avatar",
-            filter: Picks.PICK_ENTITIES,
-            faceAvatar: true,
-            scaleWithAvatar: true,
-            centerEndY: false,
-            speed: speed,
-            accelerationAxis: accelerationAxis,
-            rotateAccelerationWithAvatar: true,
-            renderStates: teleportRenderStates,
-            defaultRenderStates: teleportDefaultRenderStates,
-            maxDistance: 8.0
-        });
-        this.teleportParabolaHeadInvisible = Pointers.createPointer(PickType.Parabola, {
-            joint: "Avatar",
-            filter: Picks.PICK_ENTITIES | Picks.PICK_INCLUDE_INVISIBLE,
-            faceAvatar: true,
-            scaleWithAvatar: true,
-            centerEndY: false,
-            speed: speed,
-            accelerationAxis: accelerationAxis,
-            rotateAccelerationWithAvatar: true,
-            renderStates: teleportRenderStates,
-            maxDistance: 8.0
-        });
 
         this.cleanup = function() {
-            Pointers.removePointer(this.teleportParabolaHandVisible);
+            Pointers.removePointer(_this.teleportParabolaHandVisuals);
-            Pointers.removePointer(this.teleportParabolaHandInvisible);
+            Pointers.removePointer(_this.teleportParabolaHandCollisions);
-            Pointers.removePointer(this.teleportParabolaHeadVisible);
+            Pointers.removePointer(_this.teleportParabolaHeadVisuals);
-            Pointers.removePointer(this.teleportParabolaHeadInvisible);
+            Pointers.removePointer(_this.teleportParabolaHeadCollisions);
+            Picks.removePick(_this.teleportHandCollisionPick);
+            Picks.removePick(_this.teleportHeadCollisionPick);
         };
 
+        this.initPointers = function () {
+            if (_this.init) {
+                _this.cleanup();
+            }
+            _this.teleportParabolaHandVisuals = Pointers.createPointer(PickType.Parabola, {
+                joint: (_this.hand === RIGHT_HAND) ? "_CAMERA_RELATIVE_CONTROLLER_RIGHTHAND" : "_CAMERA_RELATIVE_CONTROLLER_LEFTHAND",
+                dirOffset: { x: 0, y: 1, z: 0.1 },
+                posOffset: { x: (_this.hand === RIGHT_HAND) ? 0.03 : -0.03, y: 0.2, z: 0.02 },
+                filter: Picks.PICK_ENTITIES | Picks.PICK_INCLUDE_INVISIBLE,
+                faceAvatar: true,
+                scaleWithAvatar: true,
+                centerEndY: false,
+                speed: speed,
+                accelerationAxis: accelerationAxis,
+                rotateAccelerationWithAvatar: true,
+                renderStates: teleportRenderStates,
+                defaultRenderStates: teleportDefaultRenderStates,
+                maxDistance: 8.0
+            });
+
+            _this.teleportParabolaHandCollisions = Pointers.createPointer(PickType.Parabola, {
+                joint: (_this.hand === RIGHT_HAND) ? "_CAMERA_RELATIVE_CONTROLLER_RIGHTHAND" : "_CAMERA_RELATIVE_CONTROLLER_LEFTHAND",
+                dirOffset: { x: 0, y: 1, z: 0.1 },
+                posOffset: { x: (_this.hand === RIGHT_HAND) ? 0.03 : -0.03, y: 0.2, z: 0.02 },
+                filter: Picks.PICK_ENTITIES | Picks.PICK_INCLUDE_INVISIBLE,
+                faceAvatar: true,
+                scaleWithAvatar: true,
+                centerEndY: false,
+                speed: speed,
+                accelerationAxis: accelerationAxis,
+                rotateAccelerationWithAvatar: true,
+                renderStates: teleportRenderStates,
+                maxDistance: 8.0
+            });
+
+            _this.teleportParabolaHeadVisuals = Pointers.createPointer(PickType.Parabola, {
+                joint: "Avatar",
+                filter: Picks.PICK_ENTITIES | Picks.PICK_INCLUDE_INVISIBLE,
+                faceAvatar: true,
+                scaleWithAvatar: true,
+                centerEndY: false,
+                speed: speed,
+                accelerationAxis: accelerationAxis,
+                rotateAccelerationWithAvatar: true,
+                renderStates: teleportRenderStates,
+                defaultRenderStates: teleportDefaultRenderStates,
+                maxDistance: 8.0
+            });
+
+            _this.teleportParabolaHeadCollisions = Pointers.createPointer(PickType.Parabola, {
+                joint: "Avatar",
+                filter: Picks.PICK_ENTITIES | Picks.PICK_INCLUDE_INVISIBLE,
+                faceAvatar: true,
+                scaleWithAvatar: true,
+                centerEndY: false,
+                speed: speed,
+                accelerationAxis: accelerationAxis,
+                rotateAccelerationWithAvatar: true,
+                renderStates: teleportRenderStates,
+                maxDistance: 8.0
+            });
+
+
+            var capsuleData = MyAvatar.getCollisionCapsule();
+
+            var sensorToWorldScale = MyAvatar.getSensorToWorldScale();
+
+            var radius = capsuleData.radius / sensorToWorldScale;
+            var height = (Vec3.distance(capsuleData.start, capsuleData.end) + (capsuleData.radius * 2.0)) / sensorToWorldScale;
+            var capsuleRatio = 10.0 * radius / height;
+            var offset = _this.pickHeightOffset * capsuleRatio;
+
+            _this.teleportHandCollisionPick = Picks.createPick(PickType.Collision, {
+                enabled: true,
+                parentID: Pointers.getPointerProperties(_this.teleportParabolaHandCollisions).renderStates["collision"].end,
+                filter: Picks.PICK_ENTITIES | Picks.PICK_AVATARS,
+                shape: {
+                    shapeType: "capsule-y",
+                    dimensions: {
+                        x: radius * 2.0,
+                        y: height - (radius * 2.0),
+                        z: radius * 2.0
+                    }
+                },
+                position: { x: 0, y: offset + height * 0.5, z: 0 },
+                threshold: _this.capsuleThreshold
+            });
+
+            _this.teleportHeadCollisionPick = Picks.createPick(PickType.Collision, {
+                enabled: true,
+                parentID: Pointers.getPointerProperties(_this.teleportParabolaHeadCollisions).renderStates["collision"].end,
+                filter: Picks.PICK_ENTITIES | Picks.PICK_AVATARS,
+                shape: {
+                    shapeType: "capsule-y",
+                    dimensions: {
+                        x: radius * 2.0,
+                        y: height - (radius * 2.0),
+                        z: radius * 2.0
+                    }
+                },
+                position: { x: 0, y: offset + height * 0.5, z: 0 },
+                threshold: _this.capsuleThreshold
+            });
+            _this.init = true;
+        }
+        
+        _this.initPointers();
+
         this.axisButtonStateX = 0; // Left/right axis button pressed.
         this.axisButtonStateY = 0; // Up/down axis button pressed.
         this.BUTTON_TRANSITION_DELAY = 100; // Allow time for transition from direction buttons to touch-pad.
@@ -254,52 +331,56 @@ Script.include("/~/system/libraries/controllers.js");
             var pose = Controller.getPoseValue(handInfo[(_this.hand === RIGHT_HAND) ? 'right' : 'left'].controllerInput);
             var mode = pose.valid ? _this.hand : 'head';
             if (!pose.valid) {
-                Pointers.disablePointer(_this.teleportParabolaHandVisible);
+                Pointers.disablePointer(_this.teleportParabolaHandVisuals);
-                Pointers.disablePointer(_this.teleportParabolaHandInvisible);
+                Pointers.disablePointer(_this.teleportParabolaHandCollisions);
-                Pointers.enablePointer(_this.teleportParabolaHeadVisible);
+                Picks.disablePick(_this.teleportHandCollisionPick);
-                Pointers.enablePointer(_this.teleportParabolaHeadInvisible);
+                Pointers.enablePointer(_this.teleportParabolaHeadVisuals);
+                Pointers.enablePointer(_this.teleportParabolaHeadCollisions);
+                Picks.enablePick(_this.teleportHeadCollisionPick);
             } else {
-                Pointers.enablePointer(_this.teleportParabolaHandVisible);
+                Pointers.enablePointer(_this.teleportParabolaHandVisuals);
-                Pointers.enablePointer(_this.teleportParabolaHandInvisible);
+                Pointers.enablePointer(_this.teleportParabolaHandCollisions);
-                Pointers.disablePointer(_this.teleportParabolaHeadVisible);
+                Picks.enablePick(_this.teleportHandCollisionPick);
-                Pointers.disablePointer(_this.teleportParabolaHeadInvisible);
+                Pointers.disablePointer(_this.teleportParabolaHeadVisuals);
+                Pointers.disablePointer(_this.teleportParabolaHeadCollisions);
+                Picks.disablePick(_this.teleportHeadCollisionPick);
             }
 
             // We do up to 2 picks to find a teleport location.
             // There are 2 types of teleport locations we are interested in:
-            //   1. A visible floor. This can be any entity surface that points within some degree of "up"
+            //
+            //   1. A visible floor. This can be any entity surface that points within some degree of "up" 
+            //      and where the avatar capsule can be positioned without colliding
+            //
             //   2. A seat. The seat can be visible or invisible.
             //
-            //  * In the first pass we pick against visible and invisible entities so that we can find invisible seats.
+            //  The Collision Pick is currently parented to the end overlay on teleportParabolaXXXXCollisions
-            //    We might hit an invisible entity that is not a seat, so we need to do a second pass.
-            //  * In the second pass we pick against visible entities only.
             //
-            var result;
+            //  TODO 
+            //  Parent the collision Pick directly to the teleportParabolaXXXXVisuals and get rid of teleportParabolaXXXXCollisions
+            //
+            var result, collisionResult;
             if (mode === 'head') {
-                result = Pointers.getPrevPickResult(_this.teleportParabolaHeadInvisible);
+                result = Pointers.getPrevPickResult(_this.teleportParabolaHeadCollisions);
+                collisionResult = Picks.getPrevPickResult(_this.teleportHeadCollisionPick);
             } else {
-                result = Pointers.getPrevPickResult(_this.teleportParabolaHandInvisible);
+                result = Pointers.getPrevPickResult(_this.teleportParabolaHandCollisions);
-            }
+                collisionResult = Picks.getPrevPickResult(_this.teleportHandCollisionPick);
-
-            var teleportLocationType = getTeleportTargetType(result);
-            if (teleportLocationType === TARGET.INVISIBLE) {
-                if (mode === 'head') {
-                    result = Pointers.getPrevPickResult(_this.teleportParabolaHeadVisible);
-                } else {
-                    result = Pointers.getPrevPickResult(_this.teleportParabolaHandVisible);
-                }
-                teleportLocationType = getTeleportTargetType(result);
             }
+           
+            var teleportLocationType = getTeleportTargetType(result, collisionResult);
 
             if (teleportLocationType === TARGET.NONE) {
                 // Use the cancel default state
                 this.setTeleportState(mode, "cancel", "");
-            } else if (teleportLocationType === TARGET.INVALID || teleportLocationType === TARGET.INVISIBLE) {
+            } else if (teleportLocationType === TARGET.INVALID) {
                 this.setTeleportState(mode, "", "cancel");
-            } else if (teleportLocationType === TARGET.SURFACE) {
+            } else if (teleportLocationType === TARGET.COLLIDES) {
-                this.setTeleportState(mode, "teleport", "");
+                this.setTeleportState(mode, "cancel", "collision");
+            } else if (teleportLocationType === TARGET.SURFACE || teleportLocationType === TARGET.DISCREPANCY) {
+                this.setTeleportState(mode, "teleport", "collision");
             } else if (teleportLocationType === TARGET.SEAT) {
-                this.setTeleportState(mode, "", "seat");
+                this.setTeleportState(mode, "collision", "seat");
             }
             return this.teleport(result, teleportLocationType);
         };
@@ -314,10 +395,11 @@ Script.include("/~/system/libraries/controllers.js");
                 // Do nothing
             } else if (target === TARGET.SEAT) {
                 Entities.callEntityMethod(result.objectID, 'sit');
-            } else if (target === TARGET.SURFACE) {
+            } else if (target === TARGET.SURFACE || target === TARGET.DISCREPANCY) {
                 var offset = getAvatarFootOffset();
                 result.intersection.y += offset;
-                MyAvatar.goToLocation(result.intersection, true, HMD.orientation, false);
+                var shouldLandSafe = target === TARGET.DISCREPANCY;
+                MyAvatar.goToLocation(result.intersection, true, HMD.orientation, false, shouldLandSafe);
                 HMD.centerUI();
                 MyAvatar.centerBody();
             }
@@ -328,33 +410,38 @@ Script.include("/~/system/libraries/controllers.js");
         };
 
         this.disableLasers = function() {
-            Pointers.disablePointer(_this.teleportParabolaHandVisible);
+            Pointers.disablePointer(_this.teleportParabolaHandVisuals);
-            Pointers.disablePointer(_this.teleportParabolaHandInvisible);
+            Pointers.disablePointer(_this.teleportParabolaHandCollisions);
-            Pointers.disablePointer(_this.teleportParabolaHeadVisible);
+            Pointers.disablePointer(_this.teleportParabolaHeadVisuals);
-            Pointers.disablePointer(_this.teleportParabolaHeadInvisible);
+            Pointers.disablePointer(_this.teleportParabolaHeadCollisions);
+            Picks.disablePick(_this.teleportHeadCollisionPick);
+            Picks.disablePick(_this.teleportHandCollisionPick);
         };
 
         this.setTeleportState = function(mode, visibleState, invisibleState) {
             if (mode === 'head') {
-                Pointers.setRenderState(_this.teleportParabolaHeadVisible, visibleState);
+                Pointers.setRenderState(_this.teleportParabolaHeadVisuals, visibleState);
-                Pointers.setRenderState(_this.teleportParabolaHeadInvisible, invisibleState);
+                Pointers.setRenderState(_this.teleportParabolaHeadCollisions, invisibleState);
             } else {
-                Pointers.setRenderState(_this.teleportParabolaHandVisible, visibleState);
+                Pointers.setRenderState(_this.teleportParabolaHandVisuals, visibleState);
-                Pointers.setRenderState(_this.teleportParabolaHandInvisible, invisibleState);
+                Pointers.setRenderState(_this.teleportParabolaHandCollisions, invisibleState);
             }
         };
 
         this.setIgnoreEntities = function(entitiesToIgnore) {
-            Pointers.setIgnoreItems(this.teleportParabolaHandVisible, entitiesToIgnore);
+            Pointers.setIgnoreItems(this.teleportParabolaHandVisuals, entitiesToIgnore);
-            Pointers.setIgnoreItems(this.teleportParabolaHandInvisible, entitiesToIgnore);
+            Pointers.setIgnoreItems(this.teleportParabolaHandCollisions, entitiesToIgnore);
-            Pointers.setIgnoreItems(this.teleportParabolaHeadVisible, entitiesToIgnore);
+            Pointers.setIgnoreItems(this.teleportParabolaHeadVisuals, entitiesToIgnore);
-            Pointers.setIgnoreItems(this.teleportParabolaHeadInvisible, entitiesToIgnore);
+            Pointers.setIgnoreItems(this.teleportParabolaHeadCollisions, entitiesToIgnore);
+            Picks.setIgnoreItems(_this.teleportHeadCollisionPick, entitiesToIgnore);
+            Picks.setIgnoreItems(_this.teleportHandCollisionPick, entitiesToIgnore);
         };
     }
 
     // related to repositioning the avatar after you teleport
     var FOOT_JOINT_NAMES = ["RightToe_End", "RightToeBase", "RightFoot"];
     var DEFAULT_ROOT_TO_FOOT_OFFSET = 0.5;
+    
     function getAvatarFootOffset() {
 
         // find a valid foot jointIndex
@@ -395,7 +482,29 @@ Script.include("/~/system/libraries/controllers.js");
     // than MAX_ANGLE_FROM_UP_TO_TELEPORT degrees from your avatar's up, then
     // you can't teleport there.
     var MAX_ANGLE_FROM_UP_TO_TELEPORT = 70;
-    function getTeleportTargetType(result) {
+    var MAX_DISCREPANCY_DISTANCE = 1.0;
+    var MAX_DOT_SIGN = -0.6;
+
+    function checkForMeshDiscrepancy(result, collisionResult) {
+        var intersectingObjects = collisionResult.intersectingObjects;
+        if (intersectingObjects.length > 0 && intersectingObjects.length < 3) {
+            for (var j = 0; j < collisionResult.intersectingObjects.length; j++) {
+                var intersectingObject = collisionResult.intersectingObjects[j];
+                for (var i = 0; i < intersectingObject.collisionContacts.length; i++) {
+                    var normal = intersectingObject.collisionContacts[i].normalOnPick;
+                    var distanceToPick = Vec3.distance(intersectingObject.collisionContacts[i].pointOnPick, result.intersection);
+                    var normalSign = Vec3.dot(normal, Quat.getUp(MyAvatar.orientation));
+                    if ((distanceToPick > MAX_DISCREPANCY_DISTANCE) || (normalSign > MAX_DOT_SIGN)) {
+                        return false;
+                    }
+                }
+            }
+            return true;
+        }
+        return false;
+    }
+    
+    function getTeleportTargetType(result, collisionResult) {
         if (result.type === Picks.INTERSECTED_NONE) {
             return TARGET.NONE;
         }
@@ -410,9 +519,14 @@ Script.include("/~/system/libraries/controllers.js");
                 return TARGET.INVALID;
             }
         }
-
+        var isDiscrepancy = false;
-        if (!props.visible) {
+        if (collisionResult.collisionRegion != undefined) {
-            return TARGET.INVISIBLE;
+            if (collisionResult.intersects) {
+                isDiscrepancy = checkForMeshDiscrepancy(result, collisionResult);
+                if (!isDiscrepancy) {
+                    return TARGET.COLLIDES;
+                } 
+            }
         }
 
         var surfaceNormal = result.surfaceNormal;
@@ -420,6 +534,8 @@ Script.include("/~/system/libraries/controllers.js");
 
         if (angle > MAX_ANGLE_FROM_UP_TO_TELEPORT) {
             return TARGET.INVALID;
+        } else if (isDiscrepancy) {
+            return TARGET.DISCREPANCY;
         } else {
             return TARGET.SURFACE;
         }
@@ -513,7 +629,14 @@ Script.include("/~/system/libraries/controllers.js");
             }
         }
     };
-
+    
+    MyAvatar.onLoadComplete.connect(function () {
+        Script.setTimeout(function () {
+            leftTeleporter.initPointers();
+            rightTeleporter.initPointers();
+        }, 500);
+    });
+    
     Messages.subscribe('Hifi-Teleport-Disabler');
     Messages.subscribe('Hifi-Teleport-Ignore-Add');
     Messages.subscribe('Hifi-Teleport-Ignore-Remove');