////  AvatarMotionState.cpp//  interface/src/avatar/////  Created by Andrew Meadows 2015.05.14//  Copyright 2015 High Fidelity, Inc.////  Distributed under the Apache License, Version 2.0.//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html// #include <PhysicsCollisionGroups.h>#include <PhysicsEngine.h>#include <PhysicsHelpers.h> #include "Avatar.h"#include "AvatarMotionState.h"#include "BulletUtil.h" AvatarMotionState::AvatarMotionState(Avatar* avatar, btCollisionShape* shape) : ObjectMotionState(shape), _avatar(avatar) {    assert(_avatar);    _type = MOTIONSTATE_TYPE_AVATAR;    if (_shape) {        _mass = 100.0f; // HACK    }} AvatarMotionState::~AvatarMotionState() {    assert(_avatar);    _avatar = nullptr;} // virtualuint32_t AvatarMotionState::getIncomingDirtyFlags() {    return _body ? _dirtyFlags : 0;} void AvatarMotionState::clearIncomingDirtyFlags() {    if (_body) {        _dirtyFlags = 0;    }} PhysicsMotionType AvatarMotionState::computePhysicsMotionType() const {    // TODO?: support non-DYNAMIC motion for avatars? (e.g. when sitting)    return MOTION_TYPE_DYNAMIC;} // virtual and protectedbtCollisionShape* AvatarMotionState::computeNewShape() {    ShapeInfo shapeInfo;    _avatar->computeShapeInfo(shapeInfo);    return getShapeManager()->getShape(shapeInfo);} // virtualbool AvatarMotionState::isMoving() const {    return false;} // virtualvoid AvatarMotionState::getWorldTransform(btTransform& worldTrans) const {    worldTrans.setOrigin(glmToBullet(getObjectPosition()));    worldTrans.setRotation(glmToBullet(getObjectRotation()));    if (_body) {        _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));        _body->setAngularVelocity(glmToBullet(getObjectLinearVelocity()));    }} // virtualvoid AvatarMotionState::setWorldTransform(const btTransform& worldTrans) {    // HACK: The PhysicsEngine does not actually move OTHER avatars -- instead it slaves their local RigidBody to the transform    // as specified by a remote simulation.  However, to give the remote simulation time to respond to our own objects we tie    // the other avatar's body to its true position with a simple spring. This is a HACK that will have to be improved later.    const float SPRING_TIMESCALE = 0.5f;    float tau = PHYSICS_ENGINE_FIXED_SUBSTEP / SPRING_TIMESCALE;    btVector3 currentPosition = worldTrans.getOrigin();    btVector3 targetPosition = glmToBullet(getObjectPosition());    btTransform newTransform;    newTransform.setOrigin((1.0f - tau) * currentPosition + tau * targetPosition);    newTransform.setRotation(glmToBullet(getObjectRotation()));    _body->setWorldTransform(newTransform);    _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));    _body->setAngularVelocity(glmToBullet(getObjectLinearVelocity()));} // These pure virtual methods must be implemented for each MotionState type// and make it possible to implement more complicated methods in this base class. // virtualfloat AvatarMotionState::getObjectRestitution() const {    return 0.5f;} // virtualfloat AvatarMotionState::getObjectFriction() const {    return 0.5f;} // virtualfloat AvatarMotionState::getObjectLinearDamping() const {    return 0.5f;} // virtualfloat AvatarMotionState::getObjectAngularDamping() const {    return 0.5f;} // virtualglm::vec3 AvatarMotionState::getObjectPosition() const {    return _avatar->getPosition();} // virtualglm::quat AvatarMotionState::getObjectRotation() const {    return _avatar->getOrientation();} // virtualglm::vec3 AvatarMotionState::getObjectLinearVelocity() const {    return _avatar->getVelocity();} // virtualglm::vec3 AvatarMotionState::getObjectAngularVelocity() const {    return _avatar->getAngularVelocity();} // virtualglm::vec3 AvatarMotionState::getObjectGravity() const {    return _avatar->getAcceleration();} // virtualconst QUuid AvatarMotionState::getObjectID() const {    return _avatar->getSessionUUID();} // virtualQUuid AvatarMotionState::getSimulatorID() const {    return _avatar->getSessionUUID();} // virtualvoid AvatarMotionState::computeCollisionGroupAndMask(int16_t& group, int16_t& mask) const {    group = BULLET_COLLISION_GROUP_OTHER_AVATAR;    mask = Physics::getDefaultCollisionMask(group);}