update assumptions about facing vector on server

src/main/java/electrosphere/entity/state/movement/groundmove/ServerGroundMovementTree.java

@@ -163,13 +163,13 @@ public class ServerGroundMovementTree implements BehaviorTree {
         if(CreatureUtils.hasVelocity(parent)){
             maxNaturalVelocity = ServerGroundMovementTree.getMaximumVelocity(parent, this.groundMovementSystem, facing);
         }
-        if(ServerPlayerViewDirTree.hasTree(parent)){
-            ServerPlayerViewDirTree serverViewTree =ServerPlayerViewDirTree.getTree(parent);
-            CreatureUtils.setFacingVector(parent, CameraEntityUtils.getFacingVec(serverViewTree.getYaw(), serverViewTree.getPitch()));
-        }
         PoseActor poseActor = EntityUtils.getPoseActor(parent);
         Vector3d position = EntityUtils.getPosition(parent);
         Vector3d facingVector = CreatureUtils.getFacingVector(parent);
+        if(ServerPlayerViewDirTree.hasTree(parent)){
+            ServerPlayerViewDirTree serverViewTree =ServerPlayerViewDirTree.getTree(parent);
+            facingVector = CameraEntityUtils.getFacingVec(serverViewTree.getYaw(), serverViewTree.getPitch());
+        }
         DBody body = PhysicsEntityUtils.getDBody(parent);
         DVector3C linearVelocity = body.getLinearVel();
 
@@ -249,6 +249,7 @@ public class ServerGroundMovementTree implements BehaviorTree {
         //state machine
         switch(state){
             case STARTUP: {
+                CreatureUtils.setFacingVector(parent, facingVector);
                 rotation.set(movementQuaternion);
                 if(poseActor != null){
                     String animationToPlay = determineCorrectAnimation(MovementTreeState.STARTUP);
@@ -298,6 +299,7 @@ public class ServerGroundMovementTree implements BehaviorTree {
                 );
             } break;
             case MOVE: {
+                CreatureUtils.setFacingVector(parent, facingVector);
                 rotation.set(movementQuaternion);
                 //check if can restart animation
                 //if yes, restart animation
@@ -344,6 +346,7 @@ public class ServerGroundMovementTree implements BehaviorTree {
                 ServerScriptUtils.fireSignalOnEntity(parent, "entityGroundMove", AccessTransforms.getVector(position));
             } break;
             case SLOWDOWN: {
+                CreatureUtils.setFacingVector(parent, facingVector);
                 rotation.set(movementQuaternion);
                 //run slowdown code
                 if(poseActor != null){

src/main/java/electrosphere/entity/state/physicssync/upright/ServerAlwaysUprightTree.java

@@ -12,8 +12,10 @@ import electrosphere.entity.EntityDataStrings;
 import electrosphere.entity.EntityUtils;
 import electrosphere.entity.btree.BehaviorTree;
 import electrosphere.entity.state.physicssync.ServerPhysicsSyncTree;
+import electrosphere.entity.types.creature.CreatureUtils;
 import electrosphere.server.datacell.Realm;
 import electrosphere.server.datacell.utils.ServerBehaviorTreeUtils;
+import electrosphere.util.math.MathUtils;
 
 public class ServerAlwaysUprightTree implements BehaviorTree {
 
@@ -43,6 +45,11 @@ public class ServerAlwaysUprightTree implements BehaviorTree {
 
             //make sure rotation is vertical
             // sourceRotation = sourceRotation.mul(0.001, 0.001, 0.001, 1).normalize();
+            //calculate rotation based on facing vector
+            if(CreatureUtils.getFacingVector(parent) != null){
+                Vector3d facingVector = CreatureUtils.getFacingVector(parent);
+                sourceRotation = new Quaterniond().rotationTo(MathUtils.getOriginVector(), new Vector3d(facingVector.x,0,facingVector.z)).normalize();
+            }
 
             EntityUtils.getPosition(parent).set(position);
             EntityUtils.getRotation(parent).set(sourceRotation);

src/main/java/electrosphere/entity/state/server/ServerPlayerViewDirTree.java

@@ -1,5 +1,7 @@
 package electrosphere.entity.state.server;
 
+import org.joml.Quaterniond;
+
 import electrosphere.controls.CameraHandler;
 import electrosphere.entity.Entity;
 import electrosphere.entity.EntityDataStrings;
@@ -95,12 +97,20 @@ public class ServerPlayerViewDirTree implements BehaviorTree {
             //if first person, set facing angle
             if(perspective == CameraHandler.CAMERA_PERSPECTIVE_FIRST){
                 CreatureUtils.setFacingVector(parent, CameraEntityUtils.getFacingVec(yaw, pitch));
-                EntityUtils.getRotation(parent).set(CameraEntityUtils.getRotationQuat(yaw,pitch));
+                EntityUtils.getRotation(parent).set(CameraEntityUtils.getUprightQuat(yaw));
             }
 
 
             this.lastUpdateTime = time;
         }
     }
+
+    /**
+     * Gets the current rotation of the view dir tree
+     * @return The current rotation of the view dir tree
+     */
+    public Quaterniond getRotationQuat(){
+        return CameraEntityUtils.getRotationQuat(yaw,pitch);
+    }
     
 }

src/main/java/electrosphere/entity/types/camera/CameraEntityUtils.java

@@ -250,6 +250,18 @@ public class CameraEntityUtils {
         return quatd;
     }
 
+    /**
+     * Gets the entity rotation for the upright-aligned entity with a given yaw
+     * @param yaw The yaw of the camera
+     * @return The upright-aligned entity rotation
+     */
+    public static Quaterniond getUprightQuat(double yaw){
+        //quaternion is multiplied by pi because we want to point away from the eye of the camera, NOT towards it
+        Quaternionf quatRaw = CameraEntityUtils.getYawQuat(yaw).mul(new Quaternionf().rotateY((float)Math.PI));
+        Quaterniond quatd = new Quaterniond(quatRaw).normalize();
+        return quatd;
+    }
+
     /**
      * Gets the rotation matrix from a yaw and pitch
      * @param yaw The yaw