From 6ecf850159d4d1f43826aa2a6ca47fcab3d665f6 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 6 Nov 2018 12:37:55 -0800
Subject: [PATCH] clarify ShapeInfo::setCapsuleY() API and use it correctly

---
 .../src/avatars-renderer/Avatar.cpp           | 17 ++++++-------
 libraries/shared/src/ShapeInfo.cpp            | 24 +++++++++----------
 libraries/shared/src/ShapeInfo.h              |  2 +-
 3 files changed, 22 insertions(+), 21 deletions(-)

diff --git a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp
index f694148b30..03938dd3de 100644
--- a/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp
+++ b/libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp
@@ -1724,22 +1724,23 @@ void Avatar::computeShapeInfo(ShapeInfo& shapeInfo) {
 }
 
 void Avatar::getCapsule(glm::vec3& start, glm::vec3& end, float& radius) {
-    // FIXME: this doesn't take into account Avatar rotation
     ShapeInfo shapeInfo;
     computeShapeInfo(shapeInfo);
-    glm::vec3 halfExtents = shapeInfo.getHalfExtents(); // x = radius, y = halfHeight
-    start = getWorldPosition() - glm::vec3(0, halfExtents.y, 0) + shapeInfo.getOffset();
-    end = getWorldPosition() + glm::vec3(0, halfExtents.y, 0) + shapeInfo.getOffset();
+    glm::vec3 halfExtents = shapeInfo.getHalfExtents(); // x = radius, y = cylinderHalfHeight + radius
     radius = halfExtents.x;
+    glm::vec3 halfCylinderAxis(0.0f, halfExtents.y - radius, 0.0f);
+    Transform transform = getTransform();
+    start = transform.getTranslation() + transform.getRotation() * (shapeInfo.getOffset() - halfCylinderAxis);
+    end = transform.getTranslation() + transform.getRotation() * (shapeInfo.getOffset() + halfCylinderAxis);
 }
 
 glm::vec3 Avatar::getWorldFeetPosition() {
     ShapeInfo shapeInfo;
-
     computeShapeInfo(shapeInfo);
-    glm::vec3 halfExtents = shapeInfo.getHalfExtents(); // x = radius, y = halfHeight
-    glm::vec3 localFeet(0.0f, shapeInfo.getOffset().y - halfExtents.y - halfExtents.x, 0.0f);
-    return getWorldOrientation() * localFeet + getWorldPosition();
+    glm::vec3 halfExtents = shapeInfo.getHalfExtents(); // x = radius, y = cylinderHalfHeight + radius
+    glm::vec3 localFeet(0.0f, shapeInfo.getOffset().y - halfExtents.y, 0.0f);
+    Transform transform = getTransform();
+    return transform.getTranslation() + transform.getRotation() * localFeet;
 }
 
 float Avatar::computeMass() {
diff --git a/libraries/shared/src/ShapeInfo.cpp b/libraries/shared/src/ShapeInfo.cpp
index 152e305bf2..52a8ad7254 100644
--- a/libraries/shared/src/ShapeInfo.cpp
+++ b/libraries/shared/src/ShapeInfo.cpp
@@ -148,12 +148,12 @@ void ShapeInfo::setPointCollection(const ShapeInfo::PointCollection& pointCollec
     _hashKey.clear();
 }
 
-void ShapeInfo::setCapsuleY(float radius, float halfHeight) {
+void ShapeInfo::setCapsuleY(float radius, float cylinderHalfHeight) {
     _url = "";
     _type = SHAPE_TYPE_CAPSULE_Y;
     radius = glm::max(radius, MIN_HALF_EXTENT);
-    halfHeight = glm::max(halfHeight, 0.0f);
-    _halfExtents = glm::vec3(radius, halfHeight, radius);
+    cylinderHalfHeight = glm::max(cylinderHalfHeight, 0.0f);
+    _halfExtents = glm::vec3(radius, cylinderHalfHeight + radius, radius);
     _hashKey.clear();
 }
 
@@ -262,26 +262,26 @@ bool ShapeInfo::contains(const glm::vec3& point) const {
             return glm::length(glm::vec2(point.x, point.y)) <= _halfExtents.y;
         case SHAPE_TYPE_CAPSULE_X: {
             if (glm::abs(point.x) <= _halfExtents.x) {
-                return glm::length(glm::vec2(point.y, point.z)) <= _halfExtents.z;
+                return glm::length(glm::vec2(point.y, point.z)) <= _halfExtents.y;
             } else {
-                glm::vec3 absPoint = glm::abs(point) - _halfExtents.x;
-                return glm::length(absPoint) <= _halfExtents.z;
+                glm::vec3 absPoint = glm::abs(point) - glm::vec3(_halfExtents.x, 0.0f, 0.0f);
+                return glm::length(absPoint) <= _halfExtents.y;
             }
         }
         case SHAPE_TYPE_CAPSULE_Y: {
             if (glm::abs(point.y) <= _halfExtents.y) {
-                return glm::length(glm::vec2(point.x, point.z)) <= _halfExtents.x;
+                return glm::length(glm::vec2(point.x, point.z)) <= _halfExtents.z;
             } else {
-                glm::vec3 absPoint = glm::abs(point) - _halfExtents.y;
-                return glm::length(absPoint) <= _halfExtents.x;
+                glm::vec3 absPoint = glm::abs(point) - glm::vec3(0.0f, _halfExtents.y, 0.0f);
+                return glm::length(absPoint) <= _halfExtents.z;
             }
         }
         case SHAPE_TYPE_CAPSULE_Z: {
             if (glm::abs(point.z) <= _halfExtents.z) {
-                return glm::length(glm::vec2(point.x, point.y)) <= _halfExtents.y;
+                return glm::length(glm::vec2(point.x, point.y)) <= _halfExtents.x;
             } else {
-                glm::vec3 absPoint = glm::abs(point) - _halfExtents.z;
-                return glm::length(absPoint) <= _halfExtents.y;
+                glm::vec3 absPoint = glm::abs(point) - glm::vec3(0.0f, 0.0f, _halfExtents.z);
+                return glm::length(absPoint) <= _halfExtents.x;
             }
         }
         case SHAPE_TYPE_BOX:
diff --git a/libraries/shared/src/ShapeInfo.h b/libraries/shared/src/ShapeInfo.h
index 5020e492cf..16e260d9db 100644
--- a/libraries/shared/src/ShapeInfo.h
+++ b/libraries/shared/src/ShapeInfo.h
@@ -67,7 +67,7 @@ public:
     void setBox(const glm::vec3& halfExtents);
     void setSphere(float radius);
     void setPointCollection(const PointCollection& pointCollection);
-    void setCapsuleY(float radius, float halfHeight);
+    void setCapsuleY(float radius, float cylinderHalfHeight);
     void setOffset(const glm::vec3& offset);
 
     ShapeType getType() const { return _type; }