qrigidbody.cpp

Go to the documentation of this file.

// Copyright (C) 2021 The Qt Company Ltd.
// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR GPL-3.0-only
 
#include "qrigidbody_p.h"
#include "qphysicscommands_p.h"
#include "qphysicsworld_p.h"
 
QT_BEGIN_NAMESPACE
 
QDynamicRigidBody::QDynamicRigidBody() = default;
 
QDynamicRigidBody::~QDynamicRigidBody()
{
    qDeleteAll(m_commandQueue);
    m_commandQueue.clear();
}
 
const QQuaternion &QDynamicRigidBody::centerOfMassRotation() const
{
    return m_centerOfMassRotation;
}
 
void QDynamicRigidBody::setCenterOfMassRotation(const QQuaternion &newCenterOfMassRotation)
{
    if (qFuzzyCompare(m_centerOfMassRotation, newCenterOfMassRotation))
        return;
    m_centerOfMassRotation = newCenterOfMassRotation;
 
    // Only inertia tensor is using rotation
    if (m_massMode == MassMode::MassAndInertiaTensor)
        m_commandQueue.enqueue(new QPhysicsCommandSetMassAndInertiaTensor(m_mass, m_inertiaTensor));
 
    emit centerOfMassRotationChanged();
}
 
const QVector3D &QDynamicRigidBody::centerOfMassPosition() const
{
    return m_centerOfMassPosition;
}
 
void QDynamicRigidBody::setCenterOfMassPosition(const QVector3D &newCenterOfMassPosition)
{
    if (qFuzzyCompare(m_centerOfMassPosition, newCenterOfMassPosition))
        return;
 
    switch (m_massMode) {
    case MassMode::MassAndInertiaTensor: {
        m_commandQueue.enqueue(new QPhysicsCommandSetMassAndInertiaTensor(m_mass, m_inertiaTensor));
        break;
    }
    case MassMode::MassAndInertiaMatrix: {
        m_commandQueue.enqueue(new QPhysicsCommandSetMassAndInertiaMatrix(m_mass, m_inertiaMatrix));
        break;
    }
    case MassMode::DefaultDensity:
    case MassMode::CustomDensity:
    case MassMode::Mass:
        break;
    }
 
    m_centerOfMassPosition = newCenterOfMassPosition;
    emit centerOfMassPositionChanged();
}
 
QDynamicRigidBody::MassMode QDynamicRigidBody::massMode() const
{
    return m_massMode;
}
 
void QDynamicRigidBody::setMassMode(const MassMode newMassMode)
{
    if (m_massMode == newMassMode)
        return;
 
    switch (newMassMode) {
    case MassMode::DefaultDensity: {
        auto world = QPhysicsWorld::getWorld(this);
        if (world) {
            m_commandQueue.enqueue(new QPhysicsCommandSetDensity(world->defaultDensity()));
        } else {
            qWarning() << "No physics world found, cannot set default density.";
        }
        break;
    }
    case MassMode::CustomDensity: {
        m_commandQueue.enqueue(new QPhysicsCommandSetDensity(m_density));
        break;
    }
    case MassMode::Mass: {
        m_commandQueue.enqueue(new QPhysicsCommandSetMass(m_mass));
        break;
    }
    case MassMode::MassAndInertiaTensor: {
        m_commandQueue.enqueue(new QPhysicsCommandSetMassAndInertiaTensor(m_mass, m_inertiaTensor));
        break;
    }
    case MassMode::MassAndInertiaMatrix: {
        m_commandQueue.enqueue(new QPhysicsCommandSetMassAndInertiaMatrix(m_mass, m_inertiaMatrix));
        break;
    }
    }
 
    m_massMode = newMassMode;
    emit massModeChanged();
}
 
const QVector3D &QDynamicRigidBody::inertiaTensor() const
{
    return m_inertiaTensor;
}
 
void QDynamicRigidBody::setInertiaTensor(const QVector3D &newInertiaTensor)
{
    if (qFuzzyCompare(m_inertiaTensor, newInertiaTensor))
        return;
    m_inertiaTensor = newInertiaTensor;
 
    if (m_massMode == MassMode::MassAndInertiaTensor)
        m_commandQueue.enqueue(new QPhysicsCommandSetMassAndInertiaTensor(m_mass, m_inertiaTensor));
 
    emit inertiaTensorChanged();
}
 
const QList<float> &QDynamicRigidBody::readInertiaMatrix() const
{
    return m_inertiaMatrixList;
}
 
static bool fuzzyEquals(const QList<float> &a, const QList<float> &b)
{
    if (a.length() != b.length())
        return false;
 
    const int length = a.length();
    for (int i = 0; i < length; i++)
        if (!qFuzzyCompare(a[i], b[i]))
            return false;
 
    return true;
}
 
void QDynamicRigidBody::setInertiaMatrix(const QList<float> &newInertiaMatrix)
{
    if (fuzzyEquals(m_inertiaMatrixList, newInertiaMatrix))
        return;
 
    m_inertiaMatrixList = newInertiaMatrix;
    const int elemsToCopy = qMin(m_inertiaMatrixList.length(), 9);
    memcpy(m_inertiaMatrix.data(), m_inertiaMatrixList.data(), elemsToCopy * sizeof(float));
    memset(m_inertiaMatrix.data() + elemsToCopy, 0, (9 - elemsToCopy) * sizeof(float));
 
    if (m_massMode == MassMode::MassAndInertiaMatrix)
        m_commandQueue.enqueue(new QPhysicsCommandSetMassAndInertiaMatrix(m_mass, m_inertiaMatrix));
 
    emit inertiaMatrixChanged();
}
 
const QMatrix3x3 &QDynamicRigidBody::inertiaMatrix() const
{
    return m_inertiaMatrix;
}
 
float QDynamicRigidBody::mass() const
{
    return m_mass;
}
 
bool QDynamicRigidBody::isKinematic() const
{
    return m_isKinematic;
}
 
bool QDynamicRigidBody::gravityEnabled() const
{
    return m_gravityEnabled;
}
 
void QDynamicRigidBody::setMass(float mass)
{
    if (mass < 0.f || qFuzzyCompare(m_mass, mass))
        return;
 
    switch (m_massMode) {
    case QDynamicRigidBody::MassMode::Mass:
        m_commandQueue.enqueue(new QPhysicsCommandSetMass(mass));
        break;
    case QDynamicRigidBody::MassMode::MassAndInertiaTensor:
        m_commandQueue.enqueue(new QPhysicsCommandSetMassAndInertiaTensor(mass, m_inertiaTensor));
        break;
    case QDynamicRigidBody::MassMode::MassAndInertiaMatrix:
        m_commandQueue.enqueue(new QPhysicsCommandSetMassAndInertiaMatrix(mass, m_inertiaMatrix));
        break;
    case QDynamicRigidBody::MassMode::DefaultDensity:
    case QDynamicRigidBody::MassMode::CustomDensity:
        break;
    }
 
    m_mass = mass;
    emit massChanged(m_mass);
}
 
float QDynamicRigidBody::density() const
{
    return m_density;
}
 
void QDynamicRigidBody::setDensity(float density)
{
    if (qFuzzyCompare(m_density, density))
        return;
 
    if (m_massMode == MassMode::CustomDensity)
        m_commandQueue.enqueue(new QPhysicsCommandSetDensity(density));
 
    m_density = density;
    emit densityChanged(m_density);
}
 
void QDynamicRigidBody::setIsKinematic(bool isKinematic)
{
    if (m_isKinematic == isKinematic)
        return;
 
    if (hasStaticShapes() && !isKinematic) {
        qWarning()
                << "Cannot make body containing trimesh/heightfield/plane non-kinematic, ignoring.";
        return;
    }
 
    m_isKinematic = isKinematic;
    m_commandQueue.enqueue(new QPhysicsCommandSetIsKinematic(m_isKinematic));
    emit isKinematicChanged(m_isKinematic);
}
 
void QDynamicRigidBody::setGravityEnabled(bool gravityEnabled)
{
    if (m_gravityEnabled == gravityEnabled)
        return;
 
    m_gravityEnabled = gravityEnabled;
    m_commandQueue.enqueue(new QPhysicsCommandSetGravityEnabled(m_gravityEnabled));
    emit gravityEnabledChanged();
}
 
void QDynamicRigidBody::setAngularVelocity(const QVector3D &angularVelocity)
{
    m_commandQueue.enqueue(new QPhysicsCommandSetAngularVelocity(angularVelocity));
}
 
QDynamicRigidBody::AxisLock QDynamicRigidBody::linearAxisLock() const
{
    return m_linearAxisLock;
}
 
void QDynamicRigidBody::setLinearAxisLock(AxisLock newAxisLockLinear)
{
    if (m_linearAxisLock == newAxisLockLinear)
        return;
    m_linearAxisLock = newAxisLockLinear;
    emit linearAxisLockChanged();
}
 
QDynamicRigidBody::AxisLock QDynamicRigidBody::angularAxisLock() const
{
    return m_angularAxisLock;
}
 
void QDynamicRigidBody::setAngularAxisLock(AxisLock newAxisLockAngular)
{
    if (m_angularAxisLock == newAxisLockAngular)
        return;
    m_angularAxisLock = newAxisLockAngular;
    emit angularAxisLockChanged();
}
 
QQueue<QPhysicsCommand *> &QDynamicRigidBody::commandQueue()
{
    return m_commandQueue;
}
 
void QDynamicRigidBody::updateDefaultDensity(float defaultDensity)
{
    if (m_massMode == MassMode::DefaultDensity)
        m_commandQueue.enqueue(new QPhysicsCommandSetDensity(defaultDensity));
}
 
void QDynamicRigidBody::applyCentralForce(const QVector3D &force)
{
    m_commandQueue.enqueue(new QPhysicsCommandApplyCentralForce(force));
}
 
void QDynamicRigidBody::applyForce(const QVector3D &force, const QVector3D &position)
{
    m_commandQueue.enqueue(new QPhysicsCommandApplyForce(force, position));
}
 
void QDynamicRigidBody::applyTorque(const QVector3D &torque)
{
    m_commandQueue.enqueue(new QPhysicsCommandApplyTorque(torque));
}
 
void QDynamicRigidBody::applyCentralImpulse(const QVector3D &impulse)
{
    m_commandQueue.enqueue(new QPhysicsCommandApplyCentralImpulse(impulse));
}
 
void QDynamicRigidBody::applyImpulse(const QVector3D &impulse, const QVector3D &position)
{
    m_commandQueue.enqueue(new QPhysicsCommandApplyImpulse(impulse, position));
}
 
void QDynamicRigidBody::applyTorqueImpulse(const QVector3D &impulse)
{
    m_commandQueue.enqueue(new QPhysicsCommandApplyTorqueImpulse(impulse));
}
 
void QDynamicRigidBody::setLinearVelocity(const QVector3D &linearVelocity)
{
    m_commandQueue.enqueue(new QPhysicsCommandSetLinearVelocity(linearVelocity));
}
 
void QDynamicRigidBody::reset(const QVector3D &position, const QVector3D &eulerRotation)
{
    m_commandQueue.enqueue(new QPhysicsCommandReset(position, eulerRotation));
}
 
void QDynamicRigidBody::setKinematicRotation(const QQuaternion &rotation)
{
    if (m_kinematicRotation == rotation)
        return;
 
    m_kinematicRotation = rotation;
    emit kinematicRotationChanged(m_kinematicRotation);
    emit kinematicEulerRotationChanged(m_kinematicRotation.getEulerRotation());
}
 
QQuaternion QDynamicRigidBody::kinematicRotation() const
{
    return m_kinematicRotation.getQuaternionRotation();
}
 
void QDynamicRigidBody::setKinematicEulerRotation(const QVector3D &rotation)
{
    if (m_kinematicRotation == rotation)
        return;
 
    m_kinematicRotation = rotation;
    emit kinematicEulerRotationChanged(m_kinematicRotation);
    emit kinematicRotationChanged(m_kinematicRotation.getQuaternionRotation());
}
 
QVector3D QDynamicRigidBody::kinematicEulerRotation() const
{
    return m_kinematicRotation.getEulerRotation();
}
 
void QDynamicRigidBody::setKinematicPivot(const QVector3D &pivot)
{
    m_kinematicPivot = pivot;
    emit kinematicPivotChanged(m_kinematicPivot);
}
 
QVector3D QDynamicRigidBody::kinematicPivot() const
{
    return m_kinematicPivot;
}
 
void QDynamicRigidBody::setKinematicPosition(const QVector3D &position)
{
    m_kinematicPosition = position;
    emit kinematicPositionChanged(m_kinematicPosition);
}
 
QVector3D QDynamicRigidBody::kinematicPosition() const
{
    return m_kinematicPosition;
}
 
QStaticRigidBody::QStaticRigidBody() = default;
 
QT_END_NAMESPACE