////  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 "AvatarMotionState.h" #include <PhysicsCollisionGroups.h>#include <PhysicsEngine.h>#include <PhysicsHelpers.h> AvatarMotionState::AvatarMotionState(OtherAvatarPointer avatar, const btCollisionShape* shape) : ObjectMotionState(shape), _avatar(avatar) {    assert(_avatar);    _type = MOTIONSTATE_TYPE_AVATAR;    _collisionGroup = BULLET_COLLISION_GROUP_OTHER_AVATAR;    cacheShapeDiameter();} void AvatarMotionState::handleEasyChanges(uint32_t& flags) {    ObjectMotionState::handleEasyChanges(flags);    if (flags & Simulation::DIRTY_PHYSICS_ACTIVATION && !_body->isActive()) {        _body->activate();    }     if (flags & Simulation::DIRTY_MASS) {        updateBodyMassProperties();    }} AvatarMotionState::~AvatarMotionState() {    assert(_avatar);    _avatar = nullptr;} // virtualuint32_t AvatarMotionState::getIncomingDirtyFlags() const {    return _body ? _dirtyFlags : 0;} void AvatarMotionState::clearIncomingDirtyFlags(uint32_t mask) {    if (_body) {        _dirtyFlags &= ~mask;    }} PhysicsMotionType AvatarMotionState::computePhysicsMotionType() const {    // TODO?: support non-DYNAMIC motion for avatars? (e.g. when sitting)    return MOTION_TYPE_DYNAMIC;} // 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(getObjectAngularVelocity()));    }} // virtualvoid AvatarMotionState::setWorldTransform(const btTransform& worldTrans) {    const float SPRING_TIMESCALE = 0.5f;    float tau = PHYSICS_ENGINE_FIXED_SUBSTEP / SPRING_TIMESCALE;    btVector3 currentPosition = worldTrans.getOrigin();    btVector3 offsetToTarget = glmToBullet(getObjectPosition()) - currentPosition;    float distance = offsetToTarget.length();    if ((1.0f - tau) * distance > _diameter) {        // the avatar body is far from its target --> slam position        btTransform newTransform;        newTransform.setOrigin(currentPosition + offsetToTarget);        newTransform.setRotation(glmToBullet(getObjectRotation()));        _body->setWorldTransform(newTransform);        _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));        _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));    } else {        // the avatar body is near its target --> slam velocity        btVector3 velocity = glmToBullet(getObjectLinearVelocity()) + (1.0f / SPRING_TIMESCALE) * offsetToTarget;        _body->setLinearVelocity(velocity);        _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));        // slam its rotation        btTransform newTransform = worldTrans;        newTransform.setRotation(glmToBullet(getObjectRotation()));        _body->setWorldTransform(newTransform);    }} // 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->getWorldPosition();} // virtualglm::quat AvatarMotionState::getObjectRotation() const {    return _avatar->getWorldOrientation();} // virtualglm::vec3 AvatarMotionState::getObjectLinearVelocity() const {    return _avatar->getWorldVelocity();} // virtualglm::vec3 AvatarMotionState::getObjectAngularVelocity() const {    // HACK: avatars use a capusle collision shape and their angularVelocity in the local simulation is unimportant.    // Therefore, as optimization toward support for larger crowds we ignore it and return zero.    //return _avatar->getWorldAngularVelocity();    return glm::vec3(0.0f);} // virtualglm::vec3 AvatarMotionState::getObjectGravity() const {    return _avatar->getAcceleration();} // virtualconst QUuid AvatarMotionState::getObjectID() const {    return _avatar->getSessionUUID();} QString AvatarMotionState::getName() const {    return _avatar->getName();} // virtualQUuid AvatarMotionState::getSimulatorID() const {    return _avatar->getSessionUUID();} // virtualvoid AvatarMotionState::computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const {    group = _collisionGroup;    mask = Physics::getDefaultCollisionMask(group);    if (!_avatar->getCollideWithOtherAvatars()) {        mask &= ~(BULLET_COLLISION_GROUP_MY_AVATAR | BULLET_COLLISION_GROUP_OTHER_AVATAR);    }} // virtualfloat AvatarMotionState::getMass() const {    return _avatar->computeMass();} void AvatarMotionState::cacheShapeDiameter() {    if (_shape) {        // shape is capsuleY        btVector3 aabbMin, aabbMax;        btTransform transform;        transform.setIdentity();        _shape->getAabb(transform, aabbMin, aabbMax);        _diameter = (aabbMax - aabbMin).getX();    } else {        _diameter = 0.0f;    }} void AvatarMotionState::setRigidBody(btRigidBody* body) {    ObjectMotionState::setRigidBody(body);    if (_body) {        // remove angular dynamics from this body        _body->setAngularFactor(0.0f);    }} void AvatarMotionState::setShape(const btCollisionShape* shape) {    ObjectMotionState::setShape(shape);    cacheShapeDiameter();}