mirror of
https://github.com/JulianGro/overte.git
synced 2025-04-25 22:55:06 +02:00
360 lines
11 KiB
C++
360 lines
11 KiB
C++
//
|
|
// ObjectConstraintConeTwist.cpp
|
|
// libraries/physics/src
|
|
//
|
|
// Created by Seth Alves 2017-4-29
|
|
// Copyright 2017 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 "ObjectConstraintConeTwist.h"
|
|
|
|
#include <LogHandler.h>
|
|
|
|
#include "QVariantGLM.h"
|
|
|
|
#include "EntityTree.h"
|
|
#include "PhysicsLogging.h"
|
|
|
|
const uint16_t CONE_TWIST_VERSION_WITH_UNUSED_PAREMETERS = 1;
|
|
const uint16_t ObjectConstraintConeTwist::constraintVersion = 2;
|
|
const glm::vec3 DEFAULT_CONE_TWIST_AXIS(1.0f, 0.0f, 0.0f);
|
|
|
|
ObjectConstraintConeTwist::ObjectConstraintConeTwist(const QUuid& id, EntityItemPointer ownerEntity) :
|
|
ObjectConstraint(DYNAMIC_TYPE_CONE_TWIST, id, ownerEntity),
|
|
_axisInA(DEFAULT_CONE_TWIST_AXIS),
|
|
_axisInB(DEFAULT_CONE_TWIST_AXIS)
|
|
{
|
|
#if WANT_DEBUG
|
|
qCDebug(physics) << "ObjectConstraintConeTwist::ObjectConstraintConeTwist";
|
|
#endif
|
|
}
|
|
|
|
ObjectConstraintConeTwist::~ObjectConstraintConeTwist() {
|
|
#if WANT_DEBUG
|
|
qCDebug(physics) << "ObjectConstraintConeTwist::~ObjectConstraintConeTwist";
|
|
#endif
|
|
}
|
|
|
|
QList<btRigidBody*> ObjectConstraintConeTwist::getRigidBodies() {
|
|
QList<btRigidBody*> result;
|
|
result += getRigidBody();
|
|
QUuid otherEntityID;
|
|
withReadLock([&]{
|
|
otherEntityID = _otherID;
|
|
});
|
|
if (!otherEntityID.isNull()) {
|
|
result += getOtherRigidBody(otherEntityID);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void ObjectConstraintConeTwist::prepareForPhysicsSimulation() {
|
|
}
|
|
|
|
void ObjectConstraintConeTwist::updateConeTwist() {
|
|
btConeTwistConstraint* constraint { nullptr };
|
|
float swingSpan1;
|
|
float swingSpan2;
|
|
float twistSpan;
|
|
|
|
withReadLock([&]{
|
|
constraint = static_cast<btConeTwistConstraint*>(_constraint);
|
|
swingSpan1 = _swingSpan1;
|
|
swingSpan2 = _swingSpan2;
|
|
twistSpan = _twistSpan;
|
|
});
|
|
|
|
if (!constraint) {
|
|
return;
|
|
}
|
|
|
|
constraint->setLimit(swingSpan1,
|
|
swingSpan2,
|
|
twistSpan);
|
|
}
|
|
|
|
|
|
btTypedConstraint* ObjectConstraintConeTwist::getConstraint() {
|
|
btConeTwistConstraint* constraint { nullptr };
|
|
QUuid otherEntityID;
|
|
glm::vec3 pivotInA;
|
|
glm::vec3 axisInA;
|
|
glm::vec3 pivotInB;
|
|
glm::vec3 axisInB;
|
|
|
|
withReadLock([&]{
|
|
constraint = static_cast<btConeTwistConstraint*>(_constraint);
|
|
pivotInA = _pivotInA;
|
|
axisInA = _axisInA;
|
|
otherEntityID = _otherID;
|
|
pivotInB = _pivotInB;
|
|
axisInB = _axisInB;
|
|
});
|
|
if (constraint) {
|
|
return constraint;
|
|
}
|
|
|
|
static int repeatMessageID = LogHandler::getInstance().newRepeatedMessageID();
|
|
|
|
btRigidBody* rigidBodyA = getRigidBody();
|
|
if (!rigidBodyA) {
|
|
HIFI_FCDEBUG_ID(physics(), repeatMessageID, "ObjectConstraintConeTwist::getConstraint -- no rigidBodyA");
|
|
return nullptr;
|
|
}
|
|
|
|
if (glm::length(axisInA) < FLT_EPSILON) {
|
|
qCWarning(physics) << "cone-twist axis cannot be a zero vector";
|
|
axisInA = DEFAULT_CONE_TWIST_AXIS;
|
|
} else {
|
|
axisInA = glm::normalize(axisInA);
|
|
}
|
|
|
|
if (!otherEntityID.isNull()) {
|
|
// This coneTwist is between two entities... find the other rigid body.
|
|
|
|
if (glm::length(axisInB) < FLT_EPSILON) {
|
|
qCWarning(physics) << "cone-twist axis cannot be a zero vector";
|
|
axisInB = DEFAULT_CONE_TWIST_AXIS;
|
|
} else {
|
|
axisInB = glm::normalize(axisInB);
|
|
}
|
|
|
|
glm::quat rotA = glm::rotation(DEFAULT_CONE_TWIST_AXIS, axisInA);
|
|
glm::quat rotB = glm::rotation(DEFAULT_CONE_TWIST_AXIS, axisInB);
|
|
|
|
btTransform frameInA(glmToBullet(rotA), glmToBullet(pivotInA));
|
|
btTransform frameInB(glmToBullet(rotB), glmToBullet(pivotInB));
|
|
|
|
btRigidBody* rigidBodyB = getOtherRigidBody(otherEntityID);
|
|
if (!rigidBodyB) {
|
|
HIFI_FCDEBUG_ID(physics(), repeatMessageID, "ObjectConstraintConeTwist::getConstraint -- no rigidBodyB");
|
|
return nullptr;
|
|
}
|
|
|
|
constraint = new btConeTwistConstraint(*rigidBodyA, *rigidBodyB, frameInA, frameInB);
|
|
} else {
|
|
// This coneTwist is between an entity and the world-frame.
|
|
|
|
glm::quat rot = glm::rotation(DEFAULT_CONE_TWIST_AXIS, axisInA);
|
|
|
|
btTransform frameInA(glmToBullet(rot), glmToBullet(pivotInA));
|
|
|
|
constraint = new btConeTwistConstraint(*rigidBodyA, frameInA);
|
|
}
|
|
|
|
withWriteLock([&]{
|
|
_constraint = constraint;
|
|
});
|
|
|
|
// if we don't wake up rigidBodyA, we may not send the dynamicData property over the network
|
|
forceBodyNonStatic();
|
|
activateBody();
|
|
|
|
updateConeTwist();
|
|
|
|
return constraint;
|
|
}
|
|
|
|
|
|
bool ObjectConstraintConeTwist::updateArguments(QVariantMap arguments) {
|
|
glm::vec3 pivotInA;
|
|
glm::vec3 axisInA;
|
|
QUuid otherEntityID;
|
|
glm::vec3 pivotInB;
|
|
glm::vec3 axisInB;
|
|
float swingSpan1;
|
|
float swingSpan2;
|
|
float twistSpan;
|
|
|
|
bool needUpdate = false;
|
|
bool somethingChanged = ObjectDynamic::updateArguments(arguments);
|
|
withReadLock([&]{
|
|
bool ok = true;
|
|
pivotInA = EntityDynamicInterface::extractVec3Argument("coneTwist constraint", arguments, "pivot", ok, false);
|
|
if (!ok) {
|
|
pivotInA = _pivotInA;
|
|
}
|
|
|
|
ok = true;
|
|
axisInA = EntityDynamicInterface::extractVec3Argument("coneTwist constraint", arguments, "axis", ok, false);
|
|
if (!ok) {
|
|
axisInA = _axisInA;
|
|
}
|
|
|
|
ok = true;
|
|
otherEntityID = QUuid(EntityDynamicInterface::extractStringArgument("coneTwist constraint",
|
|
arguments, "otherEntityID", ok, false));
|
|
if (!ok) {
|
|
otherEntityID = _otherID;
|
|
}
|
|
|
|
ok = true;
|
|
pivotInB = EntityDynamicInterface::extractVec3Argument("coneTwist constraint", arguments, "otherPivot", ok, false);
|
|
if (!ok) {
|
|
pivotInB = _pivotInB;
|
|
}
|
|
|
|
ok = true;
|
|
axisInB = EntityDynamicInterface::extractVec3Argument("coneTwist constraint", arguments, "otherAxis", ok, false);
|
|
if (!ok) {
|
|
axisInB = _axisInB;
|
|
}
|
|
|
|
ok = true;
|
|
swingSpan1 = EntityDynamicInterface::extractFloatArgument("coneTwist constraint", arguments, "swingSpan1", ok, false);
|
|
if (!ok) {
|
|
swingSpan1 = _swingSpan1;
|
|
}
|
|
|
|
ok = true;
|
|
swingSpan2 = EntityDynamicInterface::extractFloatArgument("coneTwist constraint", arguments, "swingSpan2", ok, false);
|
|
if (!ok) {
|
|
swingSpan2 = _swingSpan2;
|
|
}
|
|
|
|
ok = true;
|
|
twistSpan = EntityDynamicInterface::extractFloatArgument("coneTwist constraint", arguments, "twistSpan", ok, false);
|
|
if (!ok) {
|
|
twistSpan = _twistSpan;
|
|
}
|
|
|
|
if (somethingChanged ||
|
|
pivotInA != _pivotInA ||
|
|
axisInA != _axisInA ||
|
|
otherEntityID != _otherID ||
|
|
pivotInB != _pivotInB ||
|
|
axisInB != _axisInB ||
|
|
swingSpan1 != _swingSpan1 ||
|
|
swingSpan2 != _swingSpan2 ||
|
|
twistSpan != _twistSpan) {
|
|
// something changed
|
|
needUpdate = true;
|
|
}
|
|
});
|
|
|
|
if (needUpdate) {
|
|
withWriteLock([&] {
|
|
_pivotInA = pivotInA;
|
|
_axisInA = axisInA;
|
|
_otherID = otherEntityID;
|
|
_pivotInB = pivotInB;
|
|
_axisInB = axisInB;
|
|
_swingSpan1 = swingSpan1;
|
|
_swingSpan2 = swingSpan2;
|
|
_twistSpan = twistSpan;
|
|
|
|
_active = true;
|
|
|
|
auto ownerEntity = _ownerEntity.lock();
|
|
if (ownerEntity) {
|
|
ownerEntity->setDynamicDataDirty(true);
|
|
ownerEntity->setDynamicDataNeedsTransmit(true);
|
|
}
|
|
});
|
|
|
|
updateConeTwist();
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/**jsdoc
|
|
* The <code>"cone-twist"</code> {@link Entities.ActionType|ActionType} connects two entities with a joint that can move
|
|
* through a cone and can twist.
|
|
* It has arguments in addition to the common {@link Entities.ActionArguments|ActionArguments}.
|
|
*
|
|
* @typedef {object} Entities.ActionArguments-ConeTwist
|
|
* @property {Vec3} pivot=0,0,0 - The local offset of the joint relative to the entity's position.
|
|
* @property {Vec3} axis=1,0,0 - The axis of the entity that moves through the cone. Must be a non-zero vector.
|
|
* @property {Uuid} otherEntityID=null - The ID of the other entity that is connected to the joint.
|
|
* @property {Vec3} otherPivot=0,0,0 - The local offset of the joint relative to the other entity's position.
|
|
* @property {Vec3} otherAxis=1,0,0 - The axis of the other entity that moves through the cone. Must be a non-zero vector.
|
|
* @property {number} swingSpan1=6.238 - The angle through which the joint can move in one axis of the cone, in radians.
|
|
* @property {number} swingSpan2=6.238 - The angle through which the joint can move in the other axis of the cone, in radians.
|
|
* @property {number} twistSpan=6.238 - The angle through with the joint can twist, in radians.
|
|
*/
|
|
QVariantMap ObjectConstraintConeTwist::getArguments() {
|
|
QVariantMap arguments = ObjectDynamic::getArguments();
|
|
withReadLock([&] {
|
|
arguments["pivot"] = vec3ToQMap(_pivotInA);
|
|
arguments["axis"] = vec3ToQMap(_axisInA);
|
|
arguments["otherEntityID"] = _otherID;
|
|
arguments["otherPivot"] = vec3ToQMap(_pivotInB);
|
|
arguments["otherAxis"] = vec3ToQMap(_axisInB);
|
|
arguments["swingSpan1"] = _swingSpan1;
|
|
arguments["swingSpan2"] = _swingSpan2;
|
|
arguments["twistSpan"] = _twistSpan;
|
|
});
|
|
return arguments;
|
|
}
|
|
|
|
QByteArray ObjectConstraintConeTwist::serialize() const {
|
|
QByteArray serializedConstraintArguments;
|
|
QDataStream dataStream(&serializedConstraintArguments, QIODevice::WriteOnly);
|
|
|
|
dataStream << DYNAMIC_TYPE_CONE_TWIST;
|
|
dataStream << getID();
|
|
dataStream << ObjectConstraintConeTwist::constraintVersion;
|
|
|
|
withReadLock([&] {
|
|
dataStream << localTimeToServerTime(_expires);
|
|
dataStream << _tag;
|
|
|
|
dataStream << _pivotInA;
|
|
dataStream << _axisInA;
|
|
dataStream << _otherID;
|
|
dataStream << _pivotInB;
|
|
dataStream << _axisInB;
|
|
dataStream << _swingSpan1;
|
|
dataStream << _swingSpan2;
|
|
dataStream << _twistSpan;
|
|
});
|
|
|
|
return serializedConstraintArguments;
|
|
}
|
|
|
|
void ObjectConstraintConeTwist::deserialize(QByteArray serializedArguments) {
|
|
QDataStream dataStream(serializedArguments);
|
|
|
|
EntityDynamicType type;
|
|
dataStream >> type;
|
|
assert(type == getType());
|
|
|
|
QUuid id;
|
|
dataStream >> id;
|
|
assert(id == getID());
|
|
|
|
uint16_t serializationVersion;
|
|
dataStream >> serializationVersion;
|
|
if (serializationVersion > ObjectConstraintConeTwist::constraintVersion) {
|
|
assert(false);
|
|
return;
|
|
}
|
|
|
|
withWriteLock([&] {
|
|
quint64 serverExpires;
|
|
dataStream >> serverExpires;
|
|
_expires = serverTimeToLocalTime(serverExpires);
|
|
dataStream >> _tag;
|
|
|
|
dataStream >> _pivotInA;
|
|
dataStream >> _axisInA;
|
|
dataStream >> _otherID;
|
|
dataStream >> _pivotInB;
|
|
dataStream >> _axisInB;
|
|
dataStream >> _swingSpan1;
|
|
dataStream >> _swingSpan2;
|
|
dataStream >> _twistSpan;
|
|
if (serializationVersion == CONE_TWIST_VERSION_WITH_UNUSED_PAREMETERS) {
|
|
float softness, biasFactor, relaxationFactor;
|
|
dataStream >> softness;
|
|
dataStream >> biasFactor;
|
|
dataStream >> relaxationFactor;
|
|
}
|
|
|
|
_active = true;
|
|
});
|
|
}
|