Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-20 03:44:02 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

add support for non-physical kinematic movement

Browse source
This commit is contained in:
Andrew Meadows 2015-01-23 11:01:46 -08:00
parent 1b7074e52a
commit d173afaa70
7 changed files with 176 additions and 61 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

23
libraries/entities/src/EntityItem.cpp
View file

@ -520,7 +520,7 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef
bytesRead += readEntitySubclassDataFromBuffer(dataAt, (bytesLeftToRead - bytesRead), args, propertyFlags, overwriteLocalData);
recalculateCollisionShape();
if (overwriteLocalData && (getDirtyFlags() & EntityItem::DIRTY_POSITION)) {
if (overwriteLocalData && (getDirtyFlags() & (EntityItem::DIRTY_POSITION | EntityItem::DIRTY_VELOCITY))) {
_lastSimulated = now;
}
}
@ -770,6 +770,9 @@ void EntityItem::simulateSimpleKinematicMotion(float timeElapsed) {
float angularSpeed = glm::length(_angularVelocity);
const float EPSILON_ANGULAR_VELOCITY_LENGTH = 0.1f; //
if (angularSpeed < EPSILON_ANGULAR_VELOCITY_LENGTH) {
if (angularSpeed > 0.0f) {
_dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE;
}
setAngularVelocity(ENTITY_ITEM_ZERO_VEC3);
} else {
// NOTE: angularSpeed is currently in degrees/sec!!!
@ -799,10 +802,18 @@ void EntityItem::simulateSimpleKinematicMotion(float timeElapsed) {
// "ground" plane of the domain, but for now it's what we've got
velocity += getGravity() * timeElapsed;
}
// NOTE: the simulation should NOT set any DirtyFlags on this entity
setPosition(position); // this will automatically recalculate our collision shape
setVelocity(velocity);
float speed = glm::length(velocity);
const float EPSILON_LINEAR_VELOCITY_LENGTH = 0.001f; // 1mm/sec
if (speed < EPSILON_LINEAR_VELOCITY_LENGTH) {
setVelocity(ENTITY_ITEM_ZERO_VEC3);
if (speed > 0.0f) {
_dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE;
}
} else {
setPosition(position); // this will automatically recalculate our collision shape
setVelocity(velocity);
}
}
}
@ -886,7 +897,7 @@ bool EntityItem::setProperties(const EntityItemProperties& properties) {
if (_created != UNKNOWN_CREATED_TIME) {
setLastEdited(now);
}
if (getDirtyFlags() & EntityItem::DIRTY_POSITION) {
if (getDirtyFlags() & (EntityItem::DIRTY_POSITION | EntityItem::DIRTY_VELOCITY)) {
_lastSimulated = now;
}
}

41
libraries/physics/src/EntityMotionState.cpp
View file

@ -14,7 +14,7 @@
#include "BulletUtil.h"
#include "EntityMotionState.h"
#include "SimpleEntityKinematicController.h"
#include "PhysicsEngine.h"
QSet<EntityItem*>* _outgoingEntityList;
@ -41,8 +41,6 @@ EntityMotionState::~EntityMotionState() {
assert(_entity);
_entity->setPhysicsInfo(NULL);
_entity = NULL;
delete _kinematicController;
_kinematicController = NULL;
}
MotionType EntityMotionState::computeMotionType() const {
@ -52,13 +50,15 @@ MotionType EntityMotionState::computeMotionType() const {
return _entity->isMoving() ? MOTION_TYPE_KINEMATIC : MOTION_TYPE_STATIC;
}
void EntityMotionState::addKinematicController() {
if (!_kinematicController) {
_kinematicController = new SimpleEntityKinematicController(_entity);
_kinematicController->start();
} else {
_kinematicController->start();
}
void EntityMotionState::updateKinematicState(uint32_t substep) {
setKinematic(_entity->isMoving(), substep);
}
void EntityMotionState::stepKinematicSimulation(uint32_t substep) {
assert(_isKinematic);
float dt = (substep - _lastKinematicSubstep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
_entity->simulateSimpleKinematicMotion(dt);
_lastKinematicSubstep = substep;
}
// This callback is invoked by the physics simulation in two cases:
@ -67,8 +67,11 @@ void EntityMotionState::addKinematicController() {
// (2) at the beginning of each simulation frame for KINEMATIC RigidBody's --
// it is an opportunity for outside code to update the object's simulation position
void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
if (_kinematicController && _kinematicController->isRunning()) {
_kinematicController->stepForward();
if (_isKinematic) {
uint32_t substep = PhysicsEngine::getNumSubsteps();
// remove const-ness so we can actually update this instance
EntityMotionState* thisMotion = const_cast<EntityMotionState*>(this);
thisMotion->stepKinematicSimulation(substep);
}
worldTrans.setOrigin(glmToBullet(_entity->getPositionInMeters() - ObjectMotionState::getWorldOffset()));
worldTrans.setRotation(glmToBullet(_entity->getRotation()));
@ -229,12 +232,14 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_
uint32_t EntityMotionState::getIncomingDirtyFlags() const {
uint32_t dirtyFlags = _entity->getDirtyFlags();
// we add DIRTY_MOTION_TYPE if the body's motion type disagrees with entity velocity settings
int bodyFlags = _body->getCollisionFlags();
bool isMoving = _entity->isMoving();
if (((bodyFlags & btCollisionObject::CF_STATIC_OBJECT) && isMoving) ||
(bodyFlags & btCollisionObject::CF_KINEMATIC_OBJECT && !isMoving)) {
dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE;
if (_body) {
// we add DIRTY_MOTION_TYPE if the body's motion type disagrees with entity velocity settings
int bodyFlags = _body->getCollisionFlags();
bool isMoving = _entity->isMoving();
if (((bodyFlags & btCollisionObject::CF_STATIC_OBJECT) && isMoving) ||
(bodyFlags & btCollisionObject::CF_KINEMATIC_OBJECT && !isMoving)) {
dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE;
}
}
return dirtyFlags;
}

5
libraries/physics/src/EntityMotionState.h
View file

@ -14,7 +14,6 @@
#include <AACube.h>
#include "KinematicController.h"
#include "ObjectMotionState.h"
class EntityItem;
@ -39,8 +38,8 @@ public:
/// \return MOTION_TYPE_DYNAMIC or MOTION_TYPE_STATIC based on params set in EntityItem
MotionType computeMotionType() const;
// virtual override for ObjectMotionState
void addKinematicController();
void updateKinematicState(uint32_t substep);
void stepKinematicSimulation(uint32_t substep);
// this relays incoming position/rotation to the RigidBody
void getWorldTransform(btTransform& worldTrans) const;

26
libraries/physics/src/ObjectMotionState.cpp
View file

@ -12,7 +12,6 @@
#include <math.h>
#include "BulletUtil.h"
#include "KinematicController.h"
#include "ObjectMotionState.h"
#include "PhysicsEngine.h"
@ -56,10 +55,6 @@ ObjectMotionState::ObjectMotionState() :
ObjectMotionState::~ObjectMotionState() {
// NOTE: you MUST remove this MotionState from the world before you call the dtor.
assert(_body == NULL);
if (_kinematicController) {
delete _kinematicController;
_kinematicController = NULL;
}
}
void ObjectMotionState::setFriction(float friction) {
@ -108,6 +103,15 @@ bool ObjectMotionState::doesNotNeedToSendUpdate() const {
bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) {
assert(_body);
// if we've never checked before, our _sentFrame will be 0, and we need to initialize our state
if (_sentFrame == 0) {
_sentPosition = bulletToGLM(_body->getWorldTransform().getOrigin());
_sentVelocity = bulletToGLM(_body->getLinearVelocity());
_sentAngularVelocity = bulletToGLM(_body->getAngularVelocity());
_sentFrame = simulationFrame;
return false;
}
float dt = (float)(simulationFrame - _sentFrame) * PHYSICS_ENGINE_FIXED_SUBSTEP;
_sentFrame = simulationFrame;
bool isActive = _body->isActive();
@ -164,13 +168,6 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) {
return (fabsf(glm::dot(actualRotation, _sentRotation)) < MIN_ROTATION_DOT);
}
void ObjectMotionState::removeKinematicController() {
if (_kinematicController) {
delete _kinematicController;
_kinematicController = NULL;
}
}
void ObjectMotionState::setRigidBody(btRigidBody* body) {
// give the body a (void*) back-pointer to this ObjectMotionState
if (_body != body) {
@ -183,3 +180,8 @@ void ObjectMotionState::setRigidBody(btRigidBody* body) {
}
}
}
void ObjectMotionState::setKinematic(bool kinematic, uint32_t substep) {
_isKinematic = kinematic;
_lastKinematicSubstep = substep;
}

14
libraries/physics/src/ObjectMotionState.h
View file

@ -46,7 +46,6 @@ const uint32_t OUTGOING_DIRTY_PHYSICS_FLAGS = EntityItem::DIRTY_POSITION | Entit
class OctreeEditPacketSender;
class KinematicController;
class ObjectMotionState : public btMotionState {
public:
@ -93,11 +92,15 @@ public:
virtual MotionType computeMotionType() const = 0;
virtual void addKinematicController() = 0;
virtual void removeKinematicController();
virtual void updateKinematicState(uint32_t substep) = 0;
btRigidBody* getRigidBody() const { return _body; }
bool isKinematic() const { return _isKinematic; }
void setKinematic(bool kinematic, uint32_t substep);
virtual void stepKinematicSimulation(uint32_t substep) = 0;
friend class PhysicsEngine;
protected:
void setRigidBody(btRigidBody* body);
@ -114,6 +117,9 @@ protected:
btRigidBody* _body;
bool _isKinematic = false;
uint32_t _lastKinematicSubstep = 0;
bool _sentMoving; // true if last update was moving
int _numNonMovingUpdates; // RELIABLE_SEND_HACK for "not so reliable" resends of packets for non-moving objects
@ -124,8 +130,6 @@ protected:
glm::vec3 _sentVelocity;
glm::vec3 _sentAngularVelocity; // radians per second
glm::vec3 _sentAcceleration;
KinematicController* _kinematicController = NULL;
};
#endif // hifi_ObjectMotionState_h

119
libraries/physics/src/PhysicsEngine.cpp
View file

@ -62,7 +62,13 @@ void PhysicsEngine::addEntityInternal(EntityItem* entity) {
entity->setPhysicsInfo(static_cast<void*>(motionState));
_entityMotionStates.insert(motionState);
addObject(shapeInfo, shape, motionState);
} else {
} else if (entity->isMoving()) {
EntityMotionState* motionState = new EntityMotionState(entity);
entity->setPhysicsInfo(static_cast<void*>(motionState));
_entityMotionStates.insert(motionState);
motionState->setKinematic(true, _numSubsteps);
_nonPhysicalKinematicObjects.insert(motionState);
// We failed to add the entity to the simulation. Probably because we couldn't create a shape for it.
//qDebug() << "failed to add entity " << entity->getEntityItemID() << " to physics engine";
}
@ -74,10 +80,16 @@ void PhysicsEngine::removeEntityInternal(EntityItem* entity) {
void* physicsInfo = entity->getPhysicsInfo();
if (physicsInfo) {
EntityMotionState* motionState = static_cast<EntityMotionState*>(physicsInfo);
removeObject(motionState);
if (motionState->getRigidBody()) {
removeObject(motionState);
} else {
// only need to hunt in this list when there is no RigidBody
_nonPhysicalKinematicObjects.remove(motionState);
}
_entityMotionStates.remove(motionState);
_incomingChanges.remove(motionState);
_outgoingPackets.remove(motionState);
// NOTE: EntityMotionState dtor will remove its backpointer from EntityItem
delete motionState;
}
}
@ -117,6 +129,7 @@ void PhysicsEngine::clearEntitiesInternal() {
delete (*stateItr);
}
_entityMotionStates.clear();
_nonPhysicalKinematicObjects.clear();
_incomingChanges.clear();
_outgoingPackets.clear();
}
@ -127,19 +140,75 @@ void PhysicsEngine::relayIncomingChangesToSimulation() {
QSet<ObjectMotionState*>::iterator stateItr = _incomingChanges.begin();
while (stateItr != _incomingChanges.end()) {
ObjectMotionState* motionState = *stateItr;
++stateItr;
uint32_t flags = motionState->getIncomingDirtyFlags() & DIRTY_PHYSICS_FLAGS;
bool removeMotionState = false;
btRigidBody* body = motionState->getRigidBody();
if (body) {
if (flags & HARD_DIRTY_PHYSICS_FLAGS) {
// a HARD update requires the body be pulled out of physics engine, changed, then reinserted
// but it also handles all EASY changes
updateObjectHard(body, motionState, flags);
bool success = updateObjectHard(body, motionState, flags);
if (!success) {
// NOTE: since updateObjectHard() failed we know that motionState has been removed
// from simulation and body has been deleted. Depending on what else has changed
// we might need to remove motionState altogether...
if (flags & EntityItem::DIRTY_VELOCITY) {
motionState->updateKinematicState(_numSubsteps);
if (motionState->isKinematic()) {
// all is NOT lost, we still need to move this object around kinematically
_nonPhysicalKinematicObjects.insert(motionState);
} else {
// no need to keep motionState around
removeMotionState = true;
}
} else {
// no need to keep motionState around
removeMotionState = true;
}
}
} else if (flags) {
// an EASY update does NOT require that the body be pulled out of physics engine
// hence the MotionState has all the knowledge and authority to perform the update.
motionState->updateObjectEasy(flags, _numSubsteps);
}
} else {
// the only way we should ever get here (motionState exists but no body) is when the object
// is undergoing non-physical kinematic motion.
assert(_nonPhysicalKinematicObjects.contains(motionState));
// it is possible that the changes are such that the object can now be added to the physical simulation
if (flags & EntityItem::DIRTY_SHAPE) {
ShapeInfo shapeInfo;
motionState->computeShapeInfo(shapeInfo);
btCollisionShape* shape = _shapeManager.getShape(shapeInfo);
if (shape) {
addObject(shapeInfo, shape, motionState);
_nonPhysicalKinematicObjects.remove(motionState);
} else if (flags & EntityItem::DIRTY_VELOCITY) {
// although we couldn't add the object to the simulation, might need to update kinematic motion...
motionState->updateKinematicState(_numSubsteps);
if (!motionState->isKinematic()) {
_nonPhysicalKinematicObjects.remove(motionState);
removeMotionState = true;
}
}
} else if (flags & EntityItem::DIRTY_VELOCITY) {
// although we still can't add to physics simulation, might need to update kinematic motion...
motionState->updateKinematicState(_numSubsteps);
if (!motionState->isKinematic()) {
_nonPhysicalKinematicObjects.remove(motionState);
removeMotionState = true;
}
}
}
if (removeMotionState) {
// if we get here then there is no need to keep this motionState around (no physics or kinematics)
_outgoingPackets.remove(motionState);
// NOTE: motionState will clean up its own backpointers in the Object
delete motionState;
continue;
}
// NOTE: the grand order of operations is:
@ -152,7 +221,6 @@ void PhysicsEngine::relayIncomingChangesToSimulation() {
// outgoing changes at this point.
motionState->clearOutgoingPacketFlags(flags); // clear outgoing flags that were trumped
motionState->clearIncomingDirtyFlags(flags); // clear incoming flags that were processed
++stateItr;
}
_incomingChanges.clear();
}
@ -229,6 +297,7 @@ void PhysicsEngine::stepSimulation() {
// This is step (2).
int numSubsteps = _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, PHYSICS_ENGINE_FIXED_SUBSTEP);
_numSubsteps += (uint32_t)numSubsteps;
stepNonPhysicalKinematics();
unlock();
// This is step (3) which is done outside of stepSimulation() so we can lock _entityTree.
@ -248,6 +317,18 @@ void PhysicsEngine::stepSimulation() {
computeCollisionEvents();
}
// TODO: need to update non-physical kinematic objects
void PhysicsEngine::stepNonPhysicalKinematics() {
QSet<ObjectMotionState*>::iterator stateItr = _nonPhysicalKinematicObjects.begin();
while (stateItr != _nonPhysicalKinematicObjects.end()) {
ObjectMotionState* motionState = *stateItr;
motionState->stepKinematicSimulation(_numSubsteps);
++stateItr;
}
}
// TODO?: need to occasionally scan for stopped non-physical kinematics objects
void PhysicsEngine::computeCollisionEvents() {
// update all contacts
int numManifolds = _collisionDispatcher->getNumManifolds();
@ -322,7 +403,7 @@ void PhysicsEngine::addObject(const ShapeInfo& shapeInfo, btCollisionShape* shap
body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
body->updateInertiaTensor();
motionState->setRigidBody(body);
motionState->addKinematicController();
motionState->setKinematic(true, _numSubsteps);
const float KINEMATIC_LINEAR_VELOCITY_THRESHOLD = 0.01f; // 1 cm/sec
const float KINEMATIC_ANGULAR_VELOCITY_THRESHOLD = 0.01f; // ~1 deg/sec
body->setSleepingThresholds(KINEMATIC_LINEAR_VELOCITY_THRESHOLD, KINEMATIC_ANGULAR_VELOCITY_THRESHOLD);
@ -334,6 +415,7 @@ void PhysicsEngine::addObject(const ShapeInfo& shapeInfo, btCollisionShape* shap
body = new btRigidBody(mass, motionState, shape, inertia);
body->updateInertiaTensor();
motionState->setRigidBody(body);
motionState->setKinematic(false, _numSubsteps);
motionState->updateObjectVelocities();
// NOTE: Bullet will deactivate any object whose velocity is below these thresholds for longer than 2 seconds.
// (the 2 seconds is determined by: static btRigidBody::gDeactivationTime
@ -348,6 +430,7 @@ void PhysicsEngine::addObject(const ShapeInfo& shapeInfo, btCollisionShape* shap
body->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
body->updateInertiaTensor();
motionState->setRigidBody(body);
motionState->setKinematic(false, _numSubsteps);
break;
}
}
@ -358,7 +441,7 @@ void PhysicsEngine::addObject(const ShapeInfo& shapeInfo, btCollisionShape* shap
_dynamicsWorld->addRigidBody(body);
}
bool PhysicsEngine::removeObject(ObjectMotionState* motionState) {
void PhysicsEngine::removeObject(ObjectMotionState* motionState) {
assert(motionState);
btRigidBody* body = motionState->getRigidBody();
if (body) {
@ -369,16 +452,14 @@ bool PhysicsEngine::removeObject(ObjectMotionState* motionState) {
_shapeManager.releaseShape(shapeInfo);
delete body;
motionState->setRigidBody(NULL);
motionState->removeKinematicController();
motionState->setKinematic(false, _numSubsteps);
removeContacts(motionState);
return true;
}
return false;
}
// private
void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags) {
bool PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags) {
MotionType newType = motionState->computeMotionType();
// pull body out of physics engine
@ -393,7 +474,16 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
ShapeInfo shapeInfo;
motionState->computeShapeInfo(shapeInfo);
btCollisionShape* newShape = _shapeManager.getShape(shapeInfo);
if (newShape != oldShape) {
if (!newShape) {
// FAIL! we are unable to support these changes!
_shapeManager.releaseShape(oldShape);
delete body;
motionState->setRigidBody(NULL);
motionState->setKinematic(false, _numSubsteps);
removeContacts(motionState);
return false;
} else if (newShape != oldShape) {
// BUG: if shape doesn't change but density does then we won't compute new mass properties
// TODO: fix this BUG by replacing DIRTY_MASS with DIRTY_DENSITY and then fix logic accordingly.
body->setCollisionShape(newShape);
@ -426,7 +516,7 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
body->setMassProps(0.0f, btVector3(0.0f, 0.0f, 0.0f));
body->updateInertiaTensor();
motionState->addKinematicController();
motionState->setKinematic(true, _numSubsteps);
break;
}
case MOTION_TYPE_DYNAMIC: {
@ -443,7 +533,7 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
body->updateInertiaTensor();
}
body->forceActivationState(ACTIVE_TAG);
motionState->removeKinematicController();
motionState->setKinematic(false, _numSubsteps);
break;
}
default: {
@ -458,7 +548,7 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
body->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f));
body->setAngularVelocity(btVector3(0.0f, 0.0f, 0.0f));
motionState->removeKinematicController();
motionState->setKinematic(false, _numSubsteps);
break;
}
}
@ -467,4 +557,5 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
_dynamicsWorld->addRigidBody(body);
body->activate();
return true;
}

9
libraries/physics/src/PhysicsEngine.h
View file

@ -67,6 +67,7 @@ public:
virtual void init(EntityEditPacketSender* packetSender);
void stepSimulation();
void stepNonPhysicalKinematics();
void computeCollisionEvents();
@ -81,15 +82,16 @@ public:
void addObject(const ShapeInfo& shapeInfo, btCollisionShape* shape, ObjectMotionState* motionState);
/// \param motionState pointer to Object's MotionState
/// \return true if Object removed
bool removeObject(ObjectMotionState* motionState);
void removeObject(ObjectMotionState* motionState);
/// process queue of changed from external sources
void relayIncomingChangesToSimulation();
private:
void removeContacts(ObjectMotionState* motionState);
void updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);
// return 'true' of update was successful
bool updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);
void updateObjectEasy(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);
btClock _clock;
@ -104,6 +106,7 @@ private:
// EntitySimulation stuff
QSet<EntityMotionState*> _entityMotionStates; // all entities that we track
QSet<ObjectMotionState*> _nonPhysicalKinematicObjects; // not in physics simulation, but still need kinematic simulation
QSet<ObjectMotionState*> _incomingChanges; // entities with pending physics changes by script or packet
QSet<ObjectMotionState*> _outgoingPackets; // MotionStates with pending changes that need to be sent over wire