C++ ObjectMotionState::getRigidBody方法代码示例

btRigidBody* ObjectAction::getRigidBody() {
    auto ownerEntity = _ownerEntity.lock();
    if (!ownerEntity) {
        return nullptr;
    }
    void* physicsInfo = ownerEntity->getPhysicsInfo();
    if (!physicsInfo) {
        return nullptr;
    }
    ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
    return motionState->getRigidBody();
}

void ObjectActionOffset::updateActionWorker(btScalar deltaTimeStep) {
    withTryReadLock([&] {
        auto ownerEntity = _ownerEntity.lock();
        if (!ownerEntity) {
            return;
        }

        void* physicsInfo = ownerEntity->getPhysicsInfo();
        if (!physicsInfo) {
            return;
        }

        ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
        btRigidBody* rigidBody = motionState->getRigidBody();
        if (!rigidBody) {
            qDebug() << "ObjectActionOffset::updateActionWorker no rigidBody";
            return;
        }

        const float MAX_LINEAR_TIMESCALE = 600.0f;  // 10 minutes is a long time
        if (_positionalTargetSet && _linearTimeScale < MAX_LINEAR_TIMESCALE) {
            glm::vec3 objectPosition = bulletToGLM(rigidBody->getCenterOfMassPosition());
            glm::vec3 springAxis = objectPosition - _pointToOffsetFrom; // from anchor to object
            float distance = glm::length(springAxis);
            if (distance > FLT_EPSILON) {
                springAxis /= distance;  // normalize springAxis

                // compute (critically damped) target velocity of spring relaxation
                glm::vec3 offset = (distance - _linearDistance) * springAxis;
                glm::vec3 targetVelocity = (-1.0f / _linearTimeScale) * offset;

                // compute current velocity and its parallel component
                glm::vec3 currentVelocity = bulletToGLM(rigidBody->getLinearVelocity());
                glm::vec3 parallelVelocity = glm::dot(currentVelocity, springAxis) * springAxis;

                // we blend the parallel component with the spring's target velocity to get the new velocity
                float blend = deltaTimeStep / _linearTimeScale;
                if (blend > 1.0f) {
                    blend = 1.0f;
                }
                rigidBody->setLinearVelocity(glmToBullet(currentVelocity + blend * (targetVelocity - parallelVelocity)));
            }
        }
    });
}

btRigidBody* ObjectDynamic::getOtherRigidBody(EntityItemID otherEntityID) {
    EntityItemPointer otherEntity = getEntityByID(otherEntityID);
    if (!otherEntity) {
        return nullptr;
    }

    void* otherPhysicsInfo = otherEntity->getPhysicsInfo();
    if (!otherPhysicsInfo) {
        return nullptr;
    }

    ObjectMotionState* otherMotionState = static_cast<ObjectMotionState*>(otherPhysicsInfo);
    if (!otherMotionState) {
        return nullptr;
    }

    return otherMotionState->getRigidBody();
}

btRigidBody* ObjectDynamic::getRigidBody() {
    ObjectMotionState* motionState = nullptr;
    withReadLock([&]{
        auto ownerEntity = _ownerEntity.lock();
        if (!ownerEntity) {
            return;
        }
        void* physicsInfo = ownerEntity->getPhysicsInfo();
        if (!physicsInfo) {
            return;
        }
        motionState = static_cast<ObjectMotionState*>(physicsInfo);
    });
    if (motionState) {
        return motionState->getRigidBody();
    }
    return nullptr;
}

void AvatarActionHold::doKinematicUpdate(float deltaTimeStep) {
    auto ownerEntity = _ownerEntity.lock();
    if (!ownerEntity) {
        qDebug() << "AvatarActionHold::doKinematicUpdate -- no owning entity";
        return;
    }
    void* physicsInfo = ownerEntity->getPhysicsInfo();
    if (!physicsInfo) {
        qDebug() << "AvatarActionHold::doKinematicUpdate -- no owning physics info";
        return;
    }
    ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
    btRigidBody* rigidBody = motionState ? motionState->getRigidBody() : nullptr;
    if (!rigidBody) {
        qDebug() << "AvatarActionHold::doKinematicUpdate -- no rigidBody";
        return;
    }

    withWriteLock([&] {
        if (_kinematicSetVelocity) {
            if (_previousSet) {
                glm::vec3 positionalVelocity = (_positionalTarget - _previousPositionalTarget) / deltaTimeStep;
                rigidBody->setLinearVelocity(glmToBullet(positionalVelocity));
            }
        }

        btTransform worldTrans = rigidBody->getWorldTransform();
        worldTrans.setOrigin(glmToBullet(_positionalTarget));
        worldTrans.setRotation(glmToBullet(_rotationalTarget));
        rigidBody->setWorldTransform(worldTrans);

        motionState->dirtyInternalKinematicChanges();

        _previousPositionalTarget = _positionalTarget;
        _previousRotationalTarget = _rotationalTarget;
        _previousSet = true;
    });

    activateBody();
}

void ObjectActionSpring::updateActionWorker(btScalar deltaTimeStep) {
    if (!tryLockForRead()) {
        // don't risk hanging the thread running the physics simulation
        qDebug() << "ObjectActionSpring::updateActionWorker lock failed";
        return;
    }

    auto ownerEntity = _ownerEntity.lock();
    if (!ownerEntity) {
        return;
    }

    void* physicsInfo = ownerEntity->getPhysicsInfo();
    if (!physicsInfo) {
        unlock();
        return;
    }
    ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
    btRigidBody* rigidBody = motionState->getRigidBody();
    if (!rigidBody) {
        unlock();
        qDebug() << "ObjectActionSpring::updateActionWorker no rigidBody";
        return;
    }

    const float MAX_TIMESCALE = 600.0f; // 10 min is a long time
    if (_linearTimeScale < MAX_TIMESCALE) {
        btVector3 offset = rigidBody->getCenterOfMassPosition() - glmToBullet(_positionalTarget);
        float offsetLength = offset.length();
        float speed = (offsetLength > FLT_EPSILON) ? glm::min(offsetLength / _linearTimeScale, SPRING_MAX_SPEED) : 0.0f;

        // this action is aggresively critically damped and defeats the current velocity
        rigidBody->setLinearVelocity((- speed / offsetLength) * offset);
    }

    if (_angularTimeScale < MAX_TIMESCALE) {
        btVector3 targetVelocity(0.0f, 0.0f, 0.0f);

        btQuaternion bodyRotation = rigidBody->getOrientation();
        auto alignmentDot = bodyRotation.dot(glmToBullet(_rotationalTarget));
        const float ALMOST_ONE = 0.99999f;
        if (glm::abs(alignmentDot) < ALMOST_ONE) {
            btQuaternion target = glmToBullet(_rotationalTarget);
            if (alignmentDot < 0.0f) {
                target = -target;
            }
            // if dQ is the incremental rotation that gets an object from Q0 to Q1 then:
            //
            //      Q1 = dQ * Q0
            //
            // solving for dQ gives:
            //
            //      dQ = Q1 * Q0^
            btQuaternion deltaQ = target * bodyRotation.inverse();
            float angle = deltaQ.getAngle();
            const float MIN_ANGLE = 1.0e-4f;
            if (angle > MIN_ANGLE) {
                targetVelocity = (angle / _angularTimeScale) * deltaQ.getAxis();
            }
        }
        // this action is aggresively critically damped and defeats the current velocity
        rigidBody->setAngularVelocity(targetVelocity);
    }
    unlock();
}

void AvatarActionHold::doKinematicUpdate(float deltaTimeStep) {
    auto ownerEntity = _ownerEntity.lock();
    if (!ownerEntity) {
        qDebug() << "AvatarActionHold::doKinematicUpdate -- no owning entity";
        return;
    }
    if (ownerEntity->getParentID() != QUuid()) {
        // if the held entity has been given a parent, stop acting on it.
        return;
    }

    void* physicsInfo = ownerEntity->getPhysicsInfo();
    if (!physicsInfo) {
        qDebug() << "AvatarActionHold::doKinematicUpdate -- no owning physics info";
        return;
    }
    ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
    btRigidBody* rigidBody = motionState ? motionState->getRigidBody() : nullptr;
    if (!rigidBody) {
        qDebug() << "AvatarActionHold::doKinematicUpdate -- no rigidBody";
        return;
    }

    withWriteLock([&]{
        if (_previousSet &&
            _positionalTarget != _previousPositionalTarget) { // don't average in a zero velocity if we get the same data
            glm::vec3 oneFrameVelocity = (_positionalTarget - _previousPositionalTarget) / deltaTimeStep;

            _measuredLinearVelocities[_measuredLinearVelocitiesIndex++] = oneFrameVelocity;
            if (_measuredLinearVelocitiesIndex >= AvatarActionHold::velocitySmoothFrames) {
                _measuredLinearVelocitiesIndex = 0;
            }
        }

        glm::vec3 measuredLinearVelocity;
        for (int i = 0; i < AvatarActionHold::velocitySmoothFrames; i++) {
            // there is a bit of lag between when someone releases the trigger and when the software reacts to
            // the release.  we calculate the velocity from previous frames but we don't include several
            // of the most recent.
            //
            // if _measuredLinearVelocitiesIndex is
            //     0 -- ignore i of 3 4 5
            //     1 -- ignore i of 4 5 0
            //     2 -- ignore i of 5 0 1
            //     3 -- ignore i of 0 1 2
            //     4 -- ignore i of 1 2 3
            //     5 -- ignore i of 2 3 4

            // This code is now disabled, but I'm leaving it commented-out because I suspect it will come back.
            // if ((i + 1) % AvatarActionHold::velocitySmoothFrames == _measuredLinearVelocitiesIndex ||
            //     (i + 2) % AvatarActionHold::velocitySmoothFrames == _measuredLinearVelocitiesIndex ||
            //     (i + 3) % AvatarActionHold::velocitySmoothFrames == _measuredLinearVelocitiesIndex) {
            //     continue;
            // }

            measuredLinearVelocity += _measuredLinearVelocities[i];
        }
        measuredLinearVelocity /= (float)(AvatarActionHold::velocitySmoothFrames
                                          // - 3  // 3 because of the 3 we skipped, above
                                          );

        if (_kinematicSetVelocity) {
            rigidBody->setLinearVelocity(glmToBullet(measuredLinearVelocity));
            rigidBody->setAngularVelocity(glmToBullet(_angularVelocityTarget));
        }

        btTransform worldTrans = rigidBody->getWorldTransform();
        worldTrans.setOrigin(glmToBullet(_positionalTarget));
        worldTrans.setRotation(glmToBullet(_rotationalTarget));
        rigidBody->setWorldTransform(worldTrans);

        motionState->dirtyInternalKinematicChanges();

        _previousPositionalTarget = _positionalTarget;
        _previousRotationalTarget = _rotationalTarget;
        _previousDeltaTimeStep = deltaTimeStep;
        _previousSet = true;
    });

    forceBodyNonStatic();
    activateBody(true);
}

void ObjectActionTravelOriented::updateActionWorker(btScalar deltaTimeStep) {
    withReadLock([&] {
        auto ownerEntity = _ownerEntity.lock();
        if (!ownerEntity) {
            return;
        }
        void* physicsInfo = ownerEntity->getPhysicsInfo();
        if (!physicsInfo) {
            return;
        }
        ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
        btRigidBody* rigidBody = motionState->getRigidBody();
        if (!rigidBody) {
            qCDebug(physics) << "ObjectActionTravelOriented::updateActionWorker no rigidBody";
            return;
        }
        const float MAX_TIMESCALE = 600.0f; // 10 min is a long time
        if (_angularTimeScale > MAX_TIMESCALE) {
            return;
        }

        // find normalized velocity
        glm::vec3 velocity = bulletToGLM(rigidBody->getLinearVelocity());
        float speed = glm::length(velocity);
        const float TRAVEL_ORIENTED_TOO_SLOW = 0.001f; // meters / second
        if (speed < TRAVEL_ORIENTED_TOO_SLOW) {
            return;
        }
        glm::vec3 direction = glm::normalize(velocity);

        // find current angle of "forward"
        btQuaternion bodyRotation = rigidBody->getOrientation();
        glm::quat orientation = bulletToGLM(bodyRotation);
        glm::vec3 forwardInWorldFrame = glm::normalize(orientation * _forward);

        // find the rotation that would line up direction and forward
        glm::quat neededRotation = glm::rotation(forwardInWorldFrame, direction);
        glm::quat rotationalTarget = neededRotation * orientation;

        btVector3 targetAngularVelocity(0.0f, 0.0f, 0.0f);

        auto alignmentDot = bodyRotation.dot(glmToBullet(rotationalTarget));
        const float ALMOST_ONE = 0.99999f;
        if (glm::abs(alignmentDot) < ALMOST_ONE) {
            btQuaternion target = glmToBullet(rotationalTarget);
            if (alignmentDot < 0.0f) {
                target = -target;
            }
            // if dQ is the incremental rotation that gets an object from Q0 to Q1 then:
            //
            //      Q1 = dQ * Q0
            //
            // solving for dQ gives:
            //
            //      dQ = Q1 * Q0^
            btQuaternion deltaQ = target * bodyRotation.inverse();
            float speed = deltaQ.getAngle() / _angularTimeScale;
            targetAngularVelocity = speed * deltaQ.getAxis();
            if (speed > rigidBody->getAngularSleepingThreshold()) {
                rigidBody->activate();
            }
        }
        // this action is aggresively critically damped and defeats the current velocity
        rigidBody->setAngularVelocity(targetAngularVelocity);
    });
}