From c739db75621f9f28790ad7cbc3c5d3f743fd8b74 Mon Sep 17 00:00:00 2001
From: trent <trent@joy-machine.com>
Date: Wed, 5 Apr 2017 17:39:52 -0400
Subject: [PATCH] Added quadratic ease-in-and-out smoothing to remote avatars
 position/orientation updates (through... one path, at least?).

---
 interface/src/avatar/Avatar.cpp | 57 ++++++++++++++++++++++++++++++---
 interface/src/avatar/Avatar.h   | 13 ++++++++
 2 files changed, 65 insertions(+), 5 deletions(-)

diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp
index ade98c63d8..c1ba0bca65 100644
--- a/interface/src/avatar/Avatar.cpp
+++ b/interface/src/avatar/Avatar.cpp
@@ -97,7 +97,15 @@ Avatar::Avatar(RigPointer rig) :
     _worldUpDirection(DEFAULT_UP_DIRECTION),
     _moving(false),
     _initialized(false),
-    _voiceSphereID(GeometryCache::UNKNOWN_ID)
+    _voiceSphereID(GeometryCache::UNKNOWN_ID),
+	_smoothPositionTime(0.15f),
+	_smoothPositionTimer(std::numeric_limits<float>::max()),
+	_smoothOrientationTime(0.15f),
+	_smoothOrientationTimer(std::numeric_limits<float>::max()),
+	_smoothPositionInitial(),
+	_smoothPositionTarget(),
+	_smoothOrientationInitial(),
+	_smoothOrientationTarget()
 {
     // we may have been created in the network thread, but we live in the main thread
     moveToThread(qApp->thread());
@@ -342,6 +350,25 @@ void Avatar::simulate(float deltaTime, bool inView) {
         _simulationInViewRate.increment();
     }
 
+	if (!isMyAvatar()) {
+		if (_smoothPositionTimer < _smoothPositionTime) {
+			// Smooth the remote avatar movement.
+			_smoothPositionTimer += deltaTime;
+			if (_smoothPositionTimer < _smoothPositionTime) {
+				AvatarData::setPosition(lerp(_smoothPositionInitial, _smoothPositionTarget, easeInOutQuad(_smoothPositionTimer / _smoothPositionTime)));
+				updateAttitude();
+			}
+		}
+
+		if (_smoothOrientationTimer < _smoothOrientationTime) {
+			// Smooth the remote avatar movement.
+			_smoothOrientationTimer += deltaTime;
+			if (_smoothOrientationTimer < _smoothOrientationTime) {
+				 AvatarData::setOrientation(slerp(_smoothOrientationInitial, _smoothOrientationTarget, easeInOutQuad(_smoothOrientationTimer / _smoothOrientationTime)));
+				 updateAttitude();
+			}
+		}
+	}
 
     PerformanceTimer perfTimer("simulate");
     {
@@ -1343,13 +1370,33 @@ glm::quat Avatar::getUncachedRightPalmRotation() const {
 }
 
 void Avatar::setPosition(const glm::vec3& position) {
-    AvatarData::setPosition(position);
-    updateAttitude();
+	if (isMyAvatar()) {
+		// This is the local avatar, no need to handle any position smoothing.
+		AvatarData::setPosition(position);
+		updateAttitude();
+
+		return;
+	}
+
+	// Whether or not there is an existing smoothing going on, just reset the smoothing timer and set the starting position as the avatar's current position, then smooth to the new position.
+	_smoothPositionInitial = getPosition();
+	_smoothPositionTarget = position;
+	_smoothPositionTimer = 0.0f;
 }
 
 void Avatar::setOrientation(const glm::quat& orientation) {
-    AvatarData::setOrientation(orientation);
-    updateAttitude();
+	if (isMyAvatar()) {
+		// This is the local avatar, no need to handle any position smoothing.
+		AvatarData::setOrientation(orientation);
+		updateAttitude();
+
+		return;
+	}
+
+	// Whether or not there is an existing smoothing going on, just reset the smoothing timer and set the starting position as the avatar's current position, then smooth to the new position.
+	_smoothOrientationInitial = getOrientation();
+	_smoothOrientationTarget = orientation;
+	_smoothOrientationTimer = 0.0f;
 }
 
 void Avatar::updatePalms() {
diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h
index ba7e1c617e..8d97553039 100644
--- a/interface/src/avatar/Avatar.h
+++ b/interface/src/avatar/Avatar.h
@@ -210,6 +210,10 @@ public:
 
     bool hasNewJointData() const { return _hasNewJointData; }
 
+	inline float easeInOutQuad(float t) {
+		return((t < 0.5) ? (2*t*t) : (t*(4 - 2*t) - 1));
+	}
+
 public slots:
 
     // FIXME - these should be migrated to use Pose data instead
@@ -292,6 +296,15 @@ protected:
     RateCounter<> _skeletonModelSimulationRate;
     RateCounter<> _jointDataSimulationRate;
 
+	// Smoothing data for blending from one position/orientation to another on remote agents.
+	float _smoothPositionTime;
+	float _smoothPositionTimer;
+	float _smoothOrientationTime;
+	float _smoothOrientationTimer;
+	glm::vec3 _smoothPositionInitial;
+	glm::vec3 _smoothPositionTarget;
+	glm::quat _smoothOrientationInitial;
+	glm::quat _smoothOrientationTarget;
 
 private:
     class AvatarEntityDataHash {