//// PhysicalEntitySimulation.cpp// libraries/physcis/src//// Created by Andrew Meadows 2015.04.27// 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 "PhysicsHelpers.h"#include "PhysicsLogging.h"#include "ShapeManager.h"#include "PhysicalEntitySimulation.h"PhysicalEntitySimulation::PhysicalEntitySimulation() {}PhysicalEntitySimulation::~PhysicalEntitySimulation() {}void PhysicalEntitySimulation::init( EntityTree* tree, PhysicsEngine* physicsEngine, EntityEditPacketSender* packetSender) { assert(tree); setEntityTree(tree); assert(physicsEngine); _physicsEngine = physicsEngine; assert(packetSender); _entityPacketSender = packetSender;}// begin EntitySimulation overridesvoid PhysicalEntitySimulation::updateEntitiesInternal(const quint64& now) { // Do nothing here because the "internal" update the PhysicsEngine::stepSimualtion() which is done elsewhere.}void PhysicalEntitySimulation::addEntityInternal(EntityItemPointer entity) { assert(entity); if (entity->shouldBePhysical()) { EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo()); if (!motionState) { _pendingAdds.insert(entity); } } else if (entity->isMoving()) { _simpleKinematicEntities.insert(entity); }}void PhysicalEntitySimulation::removeEntityInternal(EntityItemPointer entity) { EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo()); if (motionState) { motionState->clearObjectBackPointer(); entity->setPhysicsInfo(nullptr); _pendingRemoves.insert(motionState); _outgoingChanges.remove(motionState); } _pendingAdds.remove(entity);}void PhysicalEntitySimulation::changeEntityInternal(EntityItemPointer entity) { // queue incoming changes: from external sources (script, EntityServer, etc) to physics engine assert(entity); EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo()); if (motionState) { if (!entity->shouldBePhysical()) { // the entity should be removed from the physical simulation _pendingChanges.remove(motionState); _physicalObjects.remove(motionState); _pendingRemoves.insert(motionState); _outgoingChanges.remove(motionState); if (entity->isMoving()) { _simpleKinematicEntities.insert(entity); } } else { _pendingChanges.insert(motionState); } } else if (entity->shouldBePhysical()) { // The intent is for this object to be in the PhysicsEngine, but it has no MotionState yet. // Perhaps it's shape has changed and it can now be added? _pendingAdds.insert(entity); _simpleKinematicEntities.remove(entity); // just in case it's non-physical-kinematic } else if (entity->isMoving()) { _simpleKinematicEntities.insert(entity); } else { _simpleKinematicEntities.remove(entity); // just in case it's non-physical-kinematic }}void PhysicalEntitySimulation::clearEntitiesInternal() { // TODO: we should probably wait to lock the _physicsEngine so we don't mess up data structures // while it is in the middle of a simulation step. As it is, we're probably in shutdown mode // anyway, so maybe the simulation was already properly shutdown? Cross our fingers... // first disconnect each MotionStates from its Entity for (auto stateItr : _physicalObjects) { EntityMotionState* motionState = static_cast<EntityMotionState*>(&(*stateItr)); EntityItemPointer entity = motionState->getEntity(); if (entity) { entity->setPhysicsInfo(nullptr); } motionState->clearObjectBackPointer(); } // then delete the objects (aka MotionStates) _physicsEngine->deleteObjects(_physicalObjects); // finally clear all lists (which now have only dangling pointers) _physicalObjects.clear(); _pendingRemoves.clear(); _pendingAdds.clear(); _pendingChanges.clear();}// end EntitySimulation overridesVectorOfMotionStates& PhysicalEntitySimulation::getObjectsToDelete() { _tempVector.clear(); for (auto stateItr : _pendingRemoves) { EntityMotionState* motionState = &(*stateItr); _pendingChanges.remove(motionState); _physicalObjects.remove(motionState); EntityItemPointer entity = motionState->getEntity(); if (entity) { _pendingAdds.remove(entity); entity->setPhysicsInfo(nullptr); motionState->clearObjectBackPointer(); } _tempVector.push_back(motionState); } _pendingRemoves.clear(); return _tempVector;}VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToAdd() { _tempVector.clear(); SetOfEntities::iterator entityItr = _pendingAdds.begin(); while (entityItr != _pendingAdds.end()) { EntityItemPointer entity = *entityItr; assert(!entity->getPhysicsInfo()); if (!entity->shouldBePhysical()) { // this entity should no longer be on the internal _pendingAdds entityItr = _pendingAdds.erase(entityItr); if (entity->isMoving()) { _simpleKinematicEntities.insert(entity); } } else if (entity->isReadyToComputeShape()) { ShapeInfo shapeInfo; entity->computeShapeInfo(shapeInfo); btCollisionShape* shape = ObjectMotionState::getShapeManager()->getShape(shapeInfo); if (shape) { EntityMotionState* motionState = new EntityMotionState(shape, entity); entity->setPhysicsInfo(static_cast<void*>(motionState)); _physicalObjects.insert(motionState); _tempVector.push_back(motionState); entityItr = _pendingAdds.erase(entityItr); } else { //qDebug() << "Warning! Failed to generate new shape for entity." << entity->getName(); ++entityItr; } } else { ++entityItr; } } return _tempVector;}VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToChange() { _tempVector.clear(); for (auto stateItr : _pendingChanges) { EntityMotionState* motionState = &(*stateItr); _tempVector.push_back(motionState); } _pendingChanges.clear(); return _tempVector;}void PhysicalEntitySimulation::handleOutgoingChanges(VectorOfMotionStates& motionStates, const QUuid& sessionID) { // walk the motionStates looking for those that correspond to entities for (auto stateItr : motionStates) { ObjectMotionState* state = &(*stateItr); if (state && state->getType() == MOTIONSTATE_TYPE_ENTITY) { EntityMotionState* entityState = static_cast<EntityMotionState*>(state); EntityItemPointer entity = entityState->getEntity(); if (entity) { if (entityState->isCandidateForOwnership(sessionID)) { _outgoingChanges.insert(entityState); } _entitiesToSort.insert(entityState->getEntity()); } } } uint32_t numSubsteps = _physicsEngine->getNumSubsteps(); if (_lastStepSendPackets != numSubsteps) { _lastStepSendPackets = numSubsteps; if (sessionID.isNull()) { // usually don't get here, but if so --> nothing to do _outgoingChanges.clear(); return; } // send outgoing packets QSet<EntityMotionState*>::iterator stateItr = _outgoingChanges.begin(); while (stateItr != _outgoingChanges.end()) { EntityMotionState* state = *stateItr; if (!state->isCandidateForOwnership(sessionID)) { stateItr = _outgoingChanges.erase(stateItr); } else if (state->shouldSendUpdate(numSubsteps, sessionID)) { state->sendUpdate(_entityPacketSender, sessionID, numSubsteps); ++stateItr; } else { ++stateItr; } } }}void PhysicalEntitySimulation::handleCollisionEvents(CollisionEvents& collisionEvents) { for (auto collision : collisionEvents) { // NOTE: The collision event is always aligned such that idA is never NULL. // however idB may be NULL. if (!collision.idB.isNull()) { emit entityCollisionWithEntity(collision.idA, collision.idB, collision); } }}void PhysicalEntitySimulation::applyActionChanges() { if (_physicsEngine) { foreach (EntityActionPointer actionToAdd, _actionsToAdd) { _physicsEngine->addAction(actionToAdd); } foreach (QUuid actionToRemove, _actionsToRemove) { _physicsEngine->removeAction(actionToRemove); } } EntitySimulation::applyActionChanges();}
//
// PhysicalEntitySimulation.cpp
// libraries/physcis/src
//
// Created by Andrew Meadows 2015.04.27
// 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 "PhysicsHelpers.h"
#include "PhysicsLogging.h"
#include "ShapeManager.h"
#include "PhysicalEntitySimulation.h"
PhysicalEntitySimulation::PhysicalEntitySimulation() {
}
PhysicalEntitySimulation::~PhysicalEntitySimulation() {
}
void PhysicalEntitySimulation::init(
EntityTree* tree,
PhysicsEngine* physicsEngine,
EntityEditPacketSender* packetSender) {
assert(tree);
setEntityTree(tree);
assert(physicsEngine);
_physicsEngine = physicsEngine;
assert(packetSender);
_entityPacketSender = packetSender;
}
// begin EntitySimulation overrides
void PhysicalEntitySimulation::updateEntitiesInternal(const quint64& now) {
// Do nothing here because the "internal" update the PhysicsEngine::stepSimualtion() which is done elsewhere.
}
void PhysicalEntitySimulation::addEntityInternal(EntityItemPointer entity) {
assert(entity);
if (entity->shouldBePhysical()) {
EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());
if (!motionState) {
_pendingAdds.insert(entity);
}
} else if (entity->isMoving()) {
_simpleKinematicEntities.insert(entity);
}
}
void PhysicalEntitySimulation::removeEntityInternal(EntityItemPointer entity) {
EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());
if (motionState) {
motionState->clearObjectBackPointer();
entity->setPhysicsInfo(nullptr);
_pendingRemoves.insert(motionState);
_outgoingChanges.remove(motionState);
}
_pendingAdds.remove(entity);
}
void PhysicalEntitySimulation::changeEntityInternal(EntityItemPointer entity) {
// queue incoming changes: from external sources (script, EntityServer, etc) to physics engine
assert(entity);
EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());
if (motionState) {
if (!entity->shouldBePhysical()) {
// the entity should be removed from the physical simulation
_pendingChanges.remove(motionState);
_physicalObjects.remove(motionState);
_pendingRemoves.insert(motionState);
_outgoingChanges.remove(motionState);
if (entity->isMoving()) {
_simpleKinematicEntities.insert(entity);
}
} else {
_pendingChanges.insert(motionState);
}
} else if (entity->shouldBePhysical()) {
// The intent is for this object to be in the PhysicsEngine, but it has no MotionState yet.
// Perhaps it's shape has changed and it can now be added?
_pendingAdds.insert(entity);
_simpleKinematicEntities.remove(entity); // just in case it's non-physical-kinematic
} else if (entity->isMoving()) {
_simpleKinematicEntities.insert(entity);
} else {
_simpleKinematicEntities.remove(entity); // just in case it's non-physical-kinematic
}
}
void PhysicalEntitySimulation::clearEntitiesInternal() {
// TODO: we should probably wait to lock the _physicsEngine so we don't mess up data structures
// while it is in the middle of a simulation step. As it is, we're probably in shutdown mode
// anyway, so maybe the simulation was already properly shutdown? Cross our fingers...
// first disconnect each MotionStates from its Entity
for (auto stateItr : _physicalObjects) {
EntityMotionState* motionState = static_cast<EntityMotionState*>(&(*stateItr));
EntityItemPointer entity = motionState->getEntity();
if (entity) {
entity->setPhysicsInfo(nullptr);
}
motionState->clearObjectBackPointer();
}
// then delete the objects (aka MotionStates)
_physicsEngine->deleteObjects(_physicalObjects);
// finally clear all lists (which now have only dangling pointers)
_physicalObjects.clear();
_pendingRemoves.clear();
_pendingAdds.clear();
_pendingChanges.clear();
}
// end EntitySimulation overrides
VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToDelete() {
_tempVector.clear();
for (auto stateItr : _pendingRemoves) {
EntityMotionState* motionState = &(*stateItr);
_pendingChanges.remove(motionState);
_physicalObjects.remove(motionState);
EntityItemPointer entity = motionState->getEntity();
if (entity) {
_pendingAdds.remove(entity);
entity->setPhysicsInfo(nullptr);
motionState->clearObjectBackPointer();
}
_tempVector.push_back(motionState);
}
_pendingRemoves.clear();
return _tempVector;
}
VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToAdd() {
_tempVector.clear();
SetOfEntities::iterator entityItr = _pendingAdds.begin();
while (entityItr != _pendingAdds.end()) {
EntityItemPointer entity = *entityItr;
assert(!entity->getPhysicsInfo());
if (!entity->shouldBePhysical()) {
// this entity should no longer be on the internal _pendingAdds
entityItr = _pendingAdds.erase(entityItr);
if (entity->isMoving()) {
_simpleKinematicEntities.insert(entity);
}
} else if (entity->isReadyToComputeShape()) {
ShapeInfo shapeInfo;
entity->computeShapeInfo(shapeInfo);
btCollisionShape* shape = ObjectMotionState::getShapeManager()->getShape(shapeInfo);
if (shape) {
EntityMotionState* motionState = new EntityMotionState(shape, entity);
entity->setPhysicsInfo(static_cast<void*>(motionState));
_physicalObjects.insert(motionState);
_tempVector.push_back(motionState);
entityItr = _pendingAdds.erase(entityItr);
} else {
//qDebug() << "Warning! Failed to generate new shape for entity." << entity->getName();
++entityItr;
}
} else {
++entityItr;
}
}
return _tempVector;
}
VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToChange() {
_tempVector.clear();
for (auto stateItr : _pendingChanges) {
EntityMotionState* motionState = &(*stateItr);
_tempVector.push_back(motionState);
}
_pendingChanges.clear();
return _tempVector;
}
void PhysicalEntitySimulation::handleOutgoingChanges(VectorOfMotionStates& motionStates, const QUuid& sessionID) {
// walk the motionStates looking for those that correspond to entities
for (auto stateItr : motionStates) {
ObjectMotionState* state = &(*stateItr);
if (state && state->getType() == MOTIONSTATE_TYPE_ENTITY) {
EntityMotionState* entityState = static_cast<EntityMotionState*>(state);
EntityItemPointer entity = entityState->getEntity();
if (entity) {
if (entityState->isCandidateForOwnership(sessionID)) {
_outgoingChanges.insert(entityState);
}
_entitiesToSort.insert(entityState->getEntity());
}
}
}
uint32_t numSubsteps = _physicsEngine->getNumSubsteps();
if (_lastStepSendPackets != numSubsteps) {
_lastStepSendPackets = numSubsteps;
if (sessionID.isNull()) {
// usually don't get here, but if so --> nothing to do
_outgoingChanges.clear();
return;
}
// send outgoing packets
QSet<EntityMotionState*>::iterator stateItr = _outgoingChanges.begin();
while (stateItr != _outgoingChanges.end()) {
EntityMotionState* state = *stateItr;
if (!state->isCandidateForOwnership(sessionID)) {
stateItr = _outgoingChanges.erase(stateItr);
} else if (state->shouldSendUpdate(numSubsteps, sessionID)) {
state->sendUpdate(_entityPacketSender, sessionID, numSubsteps);
++stateItr;
} else {
++stateItr;
}
}
}
}
void PhysicalEntitySimulation::handleCollisionEvents(CollisionEvents& collisionEvents) {
for (auto collision : collisionEvents) {
// NOTE: The collision event is always aligned such that idA is never NULL.
// however idB may be NULL.
if (!collision.idB.isNull()) {
emit entityCollisionWithEntity(collision.idA, collision.idB, collision);
}
}
}
void PhysicalEntitySimulation::applyActionChanges() {
if (_physicsEngine) {
foreach (EntityActionPointer actionToAdd, _actionsToAdd) {
_physicsEngine->addAction(actionToAdd);
}
foreach (QUuid actionToRemove, _actionsToRemove) {
_physicsEngine->removeAction(actionToRemove);
}
}
EntitySimulation::applyActionChanges();
}