From eba518cb65634edb8dd47824ecb01f25eb9b27ed Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Mon, 13 Jun 2016 14:26:41 -0700
Subject: [PATCH] try to make code that converts bullet-calculated values to
parent-frame values more effecient
---
libraries/physics/src/EntityMotionState.cpp | 53 +++++++--------------
libraries/shared/src/SpatiallyNestable.cpp | 27 -----------
libraries/shared/src/SpatiallyNestable.h | 4 --
3 files changed, 17 insertions(+), 67 deletions(-)
diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp
index 5fffc9901b..8f22c576f0 100644
--- a/libraries/physics/src/EntityMotionState.cpp
+++ b/libraries/physics/src/EntityMotionState.cpp
@@ -271,44 +271,25 @@ bool EntityMotionState::isCandidateForOwnership() const {
bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
// NOTE: we only get here if we think we own the simulation
assert(_body);
+
+ bool parentTransformSuccess;
+ Transform localToWorld = _entity->getParentTransform(parentTransformSuccess);
+ Transform worldToLocal;
+ Transform worldVelocityToLocal;
+ if (parentTransformSuccess) {
+ localToWorld.evalInverse(worldToLocal);
+ worldVelocityToLocal = worldToLocal;
+ worldVelocityToLocal.setTranslation(glm::vec3(0.0f));
+ }
+
// if we've never checked before, our _lastStep will be 0, and we need to initialize our state
if (_lastStep == 0) {
btTransform xform = _body->getWorldTransform();
-
- // _serverPosition = _entity->worldPositionToParent(bulletToGLM(xform.getOrigin()));
- // _serverRotation = _entity->worldRotationToParent(bulletToGLM(xform.getRotation()));
- // _serverVelocity = _entity->worldVelocityToParent(getBodyLinearVelocityGTSigma());
-
-
- _serverPosition = bulletToGLM(xform.getOrigin());
- _serverRotation = bulletToGLM(xform.getRotation());
- _serverVelocity = getBodyLinearVelocityGTSigma();
- bool success;
- Transform parentTransform = _entity->getParentTransform(success);
- if (success) {
- Transform bodyTransform;
- bodyTransform.setTranslation(_serverPosition);
- bodyTransform.setRotation(_serverRotation);
- Transform result;
- Transform::inverseMult(result, parentTransform, bodyTransform);
- _serverPosition = result.getTranslation();
- _serverRotation = result.getRotation();
-
- // transform velocity into parent-frame
- parentTransform.setTranslation(glm::vec3(0.0f));
- Transform velocityTransform;
- velocityTransform.setTranslation(_serverVelocity);
- Transform myWorldTransform;
- Transform::mult(myWorldTransform, parentTransform, velocityTransform);
- myWorldTransform.setTranslation(_serverVelocity);
- Transform::inverseMult(result, parentTransform, myWorldTransform);
- _serverVelocity = result.getTranslation();
- }
-
-
-
+ _serverPosition = worldToLocal.transform(bulletToGLM(xform.getOrigin()));
+ _serverRotation = worldToLocal.getRotation() * bulletToGLM(xform.getRotation());
+ _serverVelocity = worldVelocityToLocal.transform(getBodyLinearVelocityGTSigma());
_serverAcceleration = Vectors::ZERO;
- _serverAngularVelocity = _entity->worldVelocityToParent(bulletToGLM(_body->getAngularVelocity()));
+ _serverAngularVelocity = worldVelocityToLocal.transform(bulletToGLM(_body->getAngularVelocity()));
_lastStep = simulationStep;
_serverActionData = _entity->getActionData();
_numInactiveUpdates = 1;
@@ -381,7 +362,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
// compute position error
btTransform worldTrans = _body->getWorldTransform();
- glm::vec3 position = _entity->worldPositionToParent(bulletToGLM(worldTrans.getOrigin()));
+ glm::vec3 position = worldToLocal.transform(bulletToGLM(worldTrans.getOrigin()));
float dx2 = glm::distance2(position, _serverPosition);
const float MAX_POSITION_ERROR_SQUARED = 0.000004f; // corresponds to 2mm
@@ -416,7 +397,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
}
}
const float MIN_ROTATION_DOT = 0.99999f; // This corresponds to about 0.5 degrees of rotation
- glm::quat actualRotation = _entity->worldRotationToParent(bulletToGLM(worldTrans.getRotation()));
+ glm::quat actualRotation = worldToLocal.getRotation() * bulletToGLM(worldTrans.getRotation());
#ifdef WANT_DEBUG
if ((fabsf(glm::dot(actualRotation, _serverRotation)) < MIN_ROTATION_DOT)) {
diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp
index 6edf80ab98..29a033f340 100644
--- a/libraries/shared/src/SpatiallyNestable.cpp
+++ b/libraries/shared/src/SpatiallyNestable.cpp
@@ -174,15 +174,6 @@ glm::vec3 SpatiallyNestable::worldToLocal(const glm::vec3& position,
return result.getTranslation();
}
-glm::vec3 SpatiallyNestable::worldPositionToParent(const glm::vec3& position) {
- bool success;
- glm::vec3 result = SpatiallyNestable::worldToLocal(position, getParentID(), getParentJointIndex(), success);
- if (!success) {
- qDebug() << "Warning -- worldToLocal failed" << getID();
- }
- return result;
-}
-
glm::vec3 SpatiallyNestable::worldVelocityToLocal(const glm::vec3& velocity, // can be linear or angular
const QUuid& parentID, int parentJointIndex,
bool& success) {
@@ -225,15 +216,6 @@ glm::vec3 SpatiallyNestable::worldVelocityToLocal(const glm::vec3& velocity, //
return result.getTranslation();
}
-glm::vec3 SpatiallyNestable::worldVelocityToParent(const glm::vec3& velocity) {
- bool success;
- glm::vec3 result = SpatiallyNestable::worldVelocityToLocal(velocity, getParentID(), getParentJointIndex(), success);
- if (!success) {
- qDebug() << "Warning -- worldVelocityToLocal failed" << getID();
- }
- return result;
-}
-
glm::quat SpatiallyNestable::worldToLocal(const glm::quat& orientation,
const QUuid& parentID, int parentJointIndex,
bool& success) {
@@ -274,15 +256,6 @@ glm::quat SpatiallyNestable::worldToLocal(const glm::quat& orientation,
return result.getRotation();
}
-glm::quat SpatiallyNestable::worldRotationToParent(const glm::quat& orientation) {
- bool success;
- glm::quat result = SpatiallyNestable::worldToLocal(orientation, getParentID(), getParentJointIndex(), success);
- if (!success) {
- qDebug() << "Warning -- worldToLocal failed" << getID();
- }
- return result;
-}
-
glm::vec3 SpatiallyNestable::localToWorld(const glm::vec3& position,
const QUuid& parentID, int parentJointIndex,
bool& success) {
diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h
index ffb00ac040..23beffda53 100644
--- a/libraries/shared/src/SpatiallyNestable.h
+++ b/libraries/shared/src/SpatiallyNestable.h
@@ -53,10 +53,6 @@ public:
static glm::vec3 localToWorld(const glm::vec3& position, const QUuid& parentID, int parentJointIndex, bool& success);
static glm::quat localToWorld(const glm::quat& orientation, const QUuid& parentID, int parentJointIndex, bool& success);
- glm::vec3 worldPositionToParent(const glm::vec3& position);
- glm::vec3 worldVelocityToParent(const glm::vec3& velocity);
- glm::quat worldRotationToParent(const glm::quat& orientation);
-
// world frame
virtual const Transform getTransform(bool& success, int depth = 0) const;
virtual void setTransform(const Transform& transform, bool& success);